<commit_before>#include "DepthKinectStream.h"

#include <Shlobj.h>
#include "XnHash.h"
#include "XnEvent.h"
#include "XnPlatform.h"
#include "NuiApi.h"
#include "PS1080.h"
#include "D2S.h.h"
#include "S2D.h.h"
#include "KinectStreamImpl.h"

using namespace oni::driver;
using namespace kinect_device;


static const XnInt MAX_SHIFT_VAL = 2047;
static const XnInt PARAM_COEFF_VAL = 4;
static const XnInt SHIFT_SCALE_VAL = 10;
static const XnInt GAIN_VAL = 42;
static const XnInt ZPD_VAL = 120;
static const XnInt CONST_SHIFT_VAL = 200;
static const XnInt DEVICE_MAX_DEPTH_VAL = 10000;
static const XnDouble ZPPS_VAL = 0.10520000010728836;
static const XnDouble LDDIS_VAL = 7.5;

#define DEFAULT_FPS 30

DepthKinectStream::DepthKinectStream(KinectStreamImpl* pStreamImpl):
					BaseKinectStream(pStreamImpl)
{
	m_videoMode.pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM;
	m_videoMode.fps = DEFAULT_FPS;
	m_videoMode.resolutionX = KINNECT_RESOLUTION_X_640;
	m_videoMode.resolutionY = KINNECT_RESOLUTION_Y_480;
}

// discard the portion of the depth that contains only the player index
static inline unsigned short filterReliableDepthValue(unsigned short value)
{
	return (value > 0 && value < DEVICE_MAX_DEPTH_VAL) ? value : 0;
}

// Populate the image-related metadata (resolution, cropping, etc.) to OniDriverFrame.
void DepthKinectStream::populateFrameImageMetadata(OniDriverFrame* pFrame, int dataUnitSize)
{
	if (!m_cropping.enabled)
	{
		pFrame->frame.height = m_videoMode.resolutionY;
		pFrame->frame.width  = m_videoMode.resolutionX;
		pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0;
		pFrame->frame.croppingEnabled = FALSE;
	} else {
		pFrame->frame.height = m_cropping.height;
		pFrame->frame.width  = m_cropping.width;
		pFrame->frame.cropOriginX = m_cropping.originX;
		pFrame->frame.cropOriginY = m_cropping.originY;
		pFrame->frame.croppingEnabled = TRUE;
	}
	pFrame->frame.stride = pFrame->frame.width * dataUnitSize;
	pFrame->frame.videoMode.resolutionY = m_videoMode.resolutionY;
	pFrame->frame.videoMode.resolutionX = m_videoMode.resolutionX;
	pFrame->frame.videoMode.pixelFormat = m_videoMode.pixelFormat;
	pFrame->frame.videoMode.fps = m_videoMode.fps;
}

// Copy the depth pixels (NUI_DEPTH_IMAGE_PIXEL) to OniDriverFrame
// with applying cropping but NO depth-to-image registration.
void DepthKinectStream::copyDepthPixelsStraight(const NUI_DEPTH_IMAGE_PIXEL* source, int numPoints, OniDriverFrame* pFrame)
{
	unsigned short* target = (unsigned short*) pFrame->frame.data;

	if (!m_cropping.enabled) {
		// optimization for the most typical case
		for (int i = 0; i < numPoints; i++)
		{
			*(target+i) = filterReliableDepthValue((source+i)->depth);
		}
	} else {
		int minX = pFrame->frame.cropOriginX;
		int minY = pFrame->frame.cropOriginY;
		int maxX = minX + pFrame->frame.width;
		int maxY = minY + pFrame->frame.height;

		unsigned short* targetIter = target;
		for (int y = minY; y < maxY; y++)
		{
			for (int x = minX; x < maxX; x++)
			{
				const NUI_DEPTH_IMAGE_PIXEL *sourceIter = source + (m_videoMode.resolutionX * y + x);
				*(targetIter++)= filterReliableDepthValue(sourceIter->depth);
			}
		}
	}
}

// Copy the depth pixels (NUI_DEPTH_IMAGE_PIXEL) to OniDriverFrame
// with applying cropping and depth-to-image registration.
void DepthKinectStream::copyDepthPixelsWithImageRegistration(const NUI_DEPTH_IMAGE_PIXEL* source, int numPoints, OniDriverFrame* pFrame)
{
	// Note: We evaluated another implementation using INuiCoordinateMapper*::MapColorFrameToDepthFrame,
	// but, counterintuitively, that was slower than this implementation. We reverted it back.

	// For benchmarking purpose
	//static LARGE_INTEGER accTime;
	//static LONG accTimeCount;
	//LARGE_INTEGER startTime;
	//QueryPerformanceCounter(&startTime);

	NUI_IMAGE_RESOLUTION nuiResolution =
		m_pStreamImpl->getNuiImagResolution(pFrame->frame.videoMode.resolutionX, pFrame->frame.videoMode.resolutionY);

	unsigned short* target =(unsigned short*) pFrame->frame.data;
	xnOSMemSet(target, 0, pFrame->frame.dataSize);

	m_depthValuesBuffer.SetSize(numPoints);
	USHORT* depthValues = m_depthValuesBuffer.GetData();
	m_mappedCoordsBuffer.SetSize(numPoints * 2);
	LONG* mappedCoords = m_mappedCoordsBuffer.GetData();

	// pack depth data
	for (int i = 0; i < numPoints; i++) {
		*(depthValues + i) = (source + i)->depth << 3;
	}

	// Need review: not sure if it is a good idea to directly invoke INuiSensore here.
	m_pStreamImpl->getNuiSensor()->NuiImageGetColorPixelCoordinateFrameFromDepthPixelFrameAtResolution(
		nuiResolution, // assume the target image with the same resolution as the source.
		nuiResolution,
		numPoints,
		depthValues,
		numPoints * 2,
		mappedCoords
		);

	int minX = pFrame->frame.cropOriginX;
	int minY = pFrame->frame.cropOriginY;
	int maxX = minX + pFrame->frame.width;
	int maxY = minY + pFrame->frame.height;

	for (int i = 0; i < numPoints; i++)
	{
		int x = *(mappedCoords + i*2);
		int y = *(mappedCoords + i*2 + 1);
		if (x >= minX && x < maxX - 1 && y >= minY && y < maxY) {
			unsigned short d = filterReliableDepthValue((source+i)->depth);

			unsigned short* p = target + (x - minX) + (y - minY) * pFrame->frame.width;
			if (*p == 0 || *p > d) {
				*p = d;
			}

			p++;
			if (*p == 0 || *p > d) {
				*p = d;
			}
		}
	}

	// For benchmarking purpose
	//LARGE_INTEGER endTime;
	//QueryPerformanceCounter(&endTime);
	//accTime.QuadPart += endTime.QuadPart - startTime.QuadPart;
	//accTimeCount++;
	//printf("ImageRegistration: %lld\n", accTime.QuadPart / accTimeCount);
}


void DepthKinectStream::frameReceived(NUI_IMAGE_FRAME& imageFrame, NUI_LOCKED_RECT& LockedRect)
{
	OniDriverFrame* pFrame = NULL;
<commit_msg>More aggressive optimization and got >10% improvement!<commit_after>#include "DepthKinectStream.h"

#include <Shlobj.h>
#include "XnHash.h"
#include "XnEvent.h"
#include "XnPlatform.h"
#include "NuiApi.h"
#include "PS1080.h"
#include "D2S.h.h"
#include "S2D.h.h"
#include "KinectStreamImpl.h"

using namespace oni::driver;
using namespace kinect_device;


static const XnInt MAX_SHIFT_VAL = 2047;
static const XnInt PARAM_COEFF_VAL = 4;
static const XnInt SHIFT_SCALE_VAL = 10;
static const XnInt GAIN_VAL = 42;
static const XnInt ZPD_VAL = 120;
static const XnInt CONST_SHIFT_VAL = 200;
static const XnInt DEVICE_MAX_DEPTH_VAL = 10000;
static const XnDouble ZPPS_VAL = 0.10520000010728836;
static const XnDouble LDDIS_VAL = 7.5;

#define DEFAULT_FPS 30

DepthKinectStream::DepthKinectStream(KinectStreamImpl* pStreamImpl):
					BaseKinectStream(pStreamImpl)
{
	m_videoMode.pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM;
	m_videoMode.fps = DEFAULT_FPS;
	m_videoMode.resolutionX = KINNECT_RESOLUTION_X_640;
	m_videoMode.resolutionY = KINNECT_RESOLUTION_Y_480;
}

// Discard the depth value equal or greater than the max value.
inline unsigned short filterReliableDepthValue(unsigned short value)
{
	return value < DEVICE_MAX_DEPTH_VAL ? value : 0;
}

// Populate the image-related metadata (resolution, cropping, etc.) to OniDriverFrame.
void DepthKinectStream::populateFrameImageMetadata(OniDriverFrame* pFrame, int dataUnitSize)
{
	if (!m_cropping.enabled)
	{
		pFrame->frame.height = m_videoMode.resolutionY;
		pFrame->frame.width  = m_videoMode.resolutionX;
		pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0;
		pFrame->frame.croppingEnabled = FALSE;
	} else {
		pFrame->frame.height = m_cropping.height;
		pFrame->frame.width  = m_cropping.width;
		pFrame->frame.cropOriginX = m_cropping.originX;
		pFrame->frame.cropOriginY = m_cropping.originY;
		pFrame->frame.croppingEnabled = TRUE;
	}
	pFrame->frame.stride = pFrame->frame.width * dataUnitSize;
	pFrame->frame.videoMode.resolutionY = m_videoMode.resolutionY;
	pFrame->frame.videoMode.resolutionX = m_videoMode.resolutionX;
	pFrame->frame.videoMode.pixelFormat = m_videoMode.pixelFormat;
	pFrame->frame.videoMode.fps = m_videoMode.fps;
}

// FIXME: For preliminary benchmarking purpose. Should not belong here.
static void recordAverageProcessTime(const char* message, LARGE_INTEGER* pAccTime, LARGE_INTEGER* pAccCount, const LARGE_INTEGER& startTime)
{
#if 0 // set to 1 to display the process time
	LARGE_INTEGER endTime;
	QueryPerformanceCounter(&endTime);
	pAccTime->QuadPart += endTime.QuadPart - startTime.QuadPart;
	pAccCount->QuadPart++;
	printf("%s: %lld\n", message, pAccTime->QuadPart / pAccCount->QuadPart);
#endif
}

// Copy the depth pixels (NUI_DEPTH_IMAGE_PIXEL) to OniDriverFrame
// with applying cropping but NO depth-to-image registration.
void DepthKinectStream::copyDepthPixelsStraight(const NUI_DEPTH_IMAGE_PIXEL* source, int numPoints, OniDriverFrame* pFrame)
{
	// Note: The local variable assignments and const qualifiers are carefully designed to generate
	// a high performance code with VC 2010. We recommend check the performance when changing the code
	// even if it was a trivial change.

	// For benchmarking purpose
	LARGE_INTEGER startTime;
	QueryPerformanceCounter(&startTime);

	unsigned short* target = (unsigned short*) pFrame->frame.data;

	const unsigned int width = pFrame->frame.width;
	const unsigned int height = pFrame->frame.height;
	const unsigned int skipWidth = m_videoMode.resolutionX - width;

	// Offset the starting position
	source += pFrame->frame.cropOriginX + pFrame->frame.cropOriginY * m_videoMode.resolutionX;

	for (unsigned int y = 0; y < height; y++)
	{
		for (unsigned int x = 0; x < width; x++)
		{
			*(target++) = filterReliableDepthValue((source++)->depth);
		}
		source += skipWidth;
	}

	// FIXME: for preliminary benchmarking purpose
	static LARGE_INTEGER accTime, accCount;
	recordAverageProcessTime("No Image Reg", &accTime, &accCount, startTime);
}

// Copy the depth pixels (NUI_DEPTH_IMAGE_PIXEL) to OniDriverFrame
// with applying cropping and depth-to-image registration.
void DepthKinectStream::copyDepthPixelsWithImageRegistration(const NUI_DEPTH_IMAGE_PIXEL* source, int numPoints, OniDriverFrame* pFrame)
{
	// Note: We evaluated another possible implementation using INuiCoordinateMapper*::MapColorFrameToDepthFrame,
	// but, counterintuitively, it turned out to be slower than this implementation. We reverted it back.

	// Note: The local variable assignments and const qualifiers are carefully designed to generate
	// a high performance code with VC 2010. We recommend check the performance when changing the code
	// even if it was a trivial change.

	// For benchmarking purpose
	LARGE_INTEGER startTime;
	QueryPerformanceCounter(&startTime);

	NUI_IMAGE_RESOLUTION nuiResolution =
		m_pStreamImpl->getNuiImagResolution(pFrame->frame.videoMode.resolutionX, pFrame->frame.videoMode.resolutionY);

	unsigned short* const target = (unsigned short*) pFrame->frame.data;
	xnOSMemSet(target, 0, pFrame->frame.dataSize);

	m_depthValuesBuffer.SetSize(numPoints);
	m_mappedCoordsBuffer.SetSize(numPoints * 2);

	// Pack depth data for NuiImageGetColorPixelCoordinateFrameFromDepthPixelFrameAtResolution
	USHORT* depthValuesIter = m_depthValuesBuffer.GetData();
	for (int i = 0; i < numPoints; i++) {
		*(depthValuesIter++) = (source + i)->depth << 3;
	}

	// Need review: not sure if it is a good idea to directly invoke INuiSensore here.
	m_pStreamImpl->getNuiSensor()->NuiImageGetColorPixelCoordinateFrameFromDepthPixelFrameAtResolution(
		nuiResolution, // assume the target image with the same resolution as the source.
		nuiResolution,
		numPoints,
		m_depthValuesBuffer.GetData(),
		numPoints * 2,
		m_mappedCoordsBuffer.GetData()
		);

	const unsigned int minX = pFrame->frame.cropOriginX;
	const unsigned int minY = pFrame->frame.cropOriginY;
	const unsigned int width = pFrame->frame.width;
	const unsigned int height = pFrame->frame.height;
	const LONG* mappedCoordsIter = m_mappedCoordsBuffer.GetData();

	for (int i = 0; i < numPoints; i++)
	{
		const unsigned int x = *mappedCoordsIter++ - minX;
		const unsigned int y = *mappedCoordsIter++ - minY;
		if (x < width - 1 && y < height) {
			const unsigned short d = filterReliableDepthValue((source+i)->depth);
			unsigned short* p = target + x + y * width;
			if (*p == 0 || *p > d) *p = d;
			p++;
			if (*p == 0 || *p > d) *p = d;
		}
	}

	// FIXME: for preliminary benchmarking purpose
	static LARGE_INTEGER accTime, accCount;
	recordAverageProcessTime("With Image Reg", &accTime, &accCount, startTime);
}


void DepthKinectStream::frameReceived(NUI_IMAGE_FRAME& imageFrame, NUI_LOCKED_RECT& LockedRect)
{
	OniDriverFrame* pFrame = NULL;

{'fraction_non_alphanumeric': 0.10819302344316442, 'fraction_numerical': 0.012164666075888182, 'mean_word_length': 4.968269230769231, 'pattern_counts': {'":': 0, '<': 24, '<?xml version=': 0, '>': 89, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3859277', 'n_tokens_mistral': 4607, 'n_tokens_neox': 4272, 'n_words': 1046}