/// <summary>
/// Update map from a depth frame
/// </summary>
/// <param name="coordinateMapper"></param>
/// <param name="depthFrame"></param>
public void Update(CoordinateMapper coordinateMapper, DepthFrameData depthFrame)
{
if (this.dataPointer == IntPtr.Zero)
{
throw new ObjectDisposedException("ColorToDepthFrameData");
}
coordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(depthFrame.DataPointer, (uint)depthFrame.SizeInBytes, this.dataPointer, (uint)this.sizeInBytes);
}
void ProcessFrame()
{
var pDepthData = GCHandle.Alloc(_DepthData, GCHandleType.Pinned);
var pDepthCoordinatesData = GCHandle.Alloc(m_pDepthCoordinates, GCHandleType.Pinned);
m_pCoordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
pDepthData.AddrOfPinnedObject(),
(uint)_DepthData.Length * sizeof(ushort),
pDepthCoordinatesData.AddrOfPinnedObject(),
(uint)m_pDepthCoordinates.Length);
pDepthCoordinatesData.Free();
pDepthData.Free();
}
void ProcessFrame()
{
var pDepthData = GCHandle.Alloc(pDepthBuffer, GCHandleType.Pinned);
var pDepthCoordinatesData = GCHandle.Alloc(pDepthCoordinates, GCHandleType.Pinned);
coordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(pDepthData.AddrOfPinnedObject(), (uint)pDepthBuffer.Length * sizeof(ushort),
pDepthCoordinatesData.AddrOfPinnedObject(), (uint)pDepthCoordinates.Length);
pDepthCoordinatesData.Free();
pDepthData.Free();
/*
* colorTexture.LoadRawTextureData(pColorBuffer);
* colorTexture.Apply();
*/
}
//將不同解析度對照同步，讓不同的讀取內容可以互相配對。
void ProcessFrame()
{
//此段和下句可以達成同樣的效果，不過因為此段要使用IntPtr的關係與記憶體的釋放控制，下句則沒有這樣子的效果。
//m_pCoordinateMapper.MapColorFrameToDepthSpace(pDepthBuffer,m_pDepthCoordinates);
var pDepthData = GCHandle.Alloc(pDepthBuffer, GCHandleType.Pinned);
var pDepthCoordinatesData = GCHandle.Alloc(m_pDepthCoordinates, GCHandleType.Pinned);
m_pCoordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
pDepthData.AddrOfPinnedObject(),
(uint)pDepthBuffer.Length * sizeof(ushort),
pDepthCoordinatesData.AddrOfPinnedObject(),
(uint)m_pDepthCoordinates.Length);
pDepthCoordinatesData.Free();
pDepthData.Free();
m_pColorRGBX.LoadRawTextureData(pColorBuffer);
m_pColorRGBX.Apply();
}
public bool MapColorFrameToDepthCoords(KinectInterop.SensorData sensorData, ref Vector2[] vDepthCoords)
{
if (coordMapper != null && sensorData.colorImage != null && sensorData.depthImage != null)
{
var pDepthData = GCHandle.Alloc(sensorData.depthImage, GCHandleType.Pinned);
var pDepthCoordsData = GCHandle.Alloc(vDepthCoords, GCHandleType.Pinned);
coordMapper.MapColorFrameToDepthSpaceUsingIntPtr(
pDepthData.AddrOfPinnedObject(),
(uint)sensorData.depthImage.Length * sizeof(ushort),
pDepthCoordsData.AddrOfPinnedObject(),
(uint)vDepthCoords.Length);
pDepthCoordsData.Free();
pDepthData.Free();
return(true);
}
return(false);
}
void ProcessFrame()
{
GCHandle pDepthData = GCHandle.Alloc(pDepthBuffer, GCHandleType.Pinned);
GCHandle pDepthCoordinatesData = GCHandle.Alloc(m_pDepthCoordinates, GCHandleType.Pinned);
GCHandle pColorData = GCHandle.Alloc(m_pColorSpacePoints, GCHandleType.Pinned);
m_pCoordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
pDepthData.AddrOfPinnedObject(), (uint)pDepthBuffer.Length * sizeof(ushort),
pDepthCoordinatesData.AddrOfPinnedObject(), (uint)m_pDepthCoordinates.Length);
m_pCoordinateMapper.MapDepthFrameToColorSpaceUsingIntPtr(
pDepthData.AddrOfPinnedObject(), pDepthBuffer.Length * sizeof(ushort),
pColorData.AddrOfPinnedObject(), (uint)m_pColorSpacePoints.Length);
m_pDepth.LoadRawTextureData(pDepthData.AddrOfPinnedObject(), pDepthBuffer.Length * sizeof(ushort));
m_pDepth.Apply();
pColorData.Free();
pDepthCoordinatesData.Free();
pDepthData.Free();
m_pColorRGBX.LoadRawTextureData(pColorBuffer);
m_pColorRGBX.Apply();
}
/// <summary>
/// Update map from a depth frame
/// </summary>
/// <param name="coordinateMapper"></param>
/// <param name="depthFrame"></param>
public void Update(CoordinateMapper coordinateMapper, DepthFrameData depthFrame)
{
if (this.dataPointer == IntPtr.Zero)
throw new ObjectDisposedException("ColorToDepthFrameData");
coordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(depthFrame.DataPointer,(uint)depthFrame.SizeInBytes,this.dataPointer,(uint)this.sizeInBytes);
}
/// <summary>
/// Procedure invoked by Kinect when new data are available
/// </summary>
private void MultisourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
if (KinectSensor == null || Reader == null)
{
return;
}
// acquire frame data
MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();
// if the Frame has expired by the time we process this event, return.
if (multiSourceFrame == null)
{
return;
}
// Continue only if buffer is empty
if (!Buffer.IsEmpty())
{
return;
}
// declare variables for data from sensor
ColorFrame colorFrame = null;
DepthFrame depthFrame = null;
InfraredFrame infraredFrame = null;
byte[] colorFrameData = null;
ushort[] depthData = null;
ushort[] infraredData = null;
DepthSpacePoint[] pointsFromColorToDepth = null;
ColorSpacePoint[] pointsFromDepthToColor = null;
CameraSpacePoint[] cameraSpacePointsFromDepthData = null;
try
{
// get frames from sensor
colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
infraredFrame = multiSourceFrame.InfraredFrameReference.AcquireFrame();
// If any frame has expired by the time we process this event, return.
if (colorFrame == null || depthFrame == null || infraredFrame == null)
{
return;
}
// use frame data to fill arrays
colorFrameData = new byte[ColorFrameDescription.LengthInPixels * 4];
depthData = new ushort[DepthFrameDescription.LengthInPixels];
infraredData = new ushort[InfraredFrameDescription.LengthInPixels];
colorFrame.CopyConvertedFrameDataToArray(colorFrameData, ColorImageFormat.Bgra);
depthFrame.CopyFrameDataToArray(depthData);
infraredFrame.CopyFrameDataToArray(infraredData);
pointsFromColorToDepth = new DepthSpacePoint[ColorFrameDescription.LengthInPixels];
pointsFromDepthToColor = new ColorSpacePoint[DepthFrameDescription.LengthInPixels];
cameraSpacePointsFromDepthData = new CameraSpacePoint[DepthFrameDescription.LengthInPixels];
using (KinectBuffer depthFrameData = depthFrame.LockImageBuffer())
{
CoordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
depthFrameData.UnderlyingBuffer,
depthFrameData.Size,
pointsFromColorToDepth);
CoordinateMapper.MapDepthFrameToColorSpaceUsingIntPtr(
depthFrameData.UnderlyingBuffer,
depthFrameData.Size,
pointsFromDepthToColor);
CoordinateMapper.MapDepthFrameToCameraSpaceUsingIntPtr(
depthFrameData.UnderlyingBuffer,
depthFrameData.Size,
cameraSpacePointsFromDepthData);
}
}
finally
{
// dispose frames so that Kinect can continue processing
colorFrame?.Dispose();
depthFrame?.Dispose();
infraredFrame?.Dispose();
// send data futher
if (
colorFrameData != null &&
depthData != null &&
infraredData != null &&
cameraSpacePointsFromDepthData != null
)
{
// store data to buffer and notify processing thread
Buffer.Store(
new KinectData(
colorFrameData,
depthData,
infraredData,
cameraSpacePointsFromDepthData,
pointsFromColorToDepth,
pointsFromDepthToColor
)
);
TrackingManager.SendKinectUpdate();
}
}
}
/// <summary>
/// Handles the depth/color/body index frame data arriving from the sensor
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
int depthWidth;
int depthHeight;
DepthFrame depthFrame = null;
ColorFrame colorFrame = null;
BodyIndexFrame bodyIndexFrame = null;
bool isBitmapLocked = false;
MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();
// If the Frame has expired by the time we process this event, return.
if (multiSourceFrame == null)
{
return;
}
// We use a try/finally to ensure that we clean up before we exit the function.
// This includes calling Dispose on any Frame objects that we may have and unlocking the bitmap back buffer.
try
{
depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
bodyIndexFrame = multiSourceFrame.BodyIndexFrameReference.AcquireFrame();
// If any frame has expired by the time we process this event, return.
// The "finally" statement will Dispose any that are not null.
if (depthFrame == null || colorFrame == null || bodyIndexFrame == null)
{
return;
}
// Process Depth
FrameDescription depthFrameDescription = depthFrame.FrameDescription;
depthWidth = depthFrameDescription.Width;
depthHeight = depthFrameDescription.Height;
// Access the depth frame data directly via LockImageBuffer to avoid making a copy
using (KinectBuffer depthFrameData = depthFrame.LockImageBuffer())
{
coordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(depthFrameData.UnderlyingBuffer, depthFrameData.Size, colorMappedToDepthPoints);
}
// We're done with the DepthFrame
depthFrame.Dispose();
depthFrame = null;
// Process Color
// Lock the bitmap for writing
bitmap.Lock();
isBitmapLocked = true;
colorFrame.CopyConvertedFrameDataToIntPtr(bitmap.BackBuffer, bitmapBackBufferSize, ColorImageFormat.Bgra);
// We're done with the ColorFrame
colorFrame.Dispose();
colorFrame = null;
// We'll access the body index data directly to avoid a copy
using (KinectBuffer bodyIndexData = bodyIndexFrame.LockImageBuffer())
{
unsafe
{
byte *bodyIndexDataPointer = (byte *)bodyIndexData.UnderlyingBuffer;
int colorMappedToDepthPointCount = colorMappedToDepthPoints.Length;
fixed(DepthSpacePoint *colorMappedToDepthPointsPointer = colorMappedToDepthPoints)
{
// Treat the color data as 4-byte pixels
uint *bitmapPixelsPointer = (uint *)bitmap.BackBuffer;
// Loop over each row and column of the color image
// Zero out any pixels that don't correspond to a body index
for (int colorIndex = 0; colorIndex < colorMappedToDepthPointCount; ++colorIndex)
{
float colorMappedToDepthX = colorMappedToDepthPointsPointer[colorIndex].X;
float colorMappedToDepthY = colorMappedToDepthPointsPointer[colorIndex].Y;
// The sentinel value is -inf, -inf, meaning that no depth pixel corresponds to this color pixel.
if (!float.IsNegativeInfinity(colorMappedToDepthX) && !float.IsNegativeInfinity(colorMappedToDepthY))
{
// Make sure the depth pixel maps to a valid point in color space
int depthX = (int)(colorMappedToDepthX + 0.5F);
int depthY = (int)(colorMappedToDepthY + 0.5F);
// If the point is not valid, there is no body index there.
if (depthX >= 0 && depthX < depthWidth && depthY >= 0 && depthY < depthHeight)
{
int depthIndex = depthY * depthWidth + depthX;
// If we are tracking a body for the current pixel, do not zero out the pixel
if (bodyIndexDataPointer[depthIndex] != 0xFF)
{
continue;
}
}
}
bitmapPixelsPointer[colorIndex] = 0;
}
}
bitmap.AddDirtyRect(new Int32Rect(0, 0, bitmap.PixelWidth, bitmap.PixelHeight));
}
}
}
finally
{
if (isBitmapLocked)
{
bitmap.Unlock();
}
depthFrame?.Dispose();
colorFrame?.Dispose();
bodyIndexFrame?.Dispose();
}
}
public void ProcessBackgroundOld(DepthFrame depthFrame, ColorFrame colorFrame, BodyIndexFrame bodyIndexFrame)
{
// Process Depth
var depthWidth = _displayWidth;
var depthHeight = _displayHeight;
// Access the depth frame data directly via LockImageBuffer to avoid making a copy
using (var depthFrameData = depthFrame.LockImageBuffer())
{
_coordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
depthFrameData.UnderlyingBuffer,
depthFrameData.Size,
_colorMappedToDepthPoints);
}
// We're done with the DepthFrame
depthFrame.Dispose();
// Lock the bitmap for writing
_bitmap.Lock();
_isBitmapLocked = true;
colorFrame.CopyConvertedFrameDataToIntPtr(_bitmap.BackBuffer, _bitmapBackBufferSize, ColorImageFormat.Bgra);
// We're done with the ColorFrame
colorFrame.Dispose();
// We'll access the body index data directly to avoid a copy
using (var bodyIndexData = bodyIndexFrame.LockImageBuffer())
{
unsafe
{
var bodyIndexDataPointer = (byte *)bodyIndexData.UnderlyingBuffer;
var colorMappedToDepthPointCount = _colorMappedToDepthPoints.Length;
fixed(DepthSpacePoint *colorMappedToDepthPointsPointer = _colorMappedToDepthPoints)
{
// Treat the color data as 4-byte pixels
var bitmapPixelsPointer = (uint *)_bitmap.BackBuffer;
// Loop over each row and column of the color image
// Zero out any pixels that don't correspond to a body index
for (var colorIndex = 0; colorIndex < colorMappedToDepthPointCount; ++colorIndex)
{
var colorMappedToDepthX = colorMappedToDepthPointsPointer[colorIndex].X;
var colorMappedToDepthY = colorMappedToDepthPointsPointer[colorIndex].Y;
// The sentinel value is -inf, -inf, meaning that no depth pixel corresponds to this color pixel.
if (!float.IsNegativeInfinity(colorMappedToDepthX) &&
!float.IsNegativeInfinity(colorMappedToDepthY))
{
// Make sure the depth pixel maps to a valid point in color space
var depthX = (int)(colorMappedToDepthX);
var depthY = (int)(colorMappedToDepthY);
// If the point is not valid, there is no body index there.
if ((depthX >= 0) && (depthX < depthWidth) && (depthY >= 0) && (depthY < depthHeight))
{
var depthIndex = (depthY * depthWidth) + depthX;
// If we are tracking a body for the current pixel, do not zero out the pixel
if (bodyIndexDataPointer[depthIndex] != 0xff)
{
continue;
}
}
}
bitmapPixelsPointer[colorIndex] = 0;
}
}
_bitmap.AddDirtyRect(new Int32Rect(0, 0, _bitmap.PixelWidth, _bitmap.PixelHeight));
}
}
}
private void msfr_FrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
bool depthFrameProcessed = false;
if (e.FrameReference == null)
{
return;
}
DepthFrame depthFrame = null;
ColorFrame colorFrame = null;
MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();
// If the Frame has expired by the time we process this event, return.
if (multiSourceFrame == null)
{
return;
}
// We use a try/finally to ensure that we clean up before we exit the function.
// This includes calling Dispose on any Frame objects that we may have and unlocking the bitmap back buffer.
try
{
colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
// If any frame has expired by the time we process this event, return.
// The "finally" statement will Dispose any that are not null.
if ((colorFrame == null) || (depthFrame == null))
{
return;
}
// Access the depth frame data directly via LockImageBuffer to avoid making a copy
using (KinectBuffer depthBuffer = depthFrame.LockImageBuffer())
{
coordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
depthBuffer.UnderlyingBuffer,
depthBuffer.Size,
this.colorMappedToDepthPoints);
// Note: In order to see the full range of depth (including the less reliable far field depth)
// we are setting maxDepth to the extreme potential depth threshold
ushort maxDepth = ushort.MaxValue;
// If you wish to filter by reliable depth distance, uncomment the following line:
//// maxDepth = depthFrame.DepthMaxReliableDistance
ProcessDepthFrameData(depthBuffer.UnderlyingBuffer, depthBuffer.Size, depthFrame.DepthMinReliableDistance, maxDepth);
depthFrameProcessed = true;
}
colorFrame.CopyConvertedFrameDataToArray(colorPixels, ColorImageFormat.Bgra);
//depthFrame.CopyFrameDataToArray(depthPixels);
// Creating BitmapSource
var bytesPerPixel = (PixelFormats.Bgr32.BitsPerPixel) / 8;
var colorStride = bytesPerPixel * colorFrame.FrameDescription.Width;
//var depthStride = bytesPerPixel * depthFrame.FrameDescription.Width;
colorBitmapSource = BitmapSource.Create(colorFrameDesc.Width, colorFrameDesc.Height, 96.0, 96.0, PixelFormats.Bgr32, null, colorPixels, colorStride);
//depthBitmapSource = BitmapSource.Create(depthFrameDesc.Width, depthFrameDesc.Height, 96.0, 96.0, PixelFormats.Gray8, null);
// WriteableBitmap to show on UI
writeableBitmap = new WriteableBitmap(this.depthFrameDesc.Width, this.depthFrameDesc.Height, 96.0, 96.0, PixelFormats.Gray8, null);
kinectImage.Source = writeableBitmap;
if (depthFrameProcessed)
{
writeableBitmap.WritePixels(new Int32Rect(0, 0, writeableBitmap.PixelWidth, writeableBitmap.PixelHeight),
depthPixels,
writeableBitmap.PixelWidth,
0);
}
// We're done with the DepthFrame
depthFrame.Dispose();
depthFrame = null;
// We're done with the ColorFrame
colorFrame.Dispose();
colorFrame = null;
}
finally
{
if (depthFrame != null)
{
depthFrame.Dispose();
}
if (colorFrame != null)
{
colorFrame.Dispose();
}
}
}
private unsafe void ProcessMultisourceFrame(MultiSourceFrame multiSourceFrame)
{
// Aquiure all frames
using var depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
using var colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
using var bodyIndexFrame = multiSourceFrame.BodyIndexFrameReference.AcquireFrame();
// If any frame has expired by the time we process this event, return.
if (depthFrame == null || colorFrame == null || bodyIndexFrame == null)
{
return;
}
// Copy depth frame
using var depthFrameData = depthFrame.LockImageBuffer();
_coordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(depthFrameData.UnderlyingBuffer, depthFrameData.Size, _colorMappedToDepthPoints);
var depthFrameDescription = depthFrame.FrameDescription;
var depthWidth = depthFrameDescription.Width;
var depthHeight = depthFrameDescription.Height;
// Copy color frame
ColorImageLock.EnterWriteLock();
fixed(byte *ColorImagePtr = ColorImage)
{
using var colorFrameData = colorFrame.LockRawImageBuffer();
var colorFrameSpan = new Span <byte>((void *)colorFrameData.UnderlyingBuffer, (int)colorFrameData.Size);
var colorImageSpan = new Span <byte>((void *)ColorImagePtr, ColorImageLength);
var colorImageSpanUInt32 = new Span <UInt32>((void *)ColorImagePtr, ColorImageLength);
//colorFrameSpan.CopyTo(colorImageSpan);
colorFrame.CopyConvertedFrameDataToArray(ColorImage, ColorImageFormat.Bgra);
// Get body index data
using var bodyIndexData = bodyIndexFrame.LockImageBuffer();
var bodyIndexDataPointer = (byte *)bodyIndexData.UnderlyingBuffer;
var colorMappedToDepthPointsPointer = new Span <DepthSpacePoint>(_colorMappedToDepthPoints);
// Go over and black out all colors that are not a body index
for (var colorIndex = 0; colorIndex < colorMappedToDepthPointsPointer.Length; ++colorIndex)
{
float colorMappedToDepthX = colorMappedToDepthPointsPointer[colorIndex].X;
float colorMappedToDepthY = colorMappedToDepthPointsPointer[colorIndex].Y;
// The sentinel value is -inf, -inf, meaning that no depth pixel corresponds to this color pixel.
if (!float.IsNegativeInfinity(colorMappedToDepthX) &&
!float.IsNegativeInfinity(colorMappedToDepthY))
{
// Make sure the depth pixel maps to a valid point in color space
int depthX = (int)(colorMappedToDepthX + 0.5f);
int depthY = (int)(colorMappedToDepthY + 0.5f);
// If the point is not valid, there is no body index there.
if ((depthX >= 0) && (depthX < depthWidth) && (depthY >= 0) && (depthY < depthHeight))
{
int depthIndex = (depthY * depthWidth) + depthX;
// If we are tracking a body for the current pixel, do not zero out the pixel
if (bodyIndexDataPointer[depthIndex] != 0xff)
{
// Set alpha to fully opaque
// For some reason we are ARGB
colorImageSpanUInt32[colorIndex] |= 0xFF000000;
continue;
}
}
}
// Background
colorImageSpanUInt32[colorIndex] = 0x00000000;
}
}
ColorImageLock.ExitWriteLock();
NewImage?.Invoke(this, ColorImage);
}
private static void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
bitmap = new WriteableBitmap(colorWidth, colorHeight, 96.0, 96.0, PixelFormats.Bgra32, null);
// Calculate the WriteableBitmap back buffer size
bitmapBackBufferSize = (uint)((bitmap.BackBufferStride * (bitmap.PixelHeight - 1)) + (bitmap.PixelWidth * bytesPerPixel));
int depthWidth = 0;
int depthHeight = 0;
DepthFrame depthFrame = null;
ColorFrame colorFrame = null;
BodyIndexFrame bodyIndexFrame = null;
bool isBitmapLocked = false;
MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();
// If the Frame has expired by the time we process this event, return.
if (multiSourceFrame == null)
{
return;
}
// We use a try/finally to ensure that we clean up before we exit the function.
// This includes calling Dispose on any Frame objects that we may have and unlocking the bitmap back buffer.
try
{
depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
bodyIndexFrame = multiSourceFrame.BodyIndexFrameReference.AcquireFrame();
// If any frame has expired by the time we process this event, return.
// The "finally" statement will Dispose any that are not null.
if ((depthFrame == null) || (colorFrame == null) || (bodyIndexFrame == null))
{
return;
}
// Process Depth
FrameDescription depthFrameDescription = depthFrame.FrameDescription;
depthWidth = depthFrameDescription.Width;
depthHeight = depthFrameDescription.Height;
// Access the depth frame data directly via LockImageBuffer to avoid making a copy
using (KinectBuffer depthFrameData = depthFrame.LockImageBuffer())
{
coordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
depthFrameData.UnderlyingBuffer,
depthFrameData.Size,
colorMappedToDepthPoints);
}
// We're done with the DepthFrame
depthFrame.Dispose();
depthFrame = null;
// Process Color
Trace.WriteLine("\n\n\n\n BEFORE LOCK \n\n\n\n");
// Lock the bitmap for writing
bitmap.Lock();
Trace.WriteLine("\n\n\n\n LOCK!!!! \n\n\n\n");
colorFrame.CopyConvertedFrameDataToIntPtr(bitmap.BackBuffer, bitmapBackBufferSize, ColorImageFormat.Bgra);
// We're done with the ColorFrame
colorFrame.Dispose();
colorFrame = null;
// We'll access the body index data directly to avoid a copy
using (KinectBuffer bodyIndexData = bodyIndexFrame.LockImageBuffer())
{
unsafe
{
byte *bodyIndexDataPointer = (byte *)bodyIndexData.UnderlyingBuffer;
int colorMappedToDepthPointCount = colorMappedToDepthPoints.Length;
fixed(DepthSpacePoint *colorMappedToDepthPointsPointer = colorMappedToDepthPoints)
{
// Treat the color data as 4-byte pixels
uint *bitmapPixelsPointer = (uint *)bitmap.BackBuffer;
// Loop over each row and column of the color image
// Zero out any pixels that don't correspond to a body index
for (int colorIndex = 0; colorIndex < colorMappedToDepthPointCount; ++colorIndex)
{
float colorMappedToDepthX = colorMappedToDepthPointsPointer[colorIndex].X;
float colorMappedToDepthY = colorMappedToDepthPointsPointer[colorIndex].Y;
// The sentinel value is -inf, -inf, meaning that no depth pixel corresponds to this color pixel.
if (!float.IsNegativeInfinity(colorMappedToDepthX) &&
!float.IsNegativeInfinity(colorMappedToDepthY))
{
// Make sure the depth pixel maps to a valid point in color space
int depthX = (int)(colorMappedToDepthX + 0.5f);
int depthY = (int)(colorMappedToDepthY + 0.5f);
// If the point is not valid, there is no body index there.
if ((depthX >= 0) && (depthX < depthWidth) && (depthY >= 0) && (depthY < depthHeight))
{
int depthIndex = (depthY * depthWidth) + depthX;
// If we are tracking a body for the current pixel, do not zero out the pixel
if (bodyIndexDataPointer[depthIndex] != 0xff)
{
continue;
}
}
}
bitmapPixelsPointer[colorIndex] = 0;
}
}
bitmap.AddDirtyRect(new System.Windows.Int32Rect(0, 0, bitmap.PixelWidth, bitmap.PixelHeight));
}
}
}
finally
{
if (isBitmapLocked)
{
bitmap.Unlock();
var w = bitmap.PixelWidth;
var h = bitmap.PixelHeight;
var stride = w * ((bitmap.Format.BitsPerPixel + 7) / 8);
var bitmapData = new byte[h * stride];
bitmap.CopyPixels(bitmapData, stride, 0);
//greenScreenFrameData
}
if (depthFrame != null)
{
depthFrame.Dispose();
}
if (colorFrame != null)
{
colorFrame.Dispose();
}
if (bodyIndexFrame != null)
{
bodyIndexFrame.Dispose();
}
}
}