public static Blkd CreateDepthToCameraBlkd(PXCMPoint3DF32[] _depthToCameraMapping, int width, int height)
{
var BytesPerPixel = 3 * sizeof(float);
byte[] buffer = new byte[width * height * BytesPerPixel];
for (int i = 0; i < _depthToCameraMapping.Length; i++)
{
var point = _depthToCameraMapping[i];
var smallfloatBuffer = new float[] { point.x, point.y, point.z };
Buffer.BlockCopy(smallfloatBuffer, 0, buffer, i * BytesPerPixel, BytesPerPixel);
}
Blkd result = new Blkd
{
Width = (UInt16)width,
Height = (UInt16)height,
BytesPerPixel = (byte)BytesPerPixel,
Version = 2,
Data = buffer
};
return(result);
}
/// <summary>
/// Densely store depth to color mapping as BLKD.
///
/// Returns the number of shorts written to buffer.
/// </summary>
/// <param name="frame">KinectScanner.Reading.Frame</param>
/// <param name="filename">filename to store the mapping</param>
/// <returns></returns>
public Tuple <Blkd, TimeSpan> CaptureMappedFrame(LiveFrame frame, byte[] buffer)
{
DepthFrame depthFrame = frame.NativeDepthFrame;
CoordinateMapper mapper = frame.NativeCoordinateMapper;
if (buffer.Length != Frame.DEPTH_INFRARED_PIXELS * DEPTH_MAPPING_BYTES_PER_PIXEL)
{
throw new ArgumentException(string.Format("Buffer length is {0} but {1} is needed", buffer.LongLength, Frame.DEPTH_INFRARED_PIXELS * DEPTH_MAPPING_BYTES_PER_PIXEL));
}
depthFrame.CopyFrameDataToArray(_depthData);
mapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);
mapper.MapDepthFrameToCameraSpace(_depthData, _cameraSpacePoints);
Array.Clear(buffer, 0, buffer.Length);
int count = 0;
for (int i = 0; i < Frame.DEPTH_INFRARED_PIXELS; ++i)
{
ColorSpacePoint colorPoint = _colorPoints[i];
CameraSpacePoint cameraPoint = _cameraSpacePoints[i];
// make sure the depth pixel maps to a valid point in color space
short colorX = (short)Math.Floor(colorPoint.X + 0.5);
short colorY = (short)Math.Floor(colorPoint.Y + 0.5);
if (colorX < 0 || colorX >= Frame.COLOR_WIDTH || colorY < 0 || colorY >= Frame.COLOR_HEIGHT)
{
colorX = -1;
colorY = -1;
}
// Little endian === lowest order bytes at lower addresses
buffer[count++] = (byte)(colorX >> 0);
buffer[count++] = (byte)(colorX >> 8);
buffer[count++] = (byte)(colorY >> 0);
buffer[count++] = (byte)(colorY >> 8);
float[] cameraPointValues = new float[] { cameraPoint.X, cameraPoint.Y, cameraPoint.Z };
System.Buffer.BlockCopy(cameraPointValues, 0, buffer, count, 12);
count += 12;
}
Blkd result = new Blkd
{
Width = (UInt16)Frame.DEPTH_INFRARED_WIDTH,
Height = (UInt16)Frame.DEPTH_INFRARED_HEIGHT,
BytesPerPixel = DEPTH_MAPPING_BYTES_PER_PIXEL,
Version = 2,
Data = buffer
};
return(new Tuple <Blkd, TimeSpan>(result, depthFrame.RelativeTime));
}
