<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(void* source, int numPoints, OniDriverFrame* pFrame)
{
	unsigned short* targetBase = (unsigned short*) pFrame->frame.data;
	const NUI_DEPTH_IMAGE_PIXEL* sourceBase = reinterpret_cast<const NUI_DEPTH_IMAGE_PIXEL*>(source);

	if (!m_cropping.enabled) {
		// optimization for the most typical case
		for (int i = 0; i < numPoints; i++)
		{
			*(targetBase+i) = filterReliableDepthValue((sourceBase+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 = targetBase;
		for (int y = minY; y < maxY; y++)
		{
			for (int x = minX; x < maxX; x++)
			{
				const NUI_DEPTH_IMAGE_PIXEL *sourceIter = sourceBase + (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(void* source, int numPoints, OniDriverFrame* pFrame)
{
	NUI_IMAGE_RESOLUTION nuiResolution =
		m_pStreamImpl->getNuiImagResolution(pFrame->frame.videoMode.resolutionX, pFrame->frame.videoMode.resolutionY);

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

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

	xnOSMemSet(targetBase, 0, pFrame->frame.dataSize);

	const NUI_DEPTH_IMAGE_PIXEL* sourceBase = reinterpret_cast<const NUI_DEPTH_IMAGE_PIXEL*>(source);

	// It seems NuiImageGetColorPixelCoordinateFrameFromDepthPixelFrameAtResolution
	// (Kinect SDK function to perform depth-to-color translation on a frame at once)
	// does not do its job correctly. We observe horizontal deviation of the coordinates,
	// for unknown reasons.
	// We work around this issue by using pixel-by-pixel conversion instead.
#if 0
	LONG* mappedCoords = (LONG*) xnOSMalloc(sizeof(LONG) * numPoints * 2); // Need review: maybe we'd better avoid allocating each time

	// 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,
		(USHORT*)source,
		numPoints * 2,
		mappedCoords
		);

	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* p = targetBase + (x - minX) + (y - minY) * pFrame->frame.width;
			*p = *(p+1) = filterReliableDepthValue((sourceBase+i)->depth);
		}
	}

	xnOSFree(mappedCoords); // Need review: maybe we'd better avoid allocating each time
#else
	int i = 0;
	for (LONG dy = 0; dy < pFrame->frame.videoMode.resolutionY; dy++) {
		for (LONG dx = 0; dx < pFrame->frame.videoMode.resolutionX; dx++, i++) {
			LONG cx, cy;
			m_pStreamImpl->getNuiSensor()->NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution(
				nuiResolution,
				nuiResolution,
				NULL,
				dx,
				dy,
				(sourceBase+i)->depth << 3,
				&cx,
				&cy
				);
			if (cx >= minX && cx < maxX - 1 && cy >= minY && cy < maxY) {
				unsigned short* p = targetBase + (cx - minX) + (cy - minY) * pFrame->frame.width;
				*p = *(p+1) = filterReliableDepthValue((sourceBase+i)->depth);
			}
		}
	}
#endif
}


void DepthKinectStream::frameReceived(NUI_IMAGE_FRAME& imageFrame, NUI_LOCKED_RECT& LockedRect)
{

	OniDriverFrame* pFrame = NULL;

	// calculate the data size and allocate the buffer<commit_msg>Fixed the horizontal drifting issue!<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 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(void* source, int numPoints, OniDriverFrame* pFrame)
{
	unsigned short* targetBase = (unsigned short*) pFrame->frame.data;
	const NUI_DEPTH_IMAGE_PIXEL* sourceBase = reinterpret_cast<const NUI_DEPTH_IMAGE_PIXEL*>(source);

	if (!m_cropping.enabled) {
		// optimization for the most typical case
		for (int i = 0; i < numPoints; i++)
		{
			*(targetBase+i) = filterReliableDepthValue((sourceBase+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 = targetBase;
		for (int y = minY; y < maxY; y++)
		{
			for (int x = minX; x < maxX; x++)
			{
				const NUI_DEPTH_IMAGE_PIXEL *sourceIter = sourceBase + (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(void* source, int numPoints, OniDriverFrame* pFrame)
{
	NUI_IMAGE_RESOLUTION nuiResolution =
		m_pStreamImpl->getNuiImagResolution(pFrame->frame.videoMode.resolutionX, pFrame->frame.videoMode.resolutionY);

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

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

	xnOSMemSet(targetBase, 0, pFrame->frame.dataSize);

	const NUI_DEPTH_IMAGE_PIXEL* sourceBase = reinterpret_cast<const NUI_DEPTH_IMAGE_PIXEL*>(source);

	LONG* mappedCoords = (LONG*) xnOSMalloc(sizeof(LONG) * numPoints * 2); // Need review: maybe we'd better avoid allocating each time
	USHORT* depthSource = (USHORT*) xnOSMalloc(sizeof(USHORT) * numPoints); // Need review: maybe we'd better avoid allocating each time

	// pack depth data
	for (int i = 0; i < numPoints; i++) {
		*(depthSource + i) = (sourceBase + 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,
		depthSource,
		numPoints * 2,
		mappedCoords
		);

	xnOSFree(depthSource);

	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* p = targetBase + (x - minX) + (y - minY) * pFrame->frame.width;
			*p = *(p+1) = filterReliableDepthValue((sourceBase+i)->depth);
		}
	}

	xnOSFree(mappedCoords); // Need review: maybe we'd better avoid allocating each time
}


void DepthKinectStream::frameReceived(NUI_IMAGE_FRAME& imageFrame, NUI_LOCKED_RECT& LockedRect)
{

	OniDriverFrame* pFrame = NULL;

	// calculate the data size and allocate the buffer
{'fraction_non_alphanumeric': 0.11419887103777682, 'fraction_numerical': 0.012592270950933565, 'mean_word_length': 4.8073625819465455, 'pattern_counts': {'":': 0, '<': 32, '<?xml version=': 0, '>': 97, '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': '7476182', 'n_tokens_mistral': 4461, 'n_tokens_neox': 4106, 'n_words': 959}