public static ImageSource ToBitmap(this BodyIndexFrame frame)
{
int width = frame.FrameDescription.Width;
int height = frame.FrameDescription.Height;
PixelFormat format = PixelFormats.Bgr32;
byte[] frameData = new byte[width * height];
byte[] pixels = new byte[width * height * (format.BitsPerPixel + 7) / 8];
//frame.CopyFrameDataToArray(frameData);
frame.CopyFrameDataToArray(frameData);
int colorIndex = 0;
for (int depthIndex = 0; depthIndex < frameData.Length; ++depthIndex)
{
ushort depth = frameData[depthIndex];
byte intensity = (byte)(0);
pixels[colorIndex++] = intensity; // Blue
pixels[colorIndex++] = intensity; // Green
pixels[colorIndex++] = intensity; // Red
++colorIndex;
}
int stride = width * format.BitsPerPixel / 8;
return(BitmapSource.Create(width, height, 96, 96, format, null, pixels, stride));
}
private void IndexToBitmap(BodyIndexFrame frame)
{
int width = frame.FrameDescription.Width;
int height = frame.FrameDescription.Height;
frame.CopyFrameDataToArray(indexData);
int colorIndex = 0;
for (int i = 0; i < indexData.Length; ++i)
{
byte intensity = 0;
if (indexData[i] < 6)
{
intensity = 255;
}
indexBuffer[colorIndex++] = intensity; // Blue
indexBuffer[colorIndex++] = intensity; // Green
indexBuffer[colorIndex++] = intensity; // Red
++colorIndex;
}
int stride = width * PixelFormats.Bgr32.BitsPerPixel / 8;
this.indexBitmap.WritePixels(new Int32Rect(0, 0, width, height), indexBuffer, stride, 0);
}
private void Reader_BodyIndexFrameArrived(object sender, BodyIndexFrameArrivedEventArgs e)
{
using (BodyIndexFrame bodyIndexFrame = e.FrameReference.AcquireFrame())
{
if (bodyIndexFrame != null)
{
bodyIndexFrame.CopyFrameDataToArray(bodyIndexPixels);
}
for (int i = 0; i < bodyIndexPixels.Length; ++i)
{
if (bodyIndexPixels[i] != 255)
{
var color = bodyIndexColors[bodyIndexPixels[i]];
bitmapPixels[i * 4 + 0] = color.B;
bitmapPixels[i * 4 + 1] = color.G;
bitmapPixels[i * 4 + 2] = color.R;
bitmapPixels[i * 4 + 3] = 255;
}
else
{
bitmapPixels[i * 4 + 0] = 0;
bitmapPixels[i * 4 + 1] = 0;
bitmapPixels[i * 4 + 2] = 0;
bitmapPixels[i * 4 + 3] = 255;
}
}
bodyIndexBitmap.WritePixels(new Int32Rect(0, 0,
bodyIndexBitmap.PixelWidth,
bodyIndexBitmap.PixelHeight),
bitmapPixels,
bodyIndexBitmap.PixelWidth * 4, 0);
}
}
public BodyMetaData(BodyIndexFrame myframeBodyIndex)
{
pixels = new byte[DepthMetaData.XDepthMaxKinect * DepthMetaData.YDepthMaxKinect];
bodyIndexWidth = myframeBodyIndex.FrameDescription.Width;
bodyIndexHeight = myframeBodyIndex.FrameDescription.Height;
myframeBodyIndex.CopyFrameDataToArray(Pixels);
}
public unsafe void Update(BodyIndexFrame frame)
{
if (bodyIdx == null)
{
bodyIdx = new byte[DepthWidth * DepthHeight];
}
frame.CopyFrameDataToArray(bodyIdx);
}
/// <summary>
/// Converts a depth frame to the corresponding System.Windows.Media.Imaging.BitmapSource with the players highlighted.
/// </summary>
/// <param name="depthFrame">The specified depth frame.</param>
/// <param name="bodyIndexFrame">The specified body index frame.</param>
/// <returns>The corresponding System.Windows.Media.Imaging.BitmapSource representation of the depth frame.</returns>
public static BitmapSource ToBitmap(this DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
ushort minDepth = depthFrame.DepthMinReliableDistance;
ushort maxDepth = depthFrame.DepthMaxReliableDistance;
if (_bodyData == null)
{
_width = depthFrame.FrameDescription.Width;
_height = depthFrame.FrameDescription.Height;
_depthData = new ushort[_width * _height];
_bodyData = new byte[_width * _height];
_pixels = new byte[_width * _height * Constants.BYTES_PER_PIXEL];
_bitmap = new WriteableBitmap(_width, _height, Constants.DPI, Constants.DPI, Constants.FORMAT, null);
}
depthFrame.CopyFrameDataToArray(_depthData);
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
// Convert the depth to RGB
for (int depthIndex = 0, colorPixelIndex = 0; depthIndex < _depthData.Length && colorPixelIndex < _pixels.Length; depthIndex++, colorPixelIndex += 4)
{
// Get the depth for this pixel
ushort depth = _depthData[depthIndex];
byte player = _bodyData[depthIndex];
// To convert to a byte, we're discarding the most-significant
// rather than least-significant bits.
// We're preserving detail, although the intensity will "wrap."
// Values outside the reliable depth range are mapped to 0 (black).
byte intensity = (byte)(depth >= minDepth && depth <= maxDepth ? depth : 0);
if (player != 0xff)
{
// Color player gold.
_pixels[colorPixelIndex + 0] = Colors.Gold.B; // B
_pixels[colorPixelIndex + 1] = Colors.Gold.G; // G
_pixels[colorPixelIndex + 2] = Colors.Gold.R; // R
}
else
{
// Color the rest of the image in grayscale.
_pixels[colorPixelIndex + 0] = intensity; // B
_pixels[colorPixelIndex + 1] = intensity; // G
_pixels[colorPixelIndex + 2] = intensity; // R
}
}
_bitmap.Lock();
Marshal.Copy(_pixels, 0, _bitmap.BackBuffer, _pixels.Length);
_bitmap.AddDirtyRect(new Int32Rect(0, 0, _width, _height));
_bitmap.Unlock();
return(_bitmap);
}
/// <summary>
/// Handles the 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_BodyIndexFrameArrived(object sender, BodyIndexFrameArrivedEventArgs e)
{
using (BodyIndexFrame bodyIndexFrame = e.FrameReference.AcquireFrame())
{
if (bodyIndexFrame != null)
{
bodyIndexFrame.CopyFrameDataToArray(bodyIndexDataArray);
}
}
}
// *************************************************************************************************************************
bool RefreshFrame(MultiSourceFrame _frame) //updateframe to our array
{
if (_frame == null)
{
return(false);
}
try{
bodyframe = _frame.BodyFrameReference.AcquireFrame();
depthFrame = _frame.DepthFrameReference.AcquireFrame();
colorFrame = _frame.ColorFrameReference.AcquireFrame();
bodyIndexFrame = _frame.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 || bodyframe == null))
{
return(false);
}
else
{
if (BodyData == null)
{
BodyData = new Body[_Sensor.BodyFrameSource.BodyCount];
}
// update all the array and points
bodyframe.GetAndRefreshBodyData(BodyData);
colorFrame.CopyConvertedFrameDataToArray(colorData, ColorImageFormat.Rgba);
depthFrame.CopyFrameDataToArray(depthData);
bodyIndexFrame.CopyFrameDataToArray(bodyIndexData);
// update all the array and points
_frame = null;
return(true);
}
}
finally
{
if (bodyframe != null)
{
bodyframe.Dispose();
}
if (depthFrame != null)
{
depthFrame.Dispose();
}
if (colorFrame != null)
{
colorFrame.Dispose();
}
if (bodyIndexFrame != null)
{
bodyIndexFrame.Dispose();
}
}
}
public ushort[,] depthFrameToPixelsData(DepthFrame depth_Frame, BodyIndexFrame bodyIndex_Frame)
{
ushort[,] darray = new ushort[424, 512];
if (depth_Frame != null && bodyIndex_Frame != null)
{
int width = depth_Frame.FrameDescription.Width;
int height = depth_Frame.FrameDescription.Height;
byte[] _bodyData = new byte[width * height];
bodyIndex_Frame.CopyFrameDataToArray(_bodyData);
//Console.WriteLine("Width" + width);
//Console.WriteLine("Height" + height);
PixelFormat format = PixelFormats.Bgr32;
stride = width * ((format.BitsPerPixel + 7) / 8);
ushort minDepth = depth_Frame.DepthMinReliableDistance;
ushort maxDepth = depth_Frame.DepthMaxReliableDistance;
ushort[] pixelData = new ushort[width * height];
byte[] pixels = new byte[width * height * (format.BitsPerPixel + 7) / 8];
depth_Frame.CopyFrameDataToArray(pixelData);
int j = 0, k = 0;
for (int i = 0; i < (424 * 512) && j < 424; i++)
{
if (_bodyData[i] != 0xff)
{
darray[j, k] = pixelData[i];
}
else
{
darray[j, k] = 0;
}
k++;
if (k % 512 == 0)
{
k = 0;
j++;
}
}
}
return(darray);
}
/// <summary>
/// Updates the bitmap with new frame data and highlights the players.
/// </summary>
/// <param name="depthFrame">The specified Kinect depth frame.</param>
/// <param name="bodyIndexFrame">The specified Kinect body index frame.</param>
public void Update(DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
ushort minDepth = depthFrame.DepthMinReliableDistance;
ushort maxDepth = depthFrame.DepthMaxReliableDistance;
if (BodyData == null)
{
Width = depthFrame.FrameDescription.Width;
Height = depthFrame.FrameDescription.Height;
DepthData = new ushort[Width * Height];
BodyData = new byte[Width * Height];
HighlightedPixels = new byte[Width * Height * Constants.BYTES_PER_PIXEL];
HighlightedBitmap = new Bitmap(Width, Height, Constants.FORMAT);
}
depthFrame.CopyFrameDataToArray(DepthData);
bodyIndexFrame.CopyFrameDataToArray(BodyData);
// Convert the depth to RGB
for (int depthIndex = 0, colorPixelIndex = 0; depthIndex < DepthData.Length && colorPixelIndex < HighlightedPixels.Length; depthIndex++, colorPixelIndex += 4)
{
// Get the depth for this pixel
ushort depth = DepthData[depthIndex];
byte player = BodyData[depthIndex];
// To convert to a byte, we're discarding the most-significant
// rather than least-significant bits.
// We're preserving detail, although the intensity will "wrap."
// Values outside the reliable depth range are mapped to 0 (black).
byte intensity = (byte)(depth >= minDepth && depth <= maxDepth ? depth : 0);
if (player != 0xff)
{
// Color player gold.
HighlightedPixels[colorPixelIndex + 0] = Color.Gold.B; // B
HighlightedPixels[colorPixelIndex + 1] = Color.Gold.G; // G
HighlightedPixels[colorPixelIndex + 2] = Color.Gold.R; // R
}
else
{
// Color the rest of the image in grayscale.
HighlightedPixels[colorPixelIndex + 0] = intensity; // B
HighlightedPixels[colorPixelIndex + 1] = intensity; // G
HighlightedPixels[colorPixelIndex + 2] = intensity; // R
}
}
BitmapData bitmapData = HighlightedBitmap.LockBits(new Rectangle(0, 0, Width, Height), ImageLockMode.ReadWrite, HighlightedBitmap.PixelFormat);
Marshal.Copy(HighlightedPixels, 0, bitmapData.Scan0, HighlightedPixels.Length);
HighlightedBitmap.UnlockBits(bitmapData);
}
public void Reader_FrameArrived(object sender, BodyIndexFrameArrivedEventArgs e)
{
using (BodyIndexFrame bodyindexFrame = e.FrameReference.AcquireFrame())
{
if (bodyindexFrame != null)
{
byte[] bodyindexData = new byte[depthFrameWidth * depthFrameHeight];
bodyindexFrame.CopyFrameDataToArray(bodyindexData);
bodyindexframes.Add(bodyindexData);
}
}
}
/// <summary>
/// Updates the bitmap with new frame data and highlights the players.
/// </summary>
/// <param name="depthFrame">The specified Kinect depth frame.</param>
/// <param name="bodyIndexFrame">The specified Kinect body index frame.</param>
public void Update(DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
ushort minDepth = depthFrame.DepthMinReliableDistance;
ushort maxDepth = depthFrame.DepthMaxReliableDistance;
if (BodyData == null)
{
Width = depthFrame.FrameDescription.Width;
Height = depthFrame.FrameDescription.Height;
DepthData = new ushort[Width * Height];
BodyData = new byte[Width * Height];
HighlightedPixels = new byte[Width * Height * Constants.BYTES_PER_PIXEL];
HighlightedBitmap = new WriteableBitmap(Width, Height);
HighlightedStream = HighlightedBitmap.PixelBuffer.AsStream();
}
depthFrame.CopyFrameDataToArray(DepthData);
bodyIndexFrame.CopyFrameDataToArray(BodyData);
// Convert the depth to RGB
for (int depthIndex = 0, colorPixelIndex = 0; depthIndex < DepthData.Length && colorPixelIndex < HighlightedPixels.Length; depthIndex++, colorPixelIndex += 4)
{
// Get the depth for this pixel
ushort depth = DepthData[depthIndex];
byte player = BodyData[depthIndex];
// To convert to a byte, we're discarding the most-significant
// rather than least-significant bits.
// We're preserving detail, although the intensity will "wrap."
// Values outside the reliable depth range are mapped to 0 (black).
byte intensity = (byte)(depth >= minDepth && depth <= maxDepth ? depth : 0);
if (player != 0xff)
{
// Color player gold.
HighlightedPixels[colorPixelIndex + 0] = Colors.Gold.B; // B
HighlightedPixels[colorPixelIndex + 1] = Colors.Gold.G; // G
HighlightedPixels[colorPixelIndex + 2] = Colors.Gold.R; // R
}
else
{
// Color the rest of the image in grayscale.
HighlightedPixels[colorPixelIndex + 0] = intensity; // B
HighlightedPixels[colorPixelIndex + 1] = intensity; // G
HighlightedPixels[colorPixelIndex + 2] = intensity; // R
}
}
HighlightedStream.Seek(0, SeekOrigin.Begin);
HighlightedStream.Write(HighlightedPixels, 0, HighlightedPixels.Length);
HighlightedBitmap.Invalidate();
}
public Tuple <BitmapSource, TimeSpan> CaptureBodyIndexFrameBitmap(LiveFrame frame, byte[] buffer)
{
BodyIndexFrame bodyIndexFrame = frame.NativeBodyIndexFrame;
int width = bodyIndexFrame.FrameDescription.Width;
int height = bodyIndexFrame.FrameDescription.Height;
bodyIndexFrame.CopyFrameDataToArray(_teenyBuffer);
BitmapSource result = BufferCaptureBitmapHelper(_teenyBuffer, width, height, 1, buffer);
return(new Tuple <BitmapSource, TimeSpan>(result, bodyIndexFrame.RelativeTime));
}
//frame取得時のイベント
public void BodyIndexFrame_Arrived(object sender, BodyIndexFrameArrivedEventArgs e)
#region
{
BodyIndexFrame bodyIndexFrame = e.FrameReference.AcquireFrame();
if (bodyIndexFrame == null)
{
return;
}
bodyIndexFrame.CopyFrameDataToArray(bodyIndexBuffer); //人がいないところ0xff いるところ0-6？
this.KinectImagetoMat(this.kinectImage, this.bodyIndexBuffer);
//this._showImageEvent();
bodyIndexFrame.Dispose();
}
void reader_MultiSourceFrameArrived(MultiSourceFrameReader sender, MultiSourceFrameArrivedEventArgs args)
{
using (MultiSourceFrame frame = args.FrameReference.AcquireFrame())
{
if (frame != null)
{
//Récupération des images
using (BodyFrame bodyFrame = frame.BodyFrameReference.AcquireFrame()) //################# BODYFRAME
using (BodyIndexFrame bodyIndexFrame = frame.BodyIndexFrameReference.AcquireFrame()) //################# BODY INDEX
using (ColorFrame colorFrame = frame.ColorFrameReference.AcquireFrame()) //################# COLOR
{
if (bodyIndexFrame != null && bodyFrame != null && colorFrame != null)
{
//Initilisation et mise à jour du tableau de body
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
bodyFrame.GetAndRefreshBodyData(this.bodies);
//WriteableBitmap bitmap = (WriteableBitmap)this.kinectImage.Source;
//Tableau contenant les données bodyIndex
byte[] bodyIndexArray = new byte[bodyIndexFrame.FrameDescription.LengthInPixels * bodyIndexFrame.FrameDescription.BytesPerPixel];
bodyIndexFrame.CopyFrameDataToArray(bodyIndexArray);
colorFrame.CopyConvertedFrameDataToBuffer(bitmap.PixelBuffer, ColorImageFormat.Bgra);
foreach (Body b in this.bodies)
{
ColorSpacePoint[] colorPoint = new ColorSpacePoint[1];
kinect.CoordinateMapper.MapCameraPointsToColorSpace(new CameraSpacePoint[] { b.Joints[JointType.HandLeft].Position }, colorPoint);
try
{
Canvas.SetLeft(ellipse, colorPoint[0].X);
Canvas.SetTop(ellipse, colorPoint[0].Y);
}catch (Exception e)
{
}
}
}
}
}
}
}
private void DepthFrameReady(object sender, BodyIndexFrameArrivedEventArgs e)
{
BodyIndexFrame frame = e.FrameReference.AcquireFrame();
if (frame != null)
{
this.FInvalidate = true;
this.frameindex = frame.RelativeTime.Ticks;
lock (m_lock)
{
frame.CopyFrameDataToArray(this.rawdepth);
}
frame.Dispose();
}
}
internal static void CopyToFrameToPixelArray(this BodyIndexFrame bodyIndexFrame, ref byte[] frameData, ref byte[] pixels)
{
var pixelIndex = 0;
bodyIndexFrame.CopyFrameDataToArray(frameData);
for (int i = 0; i < frameData.Length; ++i)
{
var color = BodyIndexColor.GetColorFromBodyIndex(frameData[i]);
pixels[pixelIndex++] = color.B;
pixels[pixelIndex++] = color.G;
pixels[pixelIndex++] = color.R;
pixels[pixelIndex++] = color.A;
}
}
private void ProcessBodyIndexFrame(BodyIndexFrame bodyIndexFrame)
{
if (bodyIndexFrame != null)
{
byte[] outBuffer = _displayableBuffers[SourceType.BODY_INDEX];
bodyIndexFrame.CopyFrameDataToArray(_rawBodyIndexPixels);
bodyIndexFrame.Dispose();
int outIndex = 0;
for (int inIndex = 0; inIndex < _rawBodyIndexPixels.Length; ++inIndex)
{
int bodyIndex = _rawBodyIndexPixels[inIndex];
UInt32 thisColor = 0;
_bodyIndexToColorMap.TryGetValue(bodyIndex, out thisColor);
outBuffer[outIndex++] = (byte)((thisColor & 0xFF000000) >> 32); // Blue channel
outBuffer[outIndex++] = (byte)((thisColor & 0x00FF0000) >> 16); // Green channel
outBuffer[outIndex++] = (byte)((thisColor & 0x0000FF00) >> 8); // Red channel
outBuffer[outIndex++] = (byte)((thisColor & 0x000000FF)); // Alpha channel
}
}
}
/*
* The pixel values in this frame are 8-bit unsigned integers, where 0-5 map directly to the BodyData index in the BodyFrame.
* Values greater than the value obtained from BodyCount indicate the pixel is part of the background, not associated with a tracked body.
* This frame is useful for green screening applications, or any scenario where you want to display the silhouette of the user.
* It also provides a good starting bounds for custom depth algorithms.
*/
private void updateBodyIndexEvent(object sender, BodyIndexFrameArrivedEventArgs e)
{
using (BodyIndexFrame frame = e.FrameReference.AcquireFrame())
{
if (frame != null)
{
FrameDescription frameDescription = frame.FrameDescription;
// verify data and write the data to the display bitmap
if ((frameDescription.Width * frameDescription.Height) == 424 * 512)
{
WriteableBitmap Bitmap = BitmapFactory.New(frameDescription.Width, frameDescription.Height);
byte[] frameData = new byte[frameDescription.Width * frameDescription.Height];
frame.CopyFrameDataToArray(frameData);
for (int y = 0; y < frameDescription.Height; y++)
{
for (int x = 0; x < frameDescription.Width; x++)
{
int index = y * frameDescription.Width + x;
if (frameData[index] != 0xff)
{
myColor.HSB hsb = new myColor.HSB();
hsb.H = x / frameDescription.Height * 255;
hsb.S = Numbers.ConvertToRange(y, 0, frameDescription.Height / 2, 0, 1, true);
hsb.B = Numbers.ConvertToRange(y, frameDescription.Height / 2, frameDescription.Height, 1, 0, true);
System.Drawing.Color color = myColor.FromHSB(hsb);
// make a dynamic image, also there can be up to 6 images so we need them to be a little different
Bitmap.SetPixel(x, y, color.A, color.R, color.G, color.B);
}
else
{
Bitmap.SetPixel(x, y, 255, 0, 255, 0); // green
}
}
}
SendImage(Bitmap, "kinectbodyindex");
}
}
}
}
private void DepthFrameReady(object sender, BodyIndexFrameArrivedEventArgs e)
{
BodyIndexFrame frame = e.FrameReference.AcquireFrame();
int bg = this.backcolor;
if (frame != null)
{
this.FInvalidate = true;
this.frameindex = frame.RelativeTime.Ticks;
lock (m_lock)
{
frame.CopyFrameDataToArray(this.rawdepth);
for (int i16 = 0; i16 < 512 * 424; i16++)
{
byte player = rawdepth[i16];
this.playerimage[i16] = player == 255 ? bg : this.colors[player % 6];
}
}
frame.Dispose();
}
}
private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
// All frame counter
this.frameCount++;
if (this.frameCount % this.framesToCapture != 0)
{
return;
}
ColorFrame colorFrame = null;
DepthFrame depthFrame = null;
BodyFrame bodyFrame = null;
BodyIndexFrame bodyIndexFrame = null;
Body body = null;
SkeletonOfBody skel_up = new SkeletonOfBody(Constants.SKEL_UP_TOTAL_JOINTS);
try
{
var frameReference = e.FrameReference.AcquireFrame();
colorFrame = frameReference.ColorFrameReference.AcquireFrame();
depthFrame = frameReference.DepthFrameReference.AcquireFrame();
bodyFrame = frameReference.BodyFrameReference.AcquireFrame();
bodyIndexFrame = frameReference.BodyIndexFrameReference.AcquireFrame();
if (colorFrame == null || depthFrame == null || bodyFrame == null || bodyIndexFrame == null)
{
return;
}
//--------------------------------------------
// Get the color frame
//--------------------------------------------
using (KinectBuffer colorBuffer = colorFrame.LockRawImageBuffer())
{
colorFrame.CopyConvertedFrameDataToArray(this.buffColor32, ColorImageFormat.Bgra);
} //End ColorFrame
//--------------------------------------------
// Get the depth frame
//--------------------------------------------
using (KinectBuffer depthBuffer = depthFrame.LockImageBuffer())
{
depthFrame.CopyFrameDataToArray(this.buffDepth16);
//depthFrame.CopyFrameDataToArray(this.buffDepth16Copy);
// Multiplication by 20 only to turn the depth visually more perceptible
//int i = 0;
//Array.ForEach(this.buffDepth16Copy, (x) => { this.buffDepth16Copy[i++] = (ushort)(x * 20); });
} //End DepthFrame
//--------------------------------------------
// Get the body index frame
//--------------------------------------------
using (KinectBuffer bodyIndexBuffer = bodyIndexFrame.LockImageBuffer())
{
bodyIndexFrame.CopyFrameDataToArray(this.buffBodyIndex8);
}
//--------------------------------------------
// Get the body frame
//--------------------------------------------
bodyFrame.GetAndRefreshBodyData(this.listBodies);
//bodyFrame.FloorClipPlane.
//--------------------------------------------
// Map the depth frame to it color frame
//--------------------------------------------
{
Array.Clear(this.buffColorSpacePoints, 0, this.buffColorSpacePoints.Length);
Array.Clear(this.buffMapDepthToColor32, 0, this.buffMapDepthToColor32.Length);
// Coordinate mapping
this.coordinateMapper.MapDepthFrameToColorSpace(this.buffDepth16, this.buffColorSpacePoints);
unsafe
{
fixed(ColorSpacePoint *depthMappedToColorPointsPointer = buffColorSpacePoints)
{
// Loop over each row and column of the color image
// Zero out any pixels that don't correspond to a body index
for (int idxDepth = 0; idxDepth < buffColorSpacePoints.Length; ++idxDepth)
{
float depthMappedToColorX = depthMappedToColorPointsPointer[idxDepth].X;
float depthMappedToColorY = depthMappedToColorPointsPointer[idxDepth].Y;
// The sentinel value is -inf, -inf, meaning that no depth pixel corresponds to this color pixel.
if (!float.IsNegativeInfinity(depthMappedToColorX) &&
!float.IsNegativeInfinity(depthMappedToColorY))
{
// Make sure the depth pixel maps to a valid point in color space
int colorX = (int)(depthMappedToColorX + 0.5f);
int colorY = (int)(depthMappedToColorY + 0.5f);
// If the point is not valid, there is no body index there.
if ((colorX >= 0) && (colorX < this.colorImageSize.Width) && (colorY >= 0) && (colorY < this.colorImageSize.Height))
{
int idxColor = (colorY * this.colorImageSize.Width) + colorX;
// If we are tracking a body for the current pixel, save the depth data
if (this.buffBodyIndex8[idxDepth] != 0xff)
{
this.buffMapDepthToColor32[idxDepth * 4] = this.buffColor32[idxColor * 4]; // B
this.buffMapDepthToColor32[idxDepth * 4 + 1] = this.buffColor32[idxColor * 4 + 1]; // G
this.buffMapDepthToColor32[idxDepth * 4 + 2] = this.buffColor32[idxColor * 4 + 2]; // R
}
}
}
}
}
} //End Unsafe
} //End Mapping
//--------------------------------------------
// Process the face of the default body
//--------------------------------------------
// Variable to save the detected face paramenters
this.faceData = new FaceData(new BoxFace(0, 0, 0, 0), new BoxFace(0, 0, 0, 0));
#if FACE_DETECTION
// Get the default body
// Body body = this.listBodies.Where(b => b.IsTracked).FirstOrDefault();
if (this.faceFrameSource.IsActive)
{
// In our experiment we get the closest body
body = Util.GetClosestBody(this.listBodies);
if (body != null && body.IsTracked)
{
// Get the first skeleton
skel_up = Util.GetSkeletonUpperBody(this.Mapper, body);
// Draw skeleton joints
if (this.drawingDepthMarkups)
{
Util.WriteSkeletonOverFrame(this, VisTypes.Depth, skel_up, 2, ref this.buffMapDepthToColor32);
//Util.WriteSkeletonOverFrame(this, VisTypes.Depth, skeleton, 2, ref this.buffDepth16);
}
// Assign a tracking ID to the face source
this.faceFrameSource.TrackingId = body.TrackingId;
if (this.faceFrameResults != null)
{
var boxColor = this.faceFrameResults.FaceBoundingBoxInColorSpace;
var boxDepth = this.faceFrameResults.FaceBoundingBoxInInfraredSpace;
// If there are face results, then save data
// We save in a format of rectangle [x, y, width, height]
this.faceData.boxColor = new BoxFace(boxColor.Left, boxColor.Top, (boxColor.Right - boxColor.Left), (boxColor.Bottom - boxColor.Top));
this.faceData.boxDepth = new BoxFace(boxDepth.Left, boxDepth.Top, (boxDepth.Right - boxDepth.Left), (boxDepth.Bottom - boxDepth.Top));
// Draw the face
if (this.drawingDepthMarkups)
{
Util.WriteFaceOverFrame(this, VisTypes.Depth, faceData.boxDepth, 1, ref this.buffMapDepthToColor32);
//Util.WriteFaceOverFrame(this, VisTypes.Depth, faceData.boxDepth, 1, ref this.buffDepth16);
} //End Drawing
} //End FaceResult
} //End Body
}
#endif
// Update the data handler
this.frameHandler(
this.buffColor32,
this.buffDepth16,
this.buffBodyIndex8,
this.buffMapDepthToColor32,
this.listBodies,
this.faceData
);
// Recording state ture
byte[] _colorData = null;
ushort[] _depthData = null;
byte[] _bodyIndexData = null;
IList <Body> _bodies = null;
//--------------------------------------------
// Record the data
//--------------------------------------------
if (this.stateOfRecording)
{
// 25-09-15
// Discard frames where the hand is not corrected tracked (i.e., the hand has a zero coordinate)
// To discard failures in hand tracking
if (skel_up.jointDepthSpace[(int)JointUpType.HandLeft].X == 0 || skel_up.jointDepthSpace[(int)JointUpType.HandLeft].Y == 0 ||
skel_up.jointDepthSpace[(int)JointUpType.HandRight].X == 0 || skel_up.jointDepthSpace[(int)JointUpType.HandRight].Y == 0)
{
Console.WriteLine("Neglect frame {0}", this.recordedFrames);
return;
}
// Storage data;
_colorData = new byte[this.buffColor32.Length];
_depthData = new ushort[this.buffDepth16.Length];
_bodyIndexData = new byte[this.buffBodyIndex8.Length];
_bodies = new Body[this.listBodies.Count];
colorFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Bgra);
depthFrame.CopyFrameDataToArray(_depthData);
bodyIndexFrame.CopyFrameDataToArray(_bodyIndexData);
bodyFrame.GetAndRefreshBodyData(_bodies);
// Increase the counter
this.recordedFrames++;
this.dataContainer.AddColor = _colorData;
this.dataContainer.AddDepth = _depthData;
this.dataContainer.AddBodyIndex = _bodyIndexData;
this.dataContainer.AddListOfBodies = _bodies;
this.dataContainer.AddFaceData = this.faceData;
// If the user only require to save a fixed number of frames
if (this.fixedFrames == this.recordedFrames)
{
this.stateOfRecording = false;
}
}
// Notice:
// Array.Copy() --> how many elements to copy
// Buffer.BlockCopy --> how many of bytes to copy
// Update Frame Rate
UpdateGrabberFrameRate();
}
finally
{
if (this.frameCount > 100000000)
{
this.frameCount = 0;
}
if (colorFrame != null)
{
colorFrame.Dispose();
}
if (depthFrame != null)
{
depthFrame.Dispose();
}
if (bodyFrame != null)
{
bodyFrame.Dispose();
}
if (bodyIndexFrame != null)
{
bodyIndexFrame.Dispose();
}
}
}
/// <summary>
/// Converts a depth frame to the corresponding System.Windows.Media.Imaging.BitmapSource and removes the background (green-screen effect).
/// </summary>
/// <param name="depthFrame">The specified depth frame.</param>
/// <param name="colorFrame">The specified color frame.</param>
/// <param name="bodyIndexFrame">The specified body index frame.</param>
/// <returns>The corresponding System.Windows.Media.Imaging.BitmapSource representation of image.</returns>
public Bitmap GreenScreen(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
int colorWidth = colorFrame.FrameDescription.Width;
int colorHeight = colorFrame.FrameDescription.Height;
int depthWidth = depthFrame.FrameDescription.Width;
int depthHeight = depthFrame.FrameDescription.Height;
int bodyIndexWidth = bodyIndexFrame.FrameDescription.Width;
int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;
if (_displayPixels == null)
{
_depthData = new ushort[depthWidth * depthHeight];
_bodyData = new byte[depthWidth * depthHeight];
_colorData = new byte[colorWidth * colorHeight * Constants.BYTES_PER_PIXEL];
_displayPixels = new byte[depthWidth * depthHeight * Constants.BYTES_PER_PIXEL];
_colorPoints = new ColorSpacePoint[depthWidth * depthHeight];
_bitmap = new Bitmap(depthWidth, depthHeight, Constants.FORMAT);
}
if (((depthWidth * depthHeight) == _depthData.Length) && ((colorWidth * colorHeight * Constants.BYTES_PER_PIXEL) == _colorData.Length) && ((bodyIndexWidth * bodyIndexHeight) == _bodyData.Length))
{
depthFrame.CopyFrameDataToArray(_depthData);
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(_colorData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Bgra);
}
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
CoordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);
Array.Clear(_displayPixels, 0, _displayPixels.Length);
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
int depthIndex = (y * depthWidth) + x;
byte player = _bodyData[depthIndex];
if (player != 0xff)
{
ColorSpacePoint colorPoint = _colorPoints[depthIndex];
int colorX = (int)Math.Floor(colorPoint.X + 0.5);
int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
{
int colorIndex = ((colorY * colorWidth) + colorX) * Constants.BYTES_PER_PIXEL;
int displayIndex = depthIndex * Constants.BYTES_PER_PIXEL;
_displayPixels[displayIndex + 0] = _colorData[colorIndex];
_displayPixels[displayIndex + 1] = _colorData[colorIndex + 1];
_displayPixels[displayIndex + 2] = _colorData[colorIndex + 2];
_displayPixels[displayIndex + 3] = 0xff;
}
}
}
}
BitmapData bitmapData = _bitmap.LockBits(new Rectangle(0, 0, depthWidth, depthHeight), ImageLockMode.ReadWrite, _bitmap.PixelFormat);
Marshal.Copy(_displayPixels, 0, bitmapData.Scan0, _displayPixels.Length);
_bitmap.UnlockBits(bitmapData);
}
return(_bitmap);
}
/// <summary>
/// 人が映った画素領域だけを映した BitmapSource を取得します。
/// </summary>
/// <param name="colorFrame">
/// カラー画像フレーム。
/// </param>
/// <param name="depthFrame">
/// 深度フレーム。
/// </param>
/// <param name="bodyIndexFrame">
/// BodyIndex フレーム。
/// </param>
/// <returns>
/// 人が映った画素領域だけを映した BitmapSource。
/// </returns>
BitmapSource GetUserMaskImage(ColorFrame colorFrame,
DepthFrame depthFrame,
BodyIndexFrame bodyIndexFrame)
{
byte[] colors = new byte[this.colorFrameDescription.Width
* this.colorFrameDescription.Height
* this.colorFrameDescription.BytesPerPixel];
colorFrame.CopyConvertedFrameDataToArray(colors, this.colorImageFormat);
ushort[] depths = new ushort[this.depthFrameDescription.Width
* this.depthFrameDescription.Height];
depthFrame.CopyFrameDataToArray(depths);
byte[] bodyIndexes = new byte[this.bodyIndexFrameDescription.Width
* this.bodyIndexFrameDescription.Height];
bodyIndexFrame.CopyFrameDataToArray(bodyIndexes);
//人が映っているだけの画像を表す byte 配列を用意して 0 で初期化する。
byte[] bodyColors = new byte[this.bodyIndexFrameDescription.Width
* this.bodyIndexFrameDescription.Height
* this.colorFrameDescription.BytesPerPixel];
Array.Clear(bodyColors, 0, bodyColors.Length);
//カラー画像と深度画像の座標を対応させる(= マッピングする)必要がある。
//ColorSpacePoint はカラー画像の座標を示す構造体。
ColorSpacePoint[] colorSpacePoints
= new ColorSpacePoint[this.bodyIndexFrameDescription.Width
* this.bodyIndexFrameDescription.Height];
this.kinect.CoordinateMapper.MapDepthFrameToColorSpace(depths, colorSpacePoints);
for (int y = 0; y < this.depthFrameDescription.Height; y++)
{
for (int x = 0; x < this.depthFrameDescription.Width; x++)
{
int depthsIndex = y * this.depthFrameDescription.Width + x;
if (bodyIndexes[depthsIndex] != 255)
{
//対応するカラー画像の座標を取得し、
//カラー画像座標系の範囲内に収まるかを判定する。
ColorSpacePoint colorSpacePoint = colorSpacePoints[depthsIndex];
int colorX = (int)colorSpacePoint.X;
int colorY = (int)colorSpacePoint.Y;
if ((colorSpacePoint.X >= 0) &&
(colorSpacePoint.X < this.colorFrameDescription.Width) &&
(colorSpacePoint.Y >= 0) &&
(colorSpacePoint.Y < this.colorFrameDescription.Height))
{
//対応するカラー画像の画素から色情報を取得して、
//新しいカラー画像のデータに与える。
int colorsIndex =
(colorY * this.colorFrameDescription.Width + colorX) * 4;
int bodyColorsIndex = depthsIndex * 4;
bodyColors[bodyColorsIndex] = colors[colorsIndex];
bodyColors[bodyColorsIndex + 1] = colors[colorsIndex + 1];
bodyColors[bodyColorsIndex + 2] = colors[colorsIndex + 2];
bodyColors[bodyColorsIndex + 3] = 255;
}
}
}
}
BitmapSource bitmapSource
= BitmapSource.Create(depthFrameDescription.Width,
depthFrameDescription.Height,
96,
96,
PixelFormats.Bgra32,
null,
bodyColors,
depthFrameDescription.Width * 4);
return(bitmapSource);
}
/// <summary>
/// Updates the bitmap with new frame data.
/// </summary>
/// <param name="depthFrame">The specified depth frame.</param>
/// <param name="colorFrame">The specified color frame.</param>
/// <param name="bodyIndexFrame">The specified body index frame.</param>
override public void Update(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
int colorWidth = colorFrame.FrameDescription.Width;
int colorHeight = colorFrame.FrameDescription.Height;
int depthWidth = depthFrame.FrameDescription.Width;
int depthHeight = depthFrame.FrameDescription.Height;
int bodyIndexWidth = bodyIndexFrame.FrameDescription.Width;
int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;
if (Bitmap == null)
{
InitBuffers(colorFrame.FrameDescription, depthFrame.FrameDescription, bodyIndexFrame.FrameDescription);
}
if (((depthWidth * depthHeight) == _depthData.Length) && ((colorWidth * colorHeight * Constants.BYTES_PER_PIXEL) == Pixels.Length) && ((bodyIndexWidth * bodyIndexHeight) == _bodyData.Length))
{
depthFrame.CopyFrameDataToArray(_depthData);
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(Pixels);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(Pixels, ColorImageFormat.Bgra);
}
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
CoordinateMapper.MapColorFrameToDepthSpace(_depthData, _depthPoints);
// Loop over each row and column of the color image
// Zero out any pixels that don't correspond to a body index
for (int i = 0, ci = 0; i < _depthPoints.Length; ++i, ci += Constants.BYTES_PER_PIXEL)
{
float colorToDepthX = _depthPoints[i].X;
float colorToDepthY = _depthPoints[i].Y;
// The sentinel value is -inf, -inf, meaning that no depth pixel corresponds to this color pixel.
if (!float.IsNegativeInfinity(colorToDepthX) &&
!float.IsNegativeInfinity(colorToDepthY))
{
// Make sure the depth pixel maps to a valid point in color space
int depthX = (int)(colorToDepthX + 0.5f);
int depthY = (int)(colorToDepthY + 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 (_bodyData[depthIndex] != 0xff)
{
continue;
}
}
}
for (int b = 0; b < Constants.BYTES_PER_PIXEL; ++b)
{
Pixels[ci + b] = 0;
}
}
UpdateBitmap();
}
}
/// <summary>
/// Converts a depth frame to the corresponding System.Windows.Media.Imaging.BitmapSource and removes the background (green-screen effect).
/// </summary>
/// <param name="depthFrame">The specified depth frame.</param>
/// <param name="colorFrame">The specified color frame.</param>
/// <param name="bodyIndexFrame">The specified body index frame.</param>
/// <returns>The corresponding System.Windows.Media.Imaging.BitmapSource representation of image.</returns>
public WriteableBitmap GreenScreen(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
int colorWidth = colorFrame.FrameDescription.Width;
int colorHeight = colorFrame.FrameDescription.Height;
int depthWidth = depthFrame.FrameDescription.Width;
int depthHeight = depthFrame.FrameDescription.Height;
int bodyIndexWidth = bodyIndexFrame.FrameDescription.Width;
int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;
if (_displayPixels == null)
{
_depthData = new ushort[depthWidth * depthHeight];
_bodyData = new byte[depthWidth * depthHeight];
_colorData = new byte[colorWidth * colorHeight * Constants.BYTES_PER_PIXEL];
_displayPixels = new byte[depthWidth * depthHeight * Constants.BYTES_PER_PIXEL];
_colorPoints = new ColorSpacePoint[depthWidth * depthHeight];
_bitmap = new WriteableBitmap(depthWidth, depthHeight);
_stream = _bitmap.PixelBuffer.AsStream();
}
if (((depthWidth * depthHeight) == _depthData.Length) && ((colorWidth * colorHeight * Constants.BYTES_PER_PIXEL) == _colorData.Length) && ((bodyIndexWidth * bodyIndexHeight) == _bodyData.Length))
{
depthFrame.CopyFrameDataToArray(_depthData);
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(_colorData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Bgra);
}
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
CoordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);
Array.Clear(_displayPixels, 0, _displayPixels.Length);
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
int depthIndex = (y * depthWidth) + x;
byte player = _bodyData[depthIndex];
if (player != 0xff)
{
ColorSpacePoint colorPoint = _colorPoints[depthIndex];
int colorX = (int)Math.Floor(colorPoint.X + 0.5);
int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
{
int colorIndex = ((colorY * colorWidth) + colorX) * Constants.BYTES_PER_PIXEL;
int displayIndex = depthIndex * Constants.BYTES_PER_PIXEL;
_displayPixels[displayIndex + 0] = _colorData[colorIndex];
_displayPixels[displayIndex + 1] = _colorData[colorIndex + 1];
_displayPixels[displayIndex + 2] = _colorData[colorIndex + 2];
_displayPixels[displayIndex + 3] = 0xff;
}
}
}
}
_stream.Seek(0, SeekOrigin.Begin);
_stream.Write(_displayPixels, 0, _displayPixels.Length);
_bitmap.Invalidate();
}
return _bitmap;
}
private void ProcessFrames(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame, BodyFrame bodyFrame, byte [] psBytes0, byte [] psBytes1)
{
// create multiframe to process
long ticksCopyData = DateTime.Now.Ticks;
MultiFrame multiFrame = new MultiFrame();
multiFrame.FrameNb = Interlocked.Increment(ref frameNb);
// color
long ticksCreateColorData = DateTime.Now.Ticks;
byte[] colorData = new byte[colorByteSize];
Utils.UpdateTimer("CreateColorData", ticksCreateColorData);
long ticksCopyColorData = DateTime.Now.Ticks;
colorFrame.CopyConvertedFrameDataToArray(colorData, ColorImageFormat.Bgra);
Utils.UpdateTimer("CopyColorData", ticksCopyColorData);
// depth
long ticksCreateDepthData = DateTime.Now.Ticks;
ushort[] depthData = new ushort[depthPixelSize];
depthFrame.CopyFrameDataToArray(depthData);
Utils.UpdateTimer("CreateDepthData", ticksCreateDepthData);
// body index
long ticksCreateBodyIndexData = DateTime.Now.Ticks;
byte[] bodyIndexData = new byte[depthPixelSize];
bodyIndexFrame.CopyFrameDataToArray(bodyIndexData);
Utils.UpdateTimer("CreateBodyIndexData", ticksCreateBodyIndexData);
// bodies
long ticksCreateBodiesData = DateTime.Now.Ticks;
Body[] bodies = new Body[bodyFrame.BodyCount];
bodyFrame.GetAndRefreshBodyData(bodies);
Utils.UpdateTimer("CreateBodiesData", ticksCreateBodiesData);
// ps3eye
byte[] psBytes = null;
if (psBytes0 != null && psBytes1 != null)
{
long ticksCreatePS3EyeData = DateTime.Now.Ticks;
psBytes = new byte[psByteSize * 2];
Utils.UpdateTimer("CreatePS3EyeData", ticksCreatePS3EyeData);
long ticksCopyPS3EyeData = DateTime.Now.Ticks;
CopyPS3EyeDataMirror(psBytes, psBytes0, psBytes1);
Utils.UpdateTimer("CopyPS3EyeData", ticksCopyPS3EyeData);
}
// multiFrame
long ticksMultiFrame = DateTime.Now.Ticks;
multiFrame.DepthData = depthData;
multiFrame.ColorData = colorData;
multiFrame.BodyIndexData = bodyIndexData;
multiFrame.Bodies = bodies;
multiFrame.PS3EyeData = psBytes;
multiFrame.HasKinectData = true;
multiFrame.HasPS3EyeData = psBytes != null ? true : false;
Utils.UpdateTimer("MultiFrame", ticksMultiFrame);
long ticksEnqueue = DateTime.Now.Ticks;
ProcessingManager.Instance.EnqueueMultiFrame(multiFrame);
Utils.UpdateTimer("Enqueue", ticksEnqueue);
Utils.UpdateTimer("CopyFramesData", ticksCopyData);
// display timers & queues
Context.GUI.DisplayPerformance();
}
/// <summary>
/// Updates the bitmap with new frame data.
/// </summary>
/// <param name="depthFrame">The specified depth frame.</param>
/// <param name="colorFrame">The specified color frame.</param>
/// <param name="bodyIndexFrame">The specified body index frame.</param>
override public void Update(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
int colorWidth = colorFrame.FrameDescription.Width;
int colorHeight = colorFrame.FrameDescription.Height;
int depthWidth = depthFrame.FrameDescription.Width;
int depthHeight = depthFrame.FrameDescription.Height;
int bodyIndexWidth = bodyIndexFrame.FrameDescription.Width;
int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;
if (Bitmap == null)
{
InitBuffers(colorFrame.FrameDescription, depthFrame.FrameDescription, bodyIndexFrame.FrameDescription);
}
if (((depthWidth * depthHeight) == _depthData.Length) && ((colorWidth * colorHeight * Constants.BYTES_PER_PIXEL) == _colorData.Length) && ((bodyIndexWidth * bodyIndexHeight) == _bodyData.Length))
{
depthFrame.CopyFrameDataToArray(_depthData);
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(_colorData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Bgra);
}
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
CoordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);
Array.Clear(Pixels, 0, Pixels.Length);
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
int depthIndex = (y * depthWidth) + x;
if (_bodyData[depthIndex] != 0xff)
{
ColorSpacePoint colorPoint = _colorPoints[depthIndex];
int colorX = (int)(colorPoint.X + 0.5);
int colorY = (int)(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
{
int colorIndex = ((colorY * colorWidth) + colorX) * Constants.BYTES_PER_PIXEL;
int displayIndex = depthIndex * Constants.BYTES_PER_PIXEL;
for (int b = 0; b < Constants.BYTES_PER_PIXEL; ++b)
{
Pixels[displayIndex + b] = _colorData[colorIndex + b];
}
}
}
}
}
UpdateBitmap();
}
}
/// <summary>
/// Updates the bitmap with new frame data.
/// </summary>
/// <param name="depthFrame">The specified depth frame.</param>
/// <param name="colorFrame">The specified color frame.</param>
/// <param name="bodyIndexFrame">The specified body index frame.</param>
override public void Update(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
int colorWidth = colorFrame.FrameDescription.Width;
int colorHeight = colorFrame.FrameDescription.Height;
int depthWidth = depthFrame.FrameDescription.Width;
int depthHeight = depthFrame.FrameDescription.Height;
int bodyIndexWidth = bodyIndexFrame.FrameDescription.Width;
int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;
if (Bitmap == null)
{
InitBuffers(colorFrame.FrameDescription, depthFrame.FrameDescription, bodyIndexFrame.FrameDescription);
}
if (((depthWidth * depthHeight) == _depthData.Length) && ((colorWidth * colorHeight * Constants.BYTES_PER_PIXEL) == Pixels.Length) && ((bodyIndexWidth * bodyIndexHeight) == _bodyData.Length))
{
depthFrame.CopyFrameDataToArray(_depthData);
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(Pixels);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(Pixels, ColorImageFormat.Bgra);
}
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
CoordinateMapper.MapColorFrameToDepthSpace(_depthData, _depthPoints);
// Loop over each row and column of the color image
// Zero out any pixels that don't correspond to a body index
for (int i = 0, ci = 0; i < _depthPoints.Length; ++i, ci += Constants.BYTES_PER_PIXEL)
{
float colorToDepthX = _depthPoints[i].X;
float colorToDepthY = _depthPoints[i].Y;
// The sentinel value is -inf, -inf, meaning that no depth pixel corresponds to this color pixel.
if (!float.IsNegativeInfinity(colorToDepthX) &&
!float.IsNegativeInfinity(colorToDepthY))
{
// Make sure the depth pixel maps to a valid point in color space
int depthX = (int)(colorToDepthX + 0.5f);
int depthY = (int)(colorToDepthY + 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 (_bodyData[depthIndex] != 0xff)
{
continue;
}
}
}
for (int b = 0; b < Constants.BYTES_PER_PIXEL; ++b)
{
Pixels[ci + b] = 0;
}
}
UpdateBitmap();
}
}
public ColorPointCloud(ColorFrame iColorFrame, DepthFrame iDepthFrame, BodyIndexFrame iBodyIndexFrame, CoordinateMapper iCoordinateMapper)
{
TimeStamp = DateTime.Now;
Width = iDepthFrame.FrameDescription.Width;
Height = iDepthFrame.FrameDescription.Height;
var depthData = new ushort[Width * Height];
//BodyData = new int[Width * Height];
iDepthFrame.CopyFrameDataToArray(depthData);
var bodyData = new byte[Width * Height];
iBodyIndexFrame.CopyFrameDataToArray(bodyData);
var colorWidth = iColorFrame.FrameDescription.Width;
var colorHeight = iColorFrame.FrameDescription.Height;
var colorData = new byte[colorWidth * colorHeight * KinectConstants.BYTES_PER_PIXEL];
if (iColorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
iColorFrame.CopyRawFrameDataToArray(colorData);
}
else
{
iColorFrame.CopyConvertedFrameDataToArray(colorData, ColorImageFormat.Bgra);
}
var colorPoints = new ColorSpacePoint[Width * Height];
iCoordinateMapper.MapDepthFrameToColorSpace(depthData, colorPoints);
var displayPixels = new byte[Width * Height * KinectConstants.BYTES_PER_PIXEL];
Array.Clear(displayPixels, 0, displayPixels.Length);
var bodyDataBool = new bool[Width * Height];
var colorDataWithoutAlpha = new byte[Width * Height * 3];
for (var y = 0; y < Height; ++y)
{
for (var x = 0; x < Width; ++x)
{
var depthIndex = (y * Width) + x;
var player = bodyData[depthIndex];
if (player != 0xff)
{
bodyDataBool[depthIndex] = true;
var colorPoint = colorPoints[depthIndex];
var colorX = (int)Math.Floor(colorPoint.X + 0.5);
var colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
{
var colorIndex = ((colorY * colorWidth) + colorX) * KinectConstants.BYTES_PER_PIXEL;
var displayIndex = depthIndex * KinectConstants.BYTES_PER_PIXEL;
displayPixels[displayIndex + 0] = colorData[colorIndex]; // B
displayPixels[displayIndex + 1] = colorData[colorIndex + 1]; // G
displayPixels[displayIndex + 2] = colorData[colorIndex + 2]; // R
displayPixels[displayIndex + 3] = 0xff; // Alpha
var colorDataIndex = depthIndex * 3;
colorDataWithoutAlpha[colorDataIndex + 0] = colorData[colorIndex + 0];
colorDataWithoutAlpha[colorDataIndex + 1] = colorData[colorIndex + 1];
colorDataWithoutAlpha[colorDataIndex + 2] = colorData[colorIndex + 2];
}
}
}
}
DisplayPixels = displayPixels;
BodyData = bodyDataBool;
DepthData = depthData;
ColorData = colorDataWithoutAlpha;
}
/// <summary>
/// Converts a depth frame to the corresponding System.Windows.Media.Imaging.BitmapSource and removes the background (green-screen effect).
/// </summary>
/// <param name="depthFrame">The specified depth frame.</param>
/// <param name="colorFrame">The specified color frame.</param>
/// <param name="bodyIndexFrame">The specified body index frame.</param>
/// <returns>The corresponding System.Windows.Media.Imaging.BitmapSource representation of image.</returns>
public Bitmap GreenScreen(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
int colorWidth = colorFrame.FrameDescription.Width;
int colorHeight = colorFrame.FrameDescription.Height;
int depthWidth = depthFrame.FrameDescription.Width;
int depthHeight = depthFrame.FrameDescription.Height;
int bodyIndexWidth = bodyIndexFrame.FrameDescription.Width;
int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;
if (_displayPixels == null)
{
_depthData = new ushort[depthWidth * depthHeight];
_bodyData = new byte[depthWidth * depthHeight];
_colorData = new byte[colorWidth * colorHeight * Constants.BYTES_PER_PIXEL];
_displayPixels = new byte[depthWidth * depthHeight * Constants.BYTES_PER_PIXEL];
_colorPoints = new ColorSpacePoint[depthWidth * depthHeight];
_bitmap = new Bitmap(depthWidth, depthHeight, Constants.FORMAT);
}
if (((depthWidth * depthHeight) == _depthData.Length) && ((colorWidth * colorHeight * Constants.BYTES_PER_PIXEL) == _colorData.Length) && ((bodyIndexWidth * bodyIndexHeight) == _bodyData.Length))
{
depthFrame.CopyFrameDataToArray(_depthData);
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(_colorData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Bgra);
}
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
CoordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);
Array.Clear(_displayPixels, 0, _displayPixels.Length);
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
int depthIndex = (y * depthWidth) + x;
byte player = _bodyData[depthIndex];
if (player != 0xff)
{
ColorSpacePoint colorPoint = _colorPoints[depthIndex];
int colorX = (int)Math.Floor(colorPoint.X + 0.5);
int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
{
int colorIndex = ((colorY * colorWidth) + colorX) * Constants.BYTES_PER_PIXEL;
int displayIndex = depthIndex * Constants.BYTES_PER_PIXEL;
_displayPixels[displayIndex + 0] = _colorData[colorIndex];
_displayPixels[displayIndex + 1] = _colorData[colorIndex + 1];
_displayPixels[displayIndex + 2] = _colorData[colorIndex + 2];
_displayPixels[displayIndex + 3] = 0xff;
}
}
}
}
BitmapData bitmapData = _bitmap.LockBits(new Rectangle(0, 0, depthWidth, depthHeight), ImageLockMode.ReadWrite, _bitmap.PixelFormat);
Marshal.Copy(_displayPixels, 0, bitmapData.Scan0, _displayPixels.Length);
_bitmap.UnlockBits(bitmapData);
}
return _bitmap;
}
/// <summary>
/// Converts a depth frame to the corresponding System.Windows.Media.Imaging.BitmapSource and removes the background (green-screen effect).
/// </summary>
/// <param name="depthFrame">The specified depth frame.</param>
/// <param name="colorFrame">The specified color frame.</param>
/// <param name="bodyIndexFrame">The specified body index frame.</param>
/// <returns>The corresponding System.Windows.Media.Imaging.BitmapSource representation of image.</returns>
public BitmapSource GreenScreen(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
int colorWidth = colorFrame.FrameDescription.Width;
int colorHeight = colorFrame.FrameDescription.Height;
int depthWidth = depthFrame.FrameDescription.Width;
int depthHeight = depthFrame.FrameDescription.Height;
int bodyIndexWidth = bodyIndexFrame.FrameDescription.Width;
int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;
if (_displayPixels == null)
{
_depthData = new ushort[depthWidth * depthHeight];
_bodyData = new byte[depthWidth * depthHeight];
_colorData = new byte[colorWidth * colorHeight * BYTES_PER_PIXEL];
_displayPixels = new byte[depthWidth * depthHeight * BYTES_PER_PIXEL];
_colorPoints = new ColorSpacePoint[depthWidth * depthHeight];
_bitmap = new WriteableBitmap(depthWidth, depthHeight, DPI, DPI, FORMAT, null);
}
if (((depthWidth * depthHeight) == _depthData.Length) && ((colorWidth * colorHeight * BYTES_PER_PIXEL) == _colorData.Length) && ((bodyIndexWidth * bodyIndexHeight) == _bodyData.Length))
{
depthFrame.CopyFrameDataToArray(_depthData);
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(_colorData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Bgra);
}
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
_coordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);
Array.Clear(_displayPixels, 0, _displayPixels.Length);
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
int depthIndex = (y * depthWidth) + x;
byte player = _bodyData[depthIndex];
if (player != 0xff)
{
ColorSpacePoint colorPoint = _colorPoints[depthIndex];
int colorX = (int)Math.Floor(colorPoint.X + 0.5);
int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
{
int colorIndex = ((colorY * colorWidth) + colorX) * BYTES_PER_PIXEL;
int displayIndex = depthIndex * BYTES_PER_PIXEL;
_displayPixels[displayIndex + 0] = 255;
_displayPixels[displayIndex + 1] = 255;
_displayPixels[displayIndex + 2] = 255;
_displayPixels[displayIndex + 3] = 127;
// 79 195 247
}
}
}
}
_bitmap.Lock();
Marshal.Copy(_displayPixels, 0, _bitmap.BackBuffer, _displayPixels.Length);
_bitmap.AddDirtyRect(new Int32Rect(0, 0, depthWidth, depthHeight));
_bitmap.Unlock();
}
return _bitmap;
}
private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
int depthWidth = 0;
int depthHeight = 0;
int colorWidth = 0;
int colorHeight = 0;
int bodyIndexWidth = 0;
int bodyIndexHeight = 0;
bool multiSourceFrameProcessed = false;
bool colorFrameProcessed = false;
bool depthFrameProcessed = false;
bool bodyIndexFrameProcessed = false;
MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();
if (multiSourceFrame != null)
{
// Frame Acquisition should always occur first when using multiSourceFrameReader
using (DepthFrame depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame())
{
using (ColorFrame colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame())
{
using (BodyIndexFrame bodyIndexFrame = multiSourceFrame.BodyIndexFrameReference.AcquireFrame())
{
if (depthFrame != null)
{
FrameDescription depthFrameDescription = depthFrame.FrameDescription;
depthWidth = depthFrameDescription.Width;
depthHeight = depthFrameDescription.Height;
if ((depthWidth * depthHeight) == depthFrameData.Length)
{
depthFrame.CopyFrameDataToArray(depthFrameData);
depthFrameProcessed = true;
}
}
if (colorFrame != null)
{
FrameDescription colorFrameDescription = colorFrame.FrameDescription;
colorWidth = colorFrameDescription.Width;
colorHeight = colorFrameDescription.Height;
if ((colorWidth * colorHeight * bytesPerPixel) == colorFrameData.Length)
{
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(colorFrameData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(colorFrameData, ColorImageFormat.Bgra);
}
colorFrameProcessed = true;
}
}
if (bodyIndexFrame != null)
{
FrameDescription bodyIndexFrameDescription = bodyIndexFrame.FrameDescription;
bodyIndexWidth = bodyIndexFrameDescription.Width;
bodyIndexHeight = bodyIndexFrameDescription.Height;
if ((bodyIndexWidth * bodyIndexHeight) == bodyIndexFrameData.Length)
{
bodyIndexFrame.CopyFrameDataToArray(bodyIndexFrameData);
bodyIndexFrameProcessed = true;
}
}
multiSourceFrameProcessed = true;
}
}
}
}
// we got all frames
if (multiSourceFrameProcessed && depthFrameProcessed && colorFrameProcessed && bodyIndexFrameProcessed)
{
coordinateMapper.MapDepthFrameToColorSpace(depthFrameData, colorPoints);
Array.Clear(displayPixels, 0, displayPixels.Length);
// loop over each row and column of the depth
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
// calculate index into depth array
int depthIndex = (y * depthWidth) + x;
byte player = bodyIndexFrameData[depthIndex];
// if we're tracking a player for the current pixel, sets its color and alpha to full
if (player != 0xff)
{
// retrieve the depth to color mapping for the current depth pixel
ColorSpacePoint colorPoint = colorPoints[depthIndex];
// make sure the depth pixel maps to a valid point in color space
int colorX = (int)Math.Floor(colorPoint.X + 0.5);
int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
{
// calculate index into color array
int colorIndex = ((colorY * colorWidth) + colorX) * bytesPerPixel;
// set source for copy to the color pixel
int displayIndex = depthIndex * bytesPerPixel;
// write out blue byte
this.displayPixels[displayIndex++] = colorFrameData[colorIndex++];
// write out green byte
this.displayPixels[displayIndex++] = colorFrameData[colorIndex++];
// write out red byte
this.displayPixels[displayIndex++] = colorFrameData[colorIndex];
// write out alpha byte
this.displayPixels[displayIndex] = 0xff;
}
}
}
}
RenderColorPixels();
}
}
private void ProcessFrames(DepthFrame depthFrame, ColorFrame colorFrame, BodyIndexFrame bodyIndexFrame, BodyFrame bodyFrame)
{
FrameDescription depthFrameDescription = depthFrame.FrameDescription;
FrameDescription colorFrameDescription = colorFrame.FrameDescription;
FrameDescription bodyIndexFrameDescription = bodyIndexFrame.FrameDescription;
int bodyIndexWidth = bodyIndexFrameDescription.Width;
int bodyIndexHeight = bodyIndexFrameDescription.Height;
// The ImageModel object is used to transfer Kinect data into the DataFlow rotunies.
ImageModel imageModel = new ImageModel()
{
DepthWidth = depthFrameDescription.Width,
DepthHeight = depthFrameDescription.Height,
ColorWidth = colorFrameDescription.Width,
ColorHeight = colorFrameDescription.Height,
ShowTrails = _vm.LeaveTrails,
PersonFill = _vm.PersonFill,
MaxDistance = _vm.BackgroundDistance
};
imageModel.ColorFrameData = new byte[imageModel.ColorWidth * imageModel.ColorHeight * this.bytesPerPixel];
imageModel.DisplayPixels = new byte[_PreviousFrameDisplayPixels.Length];
imageModel.BodyIndexFrameData = new byte[imageModel.DepthWidth * imageModel.DepthHeight];
imageModel.ColorPoints = new ColorSpacePoint[imageModel.DepthWidth * imageModel.DepthHeight];
imageModel.BytesPerPixel = bytesPerPixel;
imageModel.Bodies = new Body[this.kinectSensor.BodyFrameSource.BodyCount];
bodyFrame.GetAndRefreshBodyData(imageModel.Bodies);
imageModel.DepthData = new ushort[imageModel.DepthWidth * imageModel.DepthHeight];
depthFrame.CopyFrameDataToArray(imageModel.DepthData);
depthFrame.CopyFrameDataToArray(this.DepthFrameData);
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(imageModel.ColorFrameData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(imageModel.ColorFrameData, ColorImageFormat.Bgra);
}
imageModel.PixelFormat = PixelFormats.Bgra32;
_ColorBitmap.WritePixels(new Int32Rect(0, 0, imageModel.ColorWidth, imageModel.ColorHeight),
imageModel.ColorFrameData,
imageModel.ColorWidth * imageModel.BytesPerPixel,
0);
//RenderTargetBitmap renderBitmap = new RenderTargetBitmap((int)CompositeImage.ActualWidth, (int)CompositeImage.ActualHeight, 96.0, 96.0, PixelFormats.Pbgra32);
//DrawingVisual dv = new DrawingVisual();
//VisualBrush brush = new VisualBrush(CompositeImage);
//foreach(Body body in _bodies)
//{
// if (body.IsTracked)
// {
// Joint joint = body.Joints[JointType.HandRight];
// using (DrawingContext dc = dv.RenderOpen())
// {
// dc.DrawRectangle(brush, null, new Rect(new Point(), new Size(CompositeImage.ActualWidth, CompositeImage.ActualHeight)));
// ImageBrush brush2 = new ImageBrush(_pointerBitmap);
// brush2.Opacity = 1.0;
// dc.DrawRectangle(brush2, null, new Rect(new Point(0, CompositeImage.ActualHeight - _Overlay.Height), new Size(_pointerBitmap.Width, _pointerBitmap.Height)));
// }
// }
//}
//ConvertIRDataToByte();
ImagePreview.Source = _ColorBitmap;
bodyIndexFrame.CopyFrameDataToArray(imageModel.BodyIndexFrameData);
this.coordinateMapper.MapDepthFrameToColorSpace(DepthFrameData, imageModel.ColorPoints);
if (_vm.LeaveTrails)
{
Array.Copy(this._PreviousFrameDisplayPixels, imageModel.DisplayPixels, this._PreviousFrameDisplayPixels.Length);
}
try
{
//Send the imageModel to the DataFlow transformer
_ImageTransformer.Post(imageModel);
}
catch (Exception ex)
{
#if DEBUG
Console.WriteLine(ex);
#endif
}
}
/// <summary>
/// Updates the bitmap with new frame data.
/// </summary>
/// <param name="depthFrame">The specified depth frame.</param>
/// <param name="colorFrame">The specified color frame.</param>
/// <param name="bodyIndexFrame">The specified body index frame.</param>
public void Update(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
int colorWidth = colorFrame.FrameDescription.Width;
int colorHeight = colorFrame.FrameDescription.Height;
int depthWidth = depthFrame.FrameDescription.Width;
int depthHeight = depthFrame.FrameDescription.Height;
int bodyIndexWidth = bodyIndexFrame.FrameDescription.Width;
int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;
if (_displayPixels == null)
{
_depthData = new ushort[depthWidth * depthHeight];
_bodyData = new byte[depthWidth * depthHeight];
_colorData = new byte[colorWidth * colorHeight * Constants.BYTES_PER_PIXEL];
_displayPixels = new byte[depthWidth * depthHeight * Constants.BYTES_PER_PIXEL];
_colorPoints = new ColorSpacePoint[depthWidth * depthHeight];
Bitmap = new WriteableBitmap(depthWidth, depthHeight, Constants.DPI, Constants.DPI, Constants.FORMAT, null);
}
if (((depthWidth * depthHeight) == _depthData.Length) && ((colorWidth * colorHeight * Constants.BYTES_PER_PIXEL) == _colorData.Length) && ((bodyIndexWidth * bodyIndexHeight) == _bodyData.Length))
{
depthFrame.CopyFrameDataToArray(_depthData);
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(_colorData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Bgra);
}
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
CoordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);
Array.Clear(_displayPixels, 0, _displayPixels.Length);
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
int depthIndex = (y * depthWidth) + x;
byte player = _bodyData[depthIndex];
if (player != 0xff)
{
ColorSpacePoint colorPoint = _colorPoints[depthIndex];
int colorX = (int)Math.Floor(colorPoint.X + 0.5);
int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
{
int colorIndex = ((colorY * colorWidth) + colorX) * Constants.BYTES_PER_PIXEL;
int displayIndex = depthIndex * Constants.BYTES_PER_PIXEL;
_displayPixels[displayIndex + 0] = _colorData[colorIndex];
_displayPixels[displayIndex + 1] = _colorData[colorIndex + 1];
_displayPixels[displayIndex + 2] = _colorData[colorIndex + 2];
_displayPixels[displayIndex + 3] = 0xff;
}
}
}
}
Bitmap.Lock();
Marshal.Copy(Pixels, 0, Bitmap.BackBuffer, _displayPixels.Length);
Bitmap.AddDirtyRect(new Int32Rect(0, 0, Width, Height));
Bitmap.Unlock();
}
}
public BitmapSource GreenScreenCustom(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
//
ushort[] _depthData = null;
byte[] _bodyData = null;
byte[] _colorData = null;
byte[] _displayPixels = null;
ColorSpacePoint[] _colorPoints = null;
CoordinateMapper _coordinateMapper = _sensor.CoordinateMapper;
int BYTES_PER_PIXEL = (PixelFormats.Bgr32.BitsPerPixel + 7) / 8;
PixelFormat FORMAT = PixelFormats.Bgra32;
double DPI = 96.0;
WriteableBitmap _bitmap = null;
//
int colorWidth = colorFrame.FrameDescription.Width;
int colorHeight = colorFrame.FrameDescription.Height;
int depthWidth = depthFrame.FrameDescription.Width;
int depthHeight = depthFrame.FrameDescription.Height;
int bodyIndexWidth = bodyIndexFrame.FrameDescription.Width;
int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;
if (_displayPixels == null)
{
_depthData = new ushort[depthWidth * depthHeight];
_bodyData = new byte[depthWidth * depthHeight];
_colorData = new byte[colorWidth * colorHeight * BYTES_PER_PIXEL];
_displayPixels = new byte[depthWidth * depthHeight * BYTES_PER_PIXEL];
_colorPoints = new ColorSpacePoint[depthWidth * depthHeight];
_bitmap = new WriteableBitmap(depthWidth, depthHeight, DPI, DPI, FORMAT, null);
}
if (((depthWidth * depthHeight) == _depthData.Length) && ((colorWidth * colorHeight * BYTES_PER_PIXEL) == _colorData.Length) && ((bodyIndexWidth * bodyIndexHeight) == _bodyData.Length))
{
depthFrame.CopyFrameDataToArray(_depthData);
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(_colorData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Bgra);
}
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
_coordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);
Array.Clear(_displayPixels, 0, _displayPixels.Length);
Array.Clear(rarray_final, 0, rarray_final.Length);
Array.Clear(garray_final, 0, rarray_final.Length);
Array.Clear(barray_final, 0, rarray_final.Length);
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
int depthIndex = (y * depthWidth) + x;
byte player = _bodyData[depthIndex];
if (player != 0xff)
{
ColorSpacePoint colorPoint = _colorPoints[depthIndex];
int colorX = (int)Math.Floor(colorPoint.X + 0.5);
int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
{
int colorIndex = ((colorY * colorWidth) + colorX) * BYTES_PER_PIXEL;
int displayIndex = depthIndex * BYTES_PER_PIXEL;
_displayPixels[displayIndex + 0] = _colorData[colorIndex];
_displayPixels[displayIndex + 1] = _colorData[colorIndex + 1];
_displayPixels[displayIndex + 2] = _colorData[colorIndex + 2];
_displayPixels[displayIndex + 3] = 0xff;
barray_final[y, x] = _colorData[colorIndex];
garray_final[y, x] = _colorData[colorIndex + 1];
rarray_final[y, x] = _colorData[colorIndex + 2];
// For ROS
_finaldisplayPixels[displayIndex + 0] = _colorData[colorIndex + 2];
_finaldisplayPixels[displayIndex + 1] = _colorData[colorIndex + 1];
_finaldisplayPixels[displayIndex + 2] = _colorData[colorIndex];
_finaldisplayPixels[displayIndex + 3] = 0xff;
}
}
}
}
_bitmap.Lock();
Marshal.Copy(_displayPixels, 0, _bitmap.BackBuffer, _displayPixels.Length);
_bitmap.AddDirtyRect(new Int32Rect(0, 0, depthWidth, depthHeight));
_bitmap.Unlock();
}
return(_bitmap);
}
static unsafe void BodyIndexFrameReady(BodyIndexFrame frame)
{
frame.CopyFrameDataToArray(sensorBodyIndexFrameData);
}
public void GreenScreen(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame) //from https://github.com/Vangos/kinect-2-background-removal
{
var colorWidth = colorFrame.FrameDescription.Width;
var colorHeight = colorFrame.FrameDescription.Height;
var depthWidth = depthFrame.FrameDescription.Width;
var depthHeight = depthFrame.FrameDescription.Height;
var bodyIndexWidth = bodyIndexFrame.FrameDescription.Width;
var bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;
if (_displayPixels == null)
{
_depthData = new ushort[depthWidth*depthHeight];
_bodyData = new byte[depthWidth*depthHeight];
_colorData = new byte[colorWidth*colorHeight*_bytesPerPixel];
_displayPixels = new byte[depthWidth*depthHeight*_bytesPerPixel];
_colorPoints = new ColorSpacePoint[depthWidth*depthHeight];
}
if (((depthWidth*depthHeight) != _depthData.Length) ||
((colorWidth*colorHeight*_bytesPerPixel) != _colorData.Length) ||
((bodyIndexWidth*bodyIndexHeight) != _bodyData.Length)) return;
depthFrame.CopyFrameDataToArray(_depthData);
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(_colorData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Bgra);
}
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
_coordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);
Array.Clear(_displayPixels, 0, _displayPixels.Length);
for (var y = 0; y < depthHeight; ++y)
{
for (var x = 0; x < depthWidth; ++x)
{
var depthIndex = (y*depthWidth) + x;
var player = _bodyData[depthIndex];
if (player == 0xff) continue;
var colorPoint = _colorPoints[depthIndex];
var colorX = (int) Math.Floor(colorPoint.X + 0.5);
var colorY = (int) Math.Floor(colorPoint.Y + 0.5);
if ((colorX < 0) || (colorX >= colorWidth) || (colorY < 0) || (colorY >= colorHeight)) continue;
var colorIndex = ((colorY*colorWidth) + colorX)*_bytesPerPixel;
var displayIndex = depthIndex*_bytesPerPixel;
_displayPixels[displayIndex + 0] = _colorData[colorIndex];
_displayPixels[displayIndex + 1] = _colorData[colorIndex + 1];
_displayPixels[displayIndex + 2] = _colorData[colorIndex + 2];
_displayPixels[displayIndex + 3] = 0xff;
}
}
Array.Clear(BufBytes, 0, BufBytes.Length); //Zerofill array
for (var d = 0; d < _displayPixels.Count(); d++)
{
if (_displayPixels[d] != 0)
{
BufBytes[d] = MainWindow.InfraPixels[d];
}
}
}
/// <summary>
/// Updates the bitmap with new frame data.
/// </summary>
/// <param name="depthFrame">The specified depth frame.</param>
/// <param name="colorFrame">The specified color frame.</param>
/// <param name="bodyIndexFrame">The specified body index frame.</param>
override public void Update(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
int colorWidth = colorFrame.FrameDescription.Width;
int colorHeight = colorFrame.FrameDescription.Height;
int depthWidth = depthFrame.FrameDescription.Width;
int depthHeight = depthFrame.FrameDescription.Height;
int bodyIndexWidth = bodyIndexFrame.FrameDescription.Width;
int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;
if (Bitmap == null)
{
InitBuffers(colorFrame.FrameDescription, depthFrame.FrameDescription, bodyIndexFrame.FrameDescription);
}
if (((depthWidth * depthHeight) == _depthData.Length) && ((colorWidth * colorHeight * Constants.BYTES_PER_PIXEL) == _colorData.Length) && ((bodyIndexWidth * bodyIndexHeight) == _bodyData.Length))
{
depthFrame.CopyFrameDataToArray(_depthData);
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(_colorData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Bgra);
}
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
CoordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);
Array.Clear(Pixels, 0, Pixels.Length);
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
int depthIndex = (y * depthWidth) + x;
if (_bodyData[depthIndex] != 0xff)
{
ColorSpacePoint colorPoint = _colorPoints[depthIndex];
int colorX = (int)(colorPoint.X + 0.5);
int colorY = (int)(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
{
int colorIndex = ((colorY * colorWidth) + colorX) * Constants.BYTES_PER_PIXEL;
int displayIndex = depthIndex * Constants.BYTES_PER_PIXEL;
for (int b = 0; b < Constants.BYTES_PER_PIXEL; ++b)
{
Pixels[displayIndex + b] = _colorData[colorIndex + b];
}
}
}
}
}
UpdateBitmap();
}
}
/// <summary>
/// Converts a depth frame to the corresponding System.Windows.Media.Imaging.BitmapSource with the players highlighted.
/// </summary>
/// <param name="depthFrame">The specified depth frame.</param>
/// <param name="bodyIndexFrame">The specified body index frame.</param>
/// <returns>The corresponding System.Windows.Media.Imaging.BitmapSource representation of the depth frame.</returns>
public static BitmapSource ToBitmap(this DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
ushort minDepth = depthFrame.DepthMinReliableDistance;
ushort maxDepth = depthFrame.DepthMaxReliableDistance;
if (_bodyData == null)
{
_width = depthFrame.FrameDescription.Width;
_height = depthFrame.FrameDescription.Height;
_depthData = new ushort[_width * _height];
_bodyData = new byte[_width * _height];
_pixels = new byte[_width * _height * Constants.BYTES_PER_PIXEL];
_bitmap = new WriteableBitmap(_width, _height, Constants.DPI, Constants.DPI, Constants.FORMAT, null);
}
depthFrame.CopyFrameDataToArray(_depthData);
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
// Convert the depth to RGB
for (int depthIndex = 0, colorPixelIndex = 0; depthIndex < _depthData.Length && colorPixelIndex < _pixels.Length; depthIndex++, colorPixelIndex += 4)
{
// Get the depth for this pixel
ushort depth = _depthData[depthIndex];
byte player = _bodyData[depthIndex];
// To convert to a byte, we're discarding the most-significant
// rather than least-significant bits.
// We're preserving detail, although the intensity will "wrap."
// Values outside the reliable depth range are mapped to 0 (black).
byte intensity = (byte)(depth >= minDepth && depth <= maxDepth ? depth : 0);
if (player != 0xff)
{
// Color player gold.
_pixels[colorPixelIndex + 0] = Colors.Gold.B; // B
_pixels[colorPixelIndex + 1] = Colors.Gold.G; // G
_pixels[colorPixelIndex + 2] = Colors.Gold.R; // R
}
else
{
// Color the rest of the image in grayscale.
_pixels[colorPixelIndex + 0] = intensity; // B
_pixels[colorPixelIndex + 1] = intensity; // G
_pixels[colorPixelIndex + 2] = intensity; // R
}
}
_bitmap.Lock();
Marshal.Copy(_pixels, 0, _bitmap.BackBuffer, _pixels.Length);
_bitmap.AddDirtyRect(new Int32Rect(0, 0, _width, _height));
_bitmap.Unlock();
return _bitmap;
}