public async void Update(DepthFrame frame)
{
if (frame != null)
{
frame.CopyFrameDataToArray(_data);
await UpdateAsync(_data, frame.DepthMinReliableDistance, frame.DepthMaxReliableDistance);
}
}
private unsafe void ProcessDepthFrameData(KinectBuffer buffer, DepthFrame depthFrame)
{
ushort* frameData = (ushort*)buffer.UnderlyingBuffer;
ushort minDepth = (ushort)(depthFrame.DepthMinReliableDistance * DEPTH_SCALE);
ushort maxDepth = (ushort)(depthFrame.DepthMaxReliableDistance * DEPTH_SCALE);
int maxCounter = (int)(buffer.Size / depthFrame.FrameDescription.BytesPerPixel);
for (int i = 0; i < maxCounter; ++i)
{
ushort depth = frameData[i];
depth *= DEPTH_SCALE;
depthPixels[i] = (ushort)(depth >= minDepth && depth <= maxDepth ? depth : 0);
}
}
public RecordDepthFrame(DepthFrame frame)
{
this.Codec = Codecs.RawColor;
this.FrameType = FrameTypes.Depth;
this.RelativeTime = frame.RelativeTime;
this.DepthMinReliableDistance = frame.DepthMinReliableDistance;
this.DepthMaxReliableDistance = frame.DepthMaxReliableDistance;
this.Width = frame.FrameDescription.Width;
this.Height = frame.FrameDescription.Height;
this.BytesPerPixel = frame.FrameDescription.BytesPerPixel;
_frameData = new ushort[this.Width * this.Height];
frame.CopyFrameDataToArray(_frameData);
}
static ROS_CS.sensor_msgs.Image GetDepthImageFromRaw(DepthFrame new_depth_frame)
{
ROS_CS.sensor_msgs.Image depth_image = new ROS_CS.sensor_msgs.Image();
depth_image.header.frame_id = "kinect2_depth_optical_frame";
depth_image.header.stamp = KinectTimestampsToROS(new_depth_frame.RelativeTime);
depth_image.is_bigendian = 0;
depth_image.height = (uint)new_depth_frame.FrameDescription.Height;
depth_image.width = (uint)new_depth_frame.FrameDescription.Width;
depth_image.step = (uint)new_depth_frame.FrameDescription.Width * 2;
depth_image.encoding = "mono16";
ushort[] depth_data = new ushort[new_depth_frame.FrameDescription.Height * new_depth_frame.FrameDescription.Width];
new_depth_frame.CopyFrameDataToArray(depth_data);
foreach (ushort depth in depth_data)
{
ushort cleaned_depth = (ushort)(depth >> 3);
byte high_byte = (byte)((cleaned_depth & 0xFF00) >> 8);
byte low_byte = (byte)(cleaned_depth & 0x00FF);
depth_image.data.Add(high_byte);
depth_image.data.Add(low_byte);
}
return depth_image;
}
private ImageSource ConverDepthToImage( DepthFrame depthFrame )
{
int BytePerPixel = 4;
var desc = depthFrame.FrameDescription;
var depth = new ushort[desc.Width * desc.Height];
depthFrame.CopyFrameDataToArray( depth );
var pixel = new byte[desc.Width * desc.Height * BytePerPixel];
for ( int i = 0; i < depth.Length; i++ ) {
int index = i * BytePerPixel;
var gray = (depth[i] * 255) / 4500;
pixel[index + 0] = (byte)gray;
pixel[index + 1] = (byte)gray;
pixel[index + 2] = (byte)gray;
}
return BitmapSource.Create( desc.Width, desc.Height, 96, 96,
PixelFormats.Bgr32, null, pixel, desc.Width * BytePerPixel );
}
/// <summary>
/// Converts the specified depth and infrared frames to a bitmap image with the players highlighted.
/// </summary>
/// <param name="depthFrame">The specified <see cref="DepthFrame"/>.</param>
/// <param name="bodyIndexFrame">The specified <see cref="BodyIndexFrame"/>.</param>
/// <returns>The bitmap representation of the current frame.</returns>
public static WriteableBitmap ToBitmap(this DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
_depthBitmapGenerator.Update(depthFrame, bodyIndexFrame);
return(_depthBitmapGenerator.HighlightedBitmap);
}
public bool GetMultiSourceFrame(KinectInterop.SensorData sensorData)
{
if (multiSourceFrameReader != null)
{
multiSourceFrame = multiSourceFrameReader.AcquireLatestFrame();
if (multiSourceFrame != null)
{
// try to get all frames at once
msBodyFrame = (sensorFlags & KinectInterop.FrameSource.TypeBody) != 0 ? multiSourceFrame.BodyFrameReference.AcquireFrame() : null;
msBodyIndexFrame = (sensorFlags & KinectInterop.FrameSource.TypeBodyIndex) != 0 ? multiSourceFrame.BodyIndexFrameReference.AcquireFrame() : null;
bool bAllSet =
((sensorFlags & KinectInterop.FrameSource.TypeBody) == 0 || msBodyFrame != null) &&
((sensorFlags & KinectInterop.FrameSource.TypeBodyIndex) == 0 || msBodyIndexFrame != null);
if (!bAllSet)
{
// release all frames
if (msBodyFrame != null)
{
msBodyFrame.Dispose();
msBodyFrame = null;
}
if (msBodyIndexFrame != null)
{
msBodyIndexFrame.Dispose();
msBodyIndexFrame = null;
}
if (msColorFrame != null)
{
msColorFrame.Dispose();
msColorFrame = null;
}
if (msDepthFrame != null)
{
msDepthFrame.Dispose();
msDepthFrame = null;
}
if (msInfraredFrame != null)
{
msInfraredFrame.Dispose();
msInfraredFrame = null;
}
}
// else
// {
// bool bNeedBody = (sensorFlags & KinectInterop.FrameSource.TypeBody) != 0;
// bool bNeedBodyIndex = (sensorFlags & KinectInterop.FrameSource.TypeBodyIndex) != 0;
// bool bNeedColor = (sensorFlags & KinectInterop.FrameSource.TypeColor) != 0;
// bool bNeedDepth = (sensorFlags & KinectInterop.FrameSource.TypeDepth) != 0;
// bool bNeedInfrared = (sensorFlags & KinectInterop.FrameSource.TypeInfrared) != 0;
//
// bAllSet = true;
// }
}
return(multiSourceFrame != null);
}
return(false);
}
/// <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();
}
}
private void StartDepthStream() {
// Get frame description for the infr output
var description = Sensor.DepthFrameSource.FrameDescription;
// Init infr buffer
DepthFrame frame = Depth = new DepthFrame();
frame.Width = description.Width;
frame.Height = description.Height;
frame.Pixels = new ushort[description.LengthInPixels];
frame.Stamp = new Timestamp();
AddOnManager.GetInstance().InitFrame(Name, frame);
Log(frame.ToString());
// Start Watch
DepthWatch = new StopwatchAvg();
}
/// <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;
}
private void InitFrames(DepthImageFrame depthFrame, ColorImageFrame colorFrame, SkeletonFrame skeletonFrame) {
if (init) { return; } init = true;
// Color Frame
Color = new ColorFrame();
Color.Width = colorFrame.Width;
Color.Height = colorFrame.Height;
Color.Pixels = new byte[colorFrame.PixelDataLength];
Color.Stamp = new Timestamp();
Color.Fps = FPS;
AddOnManager.GetInstance().InitFrame(Name, Color);
Log(Color.ToString());
ColorFormat = colorFrame.Format;
// Depth Frame
Depth = new DepthFrame();
Depth.Width = depthFrame.Width;
Depth.Height = depthFrame.Height;
Depth.Pixelss = new short[depthFrame.PixelDataLength];
Depth.Stamp = new Timestamp();
AddOnManager.GetInstance().InitFrame(Name, Depth);
Log(Depth.ToString());
var dueTime = TimeSpan.FromMilliseconds(200);
var interval = TimeSpan.FromMilliseconds(ConfigManager.GetInstance().Find("kinect_v1.motion.ms", 100));
Task = new MotionTask(dueTime, interval);
Task.Device = "";
Task.AddFrame(Depth);
Task.Start();
// Skeleton Frame
Skeletons = new BodyFrame();
Skeletons.Width = colorFrame.Width;
Skeletons.Height = colorFrame.Height;
Skeletons.RawData = new Skeleton[6];
Skeletons.Bodies = new List<NBody>(6);
Skeletons.Stamp = new Timestamp();
AddOnManager.GetInstance().InitFrame(Name, Skeletons);
Log(Skeletons.ToString());
}
public void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
var reference = e.FrameReference.AcquireFrame();
// Color
colorFrame = reference.ColorFrameReference.AcquireFrame();
{
if (colorFrame != null)
{
if (_mode == CameraMode.Color)
{
camera.Source = colorFrame.ToBitmap();
}
}
}
// Depth
depthFrame = reference.DepthFrameReference.AcquireFrame();
{
if (depthFrame != null)
{
if (_mode == CameraMode.Depth)
{
cameraDepth.Source = depthFrame.ToBitmap();
}
}
}
Ellipse ellipse1 = new Ellipse
{
Fill = Brushes.Green,
Width = 30,
Height = 30
};
Ellipse ellipse2 = new Ellipse
{
Fill = Brushes.Red,
Width = 30,
Height = 30
};
Ellipse ellipse3 = new Ellipse
{
Fill = Brushes.Green,
Width = 30,
Height = 30
};
Ellipse ellipse4 = new Ellipse
{
Fill = Brushes.Red,
Width = 30,
Height = 30
};
Ellipse ellipse5 = new Ellipse
{
Fill = Brushes.Green,
Width = 30,
Height = 30
};
Ellipse ellipse6 = new Ellipse
{
Fill = Brushes.Red,
Width = 30,
Height = 30
};
Ellipse ellipse7 = new Ellipse
{
Fill = Brushes.Green,
Width = 30,
Height = 30
};
Ellipse ellipse8 = new Ellipse
{
Fill = Brushes.Red,
Width = 30,
Height = 30
};
Ellipse ellipse9 = new Ellipse
{
Fill = Brushes.Green,
Width = 30,
Height = 30
};
Ellipse ellipse10 = new Ellipse
{
Fill = Brushes.Red,
Width = 30,
Height = 30
};
Ellipse ellipse11 = new Ellipse
{
Fill = Brushes.Green,
Width = 30,
Height = 30
};
Ellipse ellipse12 = new Ellipse
{
Fill = Brushes.Red,
Width = 30,
Height = 30
};
if (finishedCountdown)
{
if (_mode == CameraMode.Color)
{
camera.Visibility = Visibility.Visible;
canvas.Visibility = Visibility.Visible;
cameraDepth.Visibility = Visibility.Hidden;
canvasDepth.Visibility = Visibility.Hidden;
finishedCountdown = false;
}
if (_mode == CameraMode.Depth)
{
camera.Visibility = Visibility.Hidden;
canvas.Visibility = Visibility.Hidden;
cameraDepth.Visibility = Visibility.Visible;
canvasDepth.Visibility = Visibility.Visible;
finishedCountdown = false;
}
}
// Body
using (var frame = reference.BodyFrameReference.AcquireFrame())
{
if (frame != null)
{
this.canvas.Children.Clear();
this.canvasDepth.Children.Clear();
_bodies = new Body[frame.BodyFrameSource.BodyCount];
frame.GetAndRefreshBodyData(_bodies);
//int bodyIndex = 0;
foreach (var body in _bodies)
{
//if (bodyIndex != 0) continue;
//bodyIndex++;
var cameraSpacePoint = new CameraSpacePoint();
DepthSpacePoint depthPointWristLeft = new DepthSpacePoint();
DepthSpacePoint depthPointWristRight = new DepthSpacePoint();
DepthSpacePoint depthPointHead = new DepthSpacePoint();
DepthSpacePoint depthPointSpineMid = new DepthSpacePoint();
DepthSpacePoint depthPointKneeLeft = new DepthSpacePoint();
DepthSpacePoint depthPointKneeRight = new DepthSpacePoint();
ColorSpacePoint colorPointWristLeft = new ColorSpacePoint();
ColorSpacePoint colorPointWristRight = new ColorSpacePoint();
ColorSpacePoint colorPointHead = new ColorSpacePoint();
ColorSpacePoint colorPointSpineMid = new ColorSpacePoint();
ColorSpacePoint colorPointKneeLeft = new ColorSpacePoint();
ColorSpacePoint colorPointKneeRight = new ColorSpacePoint();
if (body.IsTracked)
{
BodyStructure currentFrameBody = new BodyStructure();
var lines = new List<string>();
foreach (var joint in body.Joints.Values)
{
cameraSpacePoint.X = joint.Position.X;//(float)currentFrameBody.currentFrameBody[joint.JointType].X;
cameraSpacePoint.Y = joint.Position.Y; //(float)currentFrameBody.currentFrameBody[joint.JointType].Y;
cameraSpacePoint.Z = joint.Position.Z; //(float)currentFrameBody.currentFrameBody[joint.JointType].Z;
if (PressSpaced && !escape && jointsOfInterest.Contains(joint.JointType))
{
curNumOfFrames++;
distanceSum[joint.JointType] += cameraSpacePoint.Z;
meanDistanceToSensor[joint.JointType] = distanceSum[joint.JointType] / ( (curNumOfFrames/6) + 1);
}
else if(escape && movementVector != null && jointsOfInterest.Contains(joint.JointType))
{
distanceScale[joint.JointType] = cameraSpacePoint.Z / meanDistanceToSensor[joint.JointType];
}
var colorPoint = _sensor.CoordinateMapper.MapCameraPointToColorSpace(cameraSpacePoint);
var depthPoint = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(cameraSpacePoint);
if (_mode == CameraMode.Depth)
{
/* Ellipse ellipseBlue = new Ellipse
{
Fill = Brushes.Blue,
Width = 10,
Height = 10
};
var xPos = depthPoint.X - ellipseBlue.Width / 2;
var yPos = depthPoint.Y - ellipseBlue.Height / 2;
if (xPos >= 0 && xPos < this.canvasDepth.ActualWidth &&
yPos >= 0 && yPos < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipseBlue, xPos);
Canvas.SetTop(ellipseBlue, yPos);
canvasDepth.Children.Add(ellipseBlue);
} */
Extensions.DrawSkeleton(canvas, body, jointsOfInterest, _sensor);
//currentFrameBody.currentFrameBody.Add(joint.JointType, new Point3D());
if (jointsOfInterest.Contains(joint.JointType))
{
if (joint.JointType == JointType.HandLeft)
depthPointWristLeft = depthPoint;
else if (joint.JointType == JointType.HandRight)
depthPointWristRight = depthPoint;
else if (joint.JointType == JointType.Head)
depthPointHead = depthPoint;
else if (joint.JointType == JointType.SpineMid)
depthPointSpineMid = depthPoint;
else if (joint.JointType == JointType.KneeRight)
depthPointKneeRight = depthPoint;
else if (joint.JointType == JointType.KneeLeft)
depthPointKneeLeft = depthPoint;
currentFrameBody.currentFrameBody.Add(joint.JointType, new Point3D(depthPoint.X, depthPoint.Y, 0));
}
}
else
{
Ellipse ellipseBlue = new Ellipse
{
Fill = Brushes.Blue,
Width = 10,
Height = 10
};
var xPos = colorPoint.X - ellipseBlue.Width / 2;
var yPos = colorPoint.Y - ellipseBlue.Height / 2;
if (xPos >= 0 && xPos < this.canvas.ActualWidth &&
yPos >= 0 && yPos < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipseBlue, xPos);
Canvas.SetTop(ellipseBlue, yPos);
canvas.Children.Add(ellipseBlue);
}
Extensions.DrawSkeleton(canvas, body, jointsOfInterest, _sensor);
//currentFrameBody.currentFrameBody.Add(joint.JointType, new Point3D());
if (jointsOfInterest.Contains(joint.JointType))
{
if (joint.JointType == JointType.HandLeft)
colorPointWristLeft = colorPoint;
else if (joint.JointType == JointType.HandRight)
colorPointWristRight = colorPoint;
else if (joint.JointType == JointType.Head)
colorPointHead = colorPoint;
else if (joint.JointType == JointType.SpineMid)
colorPointSpineMid = colorPoint;
else if (joint.JointType == JointType.KneeRight)
colorPointKneeRight = colorPoint;
else if (joint.JointType == JointType.KneeLeft)
colorPointKneeLeft = colorPoint;
var pointCurJoint = new Point3D(colorPoint.X, colorPoint.Y, 0);
this.dictAllMovementPositions[joint.JointType].Add(pointCurJoint);
currentFrameBody.currentFrameBody.Add(joint.JointType, pointCurJoint);
}
}
}
/*TODO - null or empty Dictionary (.Count == 0)*/
// if (movementVector == null)
if (PressSpaced && !escape)
{
if (movementVector != null && jointsChange)
{
movementVector.Clear();
}
jointsChange = false;
movementVector = record.GetMovementVectors(currentFrameBody);
}
//else if(movementVector != null)
else if (escape && movementVector != null)
{
var curFrameResult = benchPress.ValidateCurrentSkeletonFrame(movementVector, currentFrameBody, distanceScale);
var resultWristLeft = curFrameResult.BoneStates[JointType.HandLeft];
var resultWristRight = curFrameResult.BoneStates[JointType.HandRight];
Extensions.DrawSkeleton(canvas, body, jointsOfInterest, _sensor);
if (recordVisualization)
{
recordVisualization = false;
Color colorHandLeft = Color.FromArgb(255, 35, 63, 147);
Color colorHandRight = Color.FromArgb(255, 0, 136, 170);
Color colorHead = Color.FromArgb(255, 193, 84, 151);
Color colorSpineMid = Color.FromArgb(255, 238, 198, 20);
Color colorKneeLeft = Color.FromArgb(255, 116, 81, 67);
Color colorKneeRight = Color.FromArgb(255, 94, 58, 106);
foreach (var joy in this.dictAllMovementPositions)
{
Color fillColor = Color.FromArgb(255, 255, 255, 255);
switch (joy.Key)
{
case JointType.HandLeft:
fillColor = colorHandLeft;
break;
case JointType.HandRight:
fillColor = colorHandRight;
break;
case JointType.Head:
fillColor = colorHead;
break;
case JointType.SpineMid:
fillColor = colorSpineMid;
break;
case JointType.KneeLeft:
fillColor = colorKneeLeft;
break;
case JointType.KneeRight:
fillColor = colorKneeRight;
break;
}
foreach (var point in joy.Value)
{
float opacityFactor = joy.Value.Count / 255 ;
if (opacityFactor == 0.0f)
opacityFactor = 1.0f;
int alpha = (int)opacityFactor * (joy.Value.IndexOf(point) + 1);
fillColor = SetTransparency(alpha, fillColor);
Brush fill = new SolidColorBrush(fillColor);
Ellipse newEllipse = new Ellipse
{
Fill = fill,
Width = 20,
Height = 20
};
var xPosRecord = point.X - newEllipse.Width / 2;
var yPosRecord = point.Y - newEllipse.Height / 2;
if (xPosRecord >= 0 && xPosRecord < this.Record.ActualWidth &&
yPosRecord >= 0 && yPosRecord < this.Record.ActualHeight)
{
Canvas.SetLeft(newEllipse, xPosRecord);
Canvas.SetTop(newEllipse, yPosRecord);
Record.Children.Add(newEllipse);
}
}
}
}
if (sixJoints.IsChecked == true)
{
var resultHead = curFrameResult.BoneStates[JointType.Head];
var resultSpineMid = curFrameResult.BoneStates[JointType.SpineMid];
var resultKneeLeft = curFrameResult.BoneStates[JointType.KneeLeft];
var resultKneeRight = curFrameResult.BoneStates[JointType.KneeRight];
if (_mode == CameraMode.Depth)
{
if (resultSpineMid == SkeletonBoneState.Matched)
{
var xPosMid = depthPointSpineMid.X - ellipse7.Width / 2;
var yPosMid = depthPointSpineMid.Y - ellipse7.Height / 2;
if (xPosMid >= 0 && xPosMid < this.canvasDepth.ActualWidth &&
yPosMid >= 0 && yPosMid < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse7, xPosMid);
Canvas.SetTop(ellipse7, yPosMid);
canvasDepth.Children.Add(ellipse7);
}
}
else
{
var xPosMid = depthPointSpineMid.X - ellipse8.Width / 2;
var yPosMid = depthPointSpineMid.Y - ellipse8.Height / 2;
if (xPosMid >= 0 && xPosMid < this.canvasDepth.ActualWidth &&
yPosMid >= 0 && yPosMid < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse8, xPosMid);
Canvas.SetTop(ellipse8, yPosMid);
canvasDepth.Children.Add(ellipse8);
}
}
if (resultKneeLeft == SkeletonBoneState.Matched)
{
var xPosKneeLeft = depthPointKneeLeft.X - ellipse9.Width / 2;
var yPosKneeLeft = depthPointKneeLeft.Y - ellipse9.Height / 2;
if (xPosKneeLeft >= 0 && xPosKneeLeft < this.canvasDepth.ActualWidth &&
yPosKneeLeft >= 0 && yPosKneeLeft < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse9, xPosKneeLeft);
Canvas.SetTop(ellipse9, yPosKneeLeft);
canvasDepth.Children.Add(ellipse9);
}
}
else
{
var xPosKneeLeft = depthPointKneeLeft.X - ellipse10.Width / 2;
var yPosKneeLeft = depthPointKneeLeft.Y - ellipse10.Height / 2;
if (xPosKneeLeft >= 0 && xPosKneeLeft < this.canvasDepth.ActualWidth &&
yPosKneeLeft >= 0 && yPosKneeLeft < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse10, xPosKneeLeft);
Canvas.SetTop(ellipse10, yPosKneeLeft);
canvasDepth.Children.Add(ellipse10);
}
}
if (resultKneeRight == SkeletonBoneState.Matched)
{
var xPosKneeRight = depthPointKneeRight.X - ellipse11.Width / 2;
var yPosKneeRight = depthPointKneeRight.Y - ellipse11.Height / 2;
if (xPosKneeRight >= 0 && xPosKneeRight < this.canvasDepth.ActualWidth &&
yPosKneeRight >= 0 && yPosKneeRight < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse11, xPosKneeRight);
Canvas.SetTop(ellipse11, yPosKneeRight);
canvasDepth.Children.Add(ellipse11);
}
}
else
{
var xPosKneeRight = depthPointKneeRight.X - ellipse12.Width / 2;
var yPosKneeRight = depthPointKneeRight.Y - ellipse12.Height / 2;
if (xPosKneeRight >= 0 && xPosKneeRight < this.canvasDepth.ActualWidth &&
yPosKneeRight >= 0 && yPosKneeRight < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse12, xPosKneeRight);
Canvas.SetTop(ellipse12, yPosKneeRight);
canvasDepth.Children.Add(ellipse12);
}
}
if (resultHead == SkeletonBoneState.Matched)
{
var xPosHead = depthPointHead.X - ellipse5.Width / 2;
var yPosHead = depthPointHead.Y - ellipse5.Height / 2;
if (xPosHead >= 0 && xPosHead < this.canvasDepth.ActualWidth &&
yPosHead >= 0 && yPosHead < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse5, xPosHead);
Canvas.SetTop(ellipse5, yPosHead);
canvasDepth.Children.Add(ellipse5);
}
}
else
{
var xPosHead = depthPointHead.X - ellipse6.Width / 2;
var yPosHead = depthPointHead.Y - ellipse6.Height / 2;
if (xPosHead >= 0 && xPosHead < this.canvasDepth.ActualWidth &&
yPosHead >= 0 && yPosHead < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse6, xPosHead);
Canvas.SetTop(ellipse6, yPosHead);
canvasDepth.Children.Add(ellipse6);
}
}
if (resultWristLeft == SkeletonBoneState.Matched)
{
var xPosLeft = depthPointWristLeft.X - ellipse1.Width / 2;
var yPosLeft = depthPointWristLeft.Y - ellipse1.Height / 2;
if (xPosLeft >= 0 && xPosLeft < this.canvasDepth.ActualWidth &&
yPosLeft >= 0 && yPosLeft < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse1, xPosLeft);
Canvas.SetTop(ellipse1, yPosLeft);
canvasDepth.Children.Add(ellipse1);
}
}
else
{
var xPosLeft = depthPointWristLeft.X - ellipse2.Width / 2;
var yPosLeft = depthPointWristLeft.Y - ellipse2.Height / 2;
if (xPosLeft >= 0 && xPosLeft < this.canvasDepth.ActualWidth &&
yPosLeft >= 0 && yPosLeft < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse2, xPosLeft);
Canvas.SetTop(ellipse2, yPosLeft);
canvasDepth.Children.Add(ellipse2);
}
}
if (resultWristRight == SkeletonBoneState.Matched)
{
var xPosRight = depthPointWristRight.X - ellipse3.Width / 2;
var yPosRight = depthPointWristRight.Y - ellipse3.Height / 2;
if (xPosRight >= 0 && xPosRight < this.canvasDepth.ActualWidth &&
yPosRight >= 0 && yPosRight < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse3, xPosRight);
Canvas.SetTop(ellipse3, yPosRight);
canvasDepth.Children.Add(ellipse3);
}
}
else
{
var xPosRight = depthPointWristRight.X - ellipse4.Width / 2;
var yPosRight = depthPointWristRight.Y - ellipse4.Height / 2;
if (xPosRight >= 0 && xPosRight < this.canvasDepth.ActualWidth &&
yPosRight >= 0 && yPosRight < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse4, xPosRight);
Canvas.SetTop(ellipse4, yPosRight);
canvasDepth.Children.Add(ellipse4);
}
}
}
else
{
if (resultSpineMid == SkeletonBoneState.Matched)
{
var xPosMid = colorPointSpineMid.X - ellipse7.Width / 2;
var yPosMid = colorPointSpineMid.Y - ellipse7.Height / 2;
if (xPosMid >= 0 && xPosMid < this.canvas.ActualWidth &&
yPosMid >= 0 && yPosMid < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse7, xPosMid);
Canvas.SetTop(ellipse7, yPosMid);
canvas.Children.Add(ellipse7);
}
}
else
{
var xPosMid = colorPointSpineMid.X - ellipse8.Width / 2;
var yPosMid = colorPointSpineMid.Y - ellipse8.Height / 2;
if (xPosMid >= 0 && xPosMid < this.canvas.ActualWidth &&
yPosMid >= 0 && yPosMid < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse8, xPosMid);
Canvas.SetTop(ellipse8, yPosMid);
canvas.Children.Add(ellipse8);
}
}
if (resultKneeLeft == SkeletonBoneState.Matched)
{
var xPosKneeLeft = colorPointKneeLeft.X - ellipse9.Width / 2;
var yPosKneeLeft = colorPointKneeLeft.Y - ellipse9.Height / 2;
if (xPosKneeLeft >= 0 && xPosKneeLeft < this.canvas.ActualWidth &&
yPosKneeLeft >= 0 && yPosKneeLeft < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse9, xPosKneeLeft);
Canvas.SetTop(ellipse9, yPosKneeLeft);
canvas.Children.Add(ellipse9);
}
}
else
{
var xPosKneeLeft = colorPointKneeLeft.X - ellipse10.Width / 2;
var yPosKneeLeft = colorPointKneeLeft.Y - ellipse10.Height / 2;
if (xPosKneeLeft >= 0 && xPosKneeLeft < this.canvas.ActualWidth &&
yPosKneeLeft >= 0 && yPosKneeLeft < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse10, xPosKneeLeft);
Canvas.SetTop(ellipse10, yPosKneeLeft);
canvas.Children.Add(ellipse10);
}
}
if (resultKneeRight == SkeletonBoneState.Matched)
{
var xPosKneeRight = colorPointKneeRight.X - ellipse11.Width / 2;
var yPosKneeRight = colorPointKneeRight.Y - ellipse11.Height / 2;
if (xPosKneeRight >= 0 && xPosKneeRight < this.canvas.ActualWidth &&
yPosKneeRight >= 0 && yPosKneeRight < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse11, xPosKneeRight);
Canvas.SetTop(ellipse11, yPosKneeRight);
canvas.Children.Add(ellipse11);
}
}
else
{
var xPosKneeRight = colorPointKneeRight.X - ellipse12.Width / 2;
var yPosKneeRight = colorPointKneeRight.Y - ellipse12.Height / 2;
if (xPosKneeRight >= 0 && xPosKneeRight < this.canvas.ActualWidth &&
yPosKneeRight >= 0 && yPosKneeRight < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse12, xPosKneeRight);
Canvas.SetTop(ellipse12, yPosKneeRight);
canvas.Children.Add(ellipse12);
}
}
if (resultHead == SkeletonBoneState.Matched)
{
var xPosHead = colorPointHead.X - ellipse5.Width / 2;
var yPosHead = colorPointHead.Y - ellipse5.Height / 2;
if (xPosHead >= 0 && xPosHead < this.canvas.ActualWidth &&
yPosHead >= 0 && yPosHead < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse5, xPosHead);
Canvas.SetTop(ellipse5, yPosHead);
canvas.Children.Add(ellipse5);
}
}
else
{
var xPosHead = colorPointHead.X - ellipse6.Width / 2;
var yPosHead = colorPointHead.Y - ellipse6.Height / 2;
if (xPosHead >= 0 && xPosHead < this.canvas.ActualWidth &&
yPosHead >= 0 && yPosHead < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse6, xPosHead);
Canvas.SetTop(ellipse6, yPosHead);
canvas.Children.Add(ellipse6);
}
}
if (resultWristLeft == SkeletonBoneState.Matched)
{
var xPosLeft = colorPointWristLeft.X - ellipse1.Width / 2;
var yPosLeft = colorPointWristLeft.Y - ellipse1.Height / 2;
if (xPosLeft >= 0 && xPosLeft < this.canvas.ActualWidth &&
yPosLeft >= 0 && yPosLeft < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse1, xPosLeft);
Canvas.SetTop(ellipse1, yPosLeft);
canvas.Children.Add(ellipse1);
}
}
else
{
var xPosLeft = colorPointWristLeft.X - ellipse2.Width / 2;
var yPosLeft = colorPointWristLeft.Y - ellipse2.Height / 2;
if (xPosLeft >= 0 && xPosLeft < this.canvas.ActualWidth &&
yPosLeft >= 0 && yPosLeft < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse2, xPosLeft);
Canvas.SetTop(ellipse2, yPosLeft);
canvas.Children.Add(ellipse2);
}
}
if (resultWristRight == SkeletonBoneState.Matched)
{
var xPosRight = colorPointWristRight.X - ellipse3.Width / 2;
var yPosRight = colorPointWristRight.Y - ellipse3.Height / 2;
if (xPosRight >= 0 && xPosRight < this.canvas.ActualWidth &&
yPosRight >= 0 && yPosRight < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse3, xPosRight);
Canvas.SetTop(ellipse3, yPosRight);
canvas.Children.Add(ellipse3);
}
}
else
{
var xPosRight = colorPointWristRight.X - ellipse4.Width / 2;
var yPosRight = colorPointWristRight.Y - ellipse4.Height / 2;
if (xPosRight >= 0 && xPosRight < this.canvas.ActualWidth &&
yPosRight >= 0 && yPosRight < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse4, xPosRight);
Canvas.SetTop(ellipse4, yPosRight);
canvas.Children.Add(ellipse4);
}
}
}
}
else
{
if (_mode == CameraMode.Depth)
{
if (resultWristLeft == SkeletonBoneState.Matched)
{
var xPosLeft = depthPointWristLeft.X - ellipse1.Width / 2;
var yPosLeft = depthPointWristLeft.Y - ellipse1.Height / 2;
if (xPosLeft >= 0 && xPosLeft < this.canvasDepth.ActualWidth &&
yPosLeft >= 0 && yPosLeft < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse1, xPosLeft);
Canvas.SetTop(ellipse1, yPosLeft);
canvasDepth.Children.Add(ellipse1);
}
}
else
{
var xPosLeft = depthPointWristLeft.X - ellipse2.Width / 2;
var yPosLeft = depthPointWristLeft.Y - ellipse2.Height / 2;
if (xPosLeft >= 0 && xPosLeft < this.canvasDepth.ActualWidth &&
yPosLeft >= 0 && yPosLeft < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse2, xPosLeft);
Canvas.SetTop(ellipse2, yPosLeft);
canvasDepth.Children.Add(ellipse2);
}
}
if (resultWristRight == SkeletonBoneState.Matched)
{
var xPosRight = depthPointWristRight.X - ellipse3.Width / 2;
var yPosRight = depthPointWristRight.Y - ellipse3.Height / 2;
if (xPosRight >= 0 && xPosRight < this.canvasDepth.ActualWidth &&
yPosRight >= 0 && yPosRight < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse3, xPosRight);
Canvas.SetTop(ellipse3, yPosRight);
canvasDepth.Children.Add(ellipse3);
}
}
else
{
var xPosRight = depthPointWristRight.X - ellipse4.Width / 2;
var yPosRight = depthPointWristRight.Y - ellipse4.Height / 2;
if (xPosRight >= 0 && xPosRight < this.canvasDepth.ActualWidth &&
yPosRight >= 0 && yPosRight < this.canvasDepth.ActualHeight)
{
Canvas.SetLeft(ellipse4, xPosRight);
Canvas.SetTop(ellipse4, yPosRight);
canvasDepth.Children.Add(ellipse4);
}
}
}
else
{
if (resultWristLeft == SkeletonBoneState.Matched)
{
var xPosLeft = colorPointWristLeft.X - ellipse1.Width / 2;
var yPosLeft = colorPointWristLeft.Y - ellipse1.Height / 2;
if (xPosLeft >= 0 && xPosLeft < this.canvas.ActualWidth &&
yPosLeft >= 0 && yPosLeft < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse1, xPosLeft);
Canvas.SetTop(ellipse1, yPosLeft);
canvas.Children.Add(ellipse1);
}
}
else
{
var xPosLeft = colorPointWristLeft.X - ellipse2.Width / 2;
var yPosLeft = colorPointWristLeft.Y - ellipse2.Height / 2;
if (xPosLeft >= 0 && xPosLeft < this.canvas.ActualWidth &&
yPosLeft >= 0 && yPosLeft < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse2, xPosLeft);
Canvas.SetTop(ellipse2, yPosLeft);
canvas.Children.Add(ellipse2);
}
}
if (resultWristRight == SkeletonBoneState.Matched)
{
var xPosRight = colorPointWristRight.X - ellipse3.Width / 2;
var yPosRight = colorPointWristRight.Y - ellipse3.Height / 2;
if (xPosRight >= 0 && xPosRight < this.canvas.ActualWidth &&
yPosRight >= 0 && yPosRight < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse3, xPosRight);
Canvas.SetTop(ellipse3, yPosRight);
canvas.Children.Add(ellipse3);
}
}
else
{
var xPosRight = colorPointWristRight.X - ellipse4.Width / 2;
var yPosRight = colorPointWristRight.Y - ellipse4.Height / 2;
if (xPosRight >= 0 && xPosRight < this.canvas.ActualWidth &&
yPosRight >= 0 && yPosRight < this.canvas.ActualHeight)
{
Canvas.SetLeft(ellipse4, xPosRight);
Canvas.SetTop(ellipse4, yPosRight);
canvas.Children.Add(ellipse4);
}
}
}
}
}
else
{
escape = false;
}
}
}
}
}
if (depthFrame != null)
depthFrame.Dispose();
depthFrame = null;
if (colorFrame != null)
colorFrame.Dispose();
colorFrame = null;
}
/// <summary>
/// Converts the specified depth frame to a bitmap and saves it to the specified location.
/// </summary>
/// <param name="source">The source depth frame.</param>
/// <param name="destination">The destination path for the new image. JPEG, PNG, GIF, BMP and TIFF formats are supported.</param>
/// <returns>True if the frame was successfully saved. False otherwise.</returns>
public static async Task <bool> Save(this DepthFrame frame, string path)
{
var bitmap = frame.ToBitmap();
return(await _capture.Save(bitmap, path));
}
/// <summary>
/// Converts the specified depth frame to a bitmap and saves it to the specified location.
/// </summary>
/// <param name="source">The source depth frame.</param>
/// <param name="destination">The destination storage file for the image. JPEG, PNG, GIF, BMP and TIFF formats are supported.</param>
/// <returns>True if the frame was successfully saved. False otherwise.</returns>
public static async Task <bool> Save(this DepthFrame frame, StorageFile destination)
{
var bitmap = frame.ToBitmap();
return(await _capture.Save(bitmap, destination));
}
public DepthImage(DepthFrame frame)
{
this.frame = frame;
Info = frame.ToImageInfo(frame.Width, frame.Height);
}
private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
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
// KinectBuffer depthData = depthFrame.LockImageBuffer();
//uint size = 515*424;
//depthFrame.CopyFrameDataToIntPtr(depthData.UnderlyingBuffer, depthData.Size);
/*StreamWriter sw = File.AppendText("depthData.txt");
* sw.Write(depthData.UnderlyingBuffer);
* sw.Close();*/
using (KinectBuffer depthFrameData = depthFrame.LockImageBuffer())
{
this.coordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
depthFrameData.UnderlyingBuffer,
depthFrameData.Size,
this.colorMappedToDepthPoints);
}
// We're done with the DepthFrame
depthFrame.Dispose();
depthFrame = null;
// Process Color
// Lock the bitmap for writing
//this.bitmap.Lock();
isBitmapLocked = true;
//colorFrame.CopyConvertedFrameDataToIntPtr(this.bitmap.BackBuffer, this.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 = this.colorMappedToDepthPoints.Length;
fixed(DepthSpacePoint *colorMappedToDepthPointsPointer = this.colorMappedToDepthPoints)
{
// Treat the color data as 4-byte pixels
//uint* bitmapPixelsPointer = (uint*)this.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;
}
}
//this.bitmap.AddDirtyRect(new Int32Rect(0, 0, this.bitmap.PixelWidth, this.bitmap.PixelHeight));
}
}
}
finally
{
if (isBitmapLocked)
{
//this.bitmap.Unlock();
}
if (depthFrame != null)
{
depthFrame.Dispose();
}
if (colorFrame != null)
{
colorFrame.Dispose();
}
if (bodyIndexFrame != null)
{
bodyIndexFrame.Dispose();
}
}
}
void Update()
{
if (reader != null)
{
MultiSourceFrame frame = reader.AcquireLatestFrame();
if (frame != null)
{
using (ColorFrame colorFrame = frame.ColorFrameReference.AcquireFrame()) {
if (colorFrame != null)
{
colorFrame.CopyConvertedFrameDataToArray(colorData, ColorImageFormat.Rgba);
}
}
using (DepthFrame depthFrame = frame.DepthFrameReference.AcquireFrame()) {
if (depthFrame != null)
{
//Debug.Log ("bodyIndexFrame not null");
depthFrame.CopyFrameDataToArray(depthData);
}
}
using (BodyIndexFrame bodyIndexFrame = frame.BodyIndexFrameReference.AcquireFrame()) {
if (bodyIndexFrame != null)
{
//Debug.Log ("bodyIndexFrame not null");
bodyIndexFrame.CopyFrameDataToArray(bodyIndexData);
}
}
frame = null;
}
}
else
{
return;
}
Utils.copyToMat(colorData, outputMat);
Utils.copyToMat(colorData, rgbaMat);
coordinateMapper.MapColorFrameToDepthSpace(depthData, depthSpacePoints);
int width = rgbaMat.width();
int height = rgbaMat.height();
int depthWidth = 512;
byte[] maskOn = new byte[] { 0 };
for (int y = 0; y < height; y++)
{
for (int x = 0; x < width; x++)
{
int index = x + y * width;
if (bodyIndexData [(int)depthSpacePoints [index].X + (int)depthSpacePoints [index].Y * depthWidth] == 255)
{
maskData [index] = 0;
}
else
{
maskData [index] = 255;
}
}
}
Utils.copyToMat(maskData, maskMat);
if (mode == modeType.original)
{
rgbaMat.copyTo(outputMat, maskMat);
Core.putText(outputMat, "ORIGINAL MODE " + texture.width + "x" + texture.height, new Point(5, texture.height - 5), Core.FONT_HERSHEY_PLAIN, 4.0, new Scalar(255, 0, 0, 255), 3);
}
else if (mode == modeType.sepia)
{
Core.transform(rgbaMat, rgbaMat, sepiaKernel);
rgbaMat.copyTo(outputMat, maskMat);
Core.putText(outputMat, "SEPIA MODE " + texture.width + "x" + texture.height, new Point(5, texture.height - 5), Core.FONT_HERSHEY_PLAIN, 4.0, new Scalar(255, 0, 0, 255), 3);
}
else if (mode == modeType.pixelize)
{
Imgproc.resize(rgbaMat, pixelizeIntermediateMat, pixelizeSize0, 0.1, 0.1, Imgproc.INTER_NEAREST);
Imgproc.resize(pixelizeIntermediateMat, rgbaMat, rgbaMat.size(), 0.0, 0.0, Imgproc.INTER_NEAREST);
rgbaMat.copyTo(outputMat, maskMat);
Core.putText(outputMat, "PIXELIZE MODE" + texture.width + "x" + texture.height, new Point(5, texture.height - 5), Core.FONT_HERSHEY_PLAIN, 4.0, new Scalar(255, 0, 0, 255), 3);
}
else if (mode == modeType.comic)
{
Imgproc.cvtColor(rgbaMat, comicGrayMat, Imgproc.COLOR_RGBA2GRAY);
comicBgMat.copyTo(comicDstMat);
Imgproc.GaussianBlur(comicGrayMat, comicLineMat, new Size(3, 3), 0);
Utils.copyFromMat(comicGrayMat, comicGrayPixels);
for (int i = 0; i < comicGrayPixels.Length; i++)
{
comicMaskPixels [i] = 0;
if (comicGrayPixels [i] < 70)
{
comicGrayPixels [i] = 0;
comicMaskPixels [i] = 1;
}
else if (70 <= comicGrayPixels [i] && comicGrayPixels [i] < 120)
{
comicGrayPixels [i] = 100;
}
else
{
comicGrayPixels [i] = 255;
comicMaskPixels [i] = 1;
}
}
Utils.copyToMat(comicGrayPixels, comicGrayMat);
Utils.copyToMat(comicMaskPixels, comicMaskMat);
comicGrayMat.copyTo(comicDstMat, comicMaskMat);
Imgproc.Canny(comicLineMat, comicLineMat, 20, 120);
comicLineMat.copyTo(comicMaskMat);
Core.bitwise_not(comicLineMat, comicLineMat);
comicLineMat.copyTo(comicDstMat, comicMaskMat);
Imgproc.cvtColor(comicDstMat, rgbaMat, Imgproc.COLOR_GRAY2RGBA);
rgbaMat.copyTo(outputMat, maskMat);
Core.putText(outputMat, "COMIC MODE " + texture.width + "x" + texture.height, new Point(5, texture.height - 5), Core.FONT_HERSHEY_PLAIN, 4.0, new Scalar(255, 0, 0, 255), 3);
}
Utils.matToTexture(outputMat, texture);
}
void DepthUpdate(DepthFrame frame)
{
depthFrame = frame;
}
/// <summary>
/// Converts the specified depth frame to a bitmap and saves it to the specified location.
/// </summary>
/// <param name="frame">The source depth frame.</param>
/// <param name="path">The destination path for the new image. JPEG, PNG, GIF, BMP and TIFF formats are supported.</param>
/// <returns>True if the frame was successfully saved. False otherwise.</returns>
public static bool Save(this DepthFrame frame, string path)
{
var bitmap = frame.ToBitmap();
return(_capture.Save(bitmap, path));
}
/// <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;
}
/// <summary>
/// Converts the specified depth and body index frames to a bitmap and saves it to the specified location.
/// </summary>
/// <param name="depthFrame">The source depth frame.</param>
/// <param name="bodyIndexFrame">The source body index frame.</param>
/// <param name="destination">The destination storage file for the image. JPEG, PNG, GIF, BMP and TIFF formats are supported.</param>
/// <param name="width">The width of the image file.</param>
/// <param name="height">The height of the image file.</param>
/// <returns>True if the image was successfully saved. False otherwise.</returns>
public static async Task <bool> Save(this DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame, StorageFile destination, int width, int height)
{
var bitmap = depthFrame.ToBitmap(bodyIndexFrame);
return(await _capture.Save(bitmap, destination, width, height));
}
private void MultiFrameSourceReader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs ex)
{
__AvatarData__ = new List <byte>();
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;
bool bodyFrameProcessed = false;
MultiSourceFrame multiSourceFrame = ex.FrameReference.AcquireFrame();
if (multiSourceFrame != null)
{
using (_frameCounter.Increment())
{
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) == this.depthFrameData.Length)
{
depthFrame.CopyFrameDataToArray(this.depthFrameData);
depthFrameProcessed = true;
}
if (colorFrame != null)
{
FrameDescription colorFrameDescription = colorFrame.FrameDescription;
colorWidth = colorFrameDescription.Width;
colorHeight = colorFrameDescription.Height;
if ((colorWidth * colorHeight * this.bytesPerPixel) == this.colorFrameData.Length)
{
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(this.colorFrameData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(this.colorFrameData, ColorImageFormat.Bgra);
}
colorFrameProcessed = true;
}
}
if (bodyIndexFrame != null)
{
FrameDescription bodyIndexFrameDescription = bodyIndexFrame.FrameDescription;
bodyIndexWidth = bodyIndexFrameDescription.Width;
bodyIndexHeight = bodyIndexFrameDescription.Height;
if ((bodyIndexWidth * bodyIndexHeight) == this.bodyIndexFrameData.Length)
{
bodyIndexFrame.CopyFrameDataToArray(this.bodyIndexFrameData);
bodyIndexFrameProcessed = true;
}
}
multiSourceFrameProcessed = true;
}
}
using (BodyFrame bodyFrame = multiSourceFrame.BodyFrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
bodyFrame.GetAndRefreshBodyData(this.bodies);
bodyFrameProcessed = true;
}
}
}
}
}
if (multiSourceFrameProcessed && depthFrameProcessed && colorFrameProcessed && bodyIndexFrameProcessed && bodyFrameProcessed)
{
_connectionLine.Stroke = _tcp.Connected ? Brushes.Green : Brushes.Red;
this.displayFrame = new byte[depthWidth * depthHeight * this.bytesPerPixel];
this.coordinateMapper.MapDepthFrameToColorSpace(this.depthFrameData, this.colorPoints);
this.coordinateMapper.MapDepthFrameToCameraSpace(this.depthFrameData, this.cameraPoints);
Array.Clear(displayFrame, 0, displayFrame.Length);
int step = 1;
int depthWidthIndex = 0;
int depthHeightIndex = 0;
for (int depthIndex = 0; depthIndex < depthFrameData.Length; depthIndex += step)
{
byte player = this.bodyIndexFrameData[depthIndex];
bool?c = OnlyPlayersMenuItem.IsChecked;
bool val = c != null ? (bool)c : false;
if (!val || player != 0xff)
//if (!val || player == 0)
{
CameraSpacePoint p = this.cameraPoints[depthIndex];
ColorSpacePoint colorPoint = this.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);
// set source for copy to the color pixel
int displayIndex = depthIndex * this.bytesPerPixel;
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight) && p.Z > 0)
{
// calculate index into color array
int colorIndex = ((colorY * colorWidth) + colorX) * this.bytesPerPixel;
this.displayFrame[displayIndex] = this.colorFrameData[colorIndex]; // B
this.displayFrame[displayIndex + 1] = this.colorFrameData[colorIndex + 1]; // G
this.displayFrame[displayIndex + 2] = this.colorFrameData[colorIndex + 2]; // R
this.displayFrame[displayIndex + 3] = this.colorFrameData[colorIndex + 3]; // A
if (player != 0xff && (!(Double.IsInfinity(p.X)) && !(Double.IsInfinity(p.Y)) && !(Double.IsInfinity(p.Z))))
{
#region Void Compression Algorithm
//Int16 depth = Convert.ToInt16(Math.Round((Decimal)(p.Z * 1000.0f), 0));
ushort depth = depthFrameData[depthIndex];
__ZDepth0__ = (byte)(depth % 256);
__ZDepth1__ = (byte)(depth / 256);
__Blue__ = this.colorFrameData[colorIndex];
__Green__ = this.colorFrameData[colorIndex + 1];
__Red__ = this.colorFrameData[colorIndex + 2];
//Int16 d1 = (Int16)__ZDepth0__;
//Int16 d2 = (Int16)(__ZDepth1__ * 256);
//int d3 = d1 + d2;
//if (shouldwrite)
{
__index0__ = (byte)(depthWidthIndex % 256);
__index1__ = (byte)(depthWidthIndex / 256);
__AvatarData__.Add(__index0__);
__AvatarData__.Add(__index1__);
}
__AvatarData__.Add(__Blue__);
__AvatarData__.Add(__Green__);
__AvatarData__.Add(__Red__);
__AvatarData__.Add(__ZDepth0__);
__AvatarData__.Add(__ZDepth1__);
#endregion
//shouldwrite = false;
}
else
{
//shouldwrite = true;
}
}
else
{
this.displayFrame[displayIndex] = 0;
this.displayFrame[displayIndex + 1] = 0;
this.displayFrame[displayIndex + 2] = 0;
this.displayFrame[displayIndex + 3] = 100;
}
}
//
if (++depthWidthIndex == depthWidth)
{
depthWidthIndex = 0;
++depthHeightIndex;
int newline = 5000;
byte nl0 = (byte)(newline % 256);
byte nl1 = (byte)(newline / 256);
__AvatarData__.Add(nl0);
__AvatarData__.Add(nl1);
}
}//
int numOfElements = __AvatarData__.Count;
if (numOfElements > 0)
{
if (_tcp.Connected)
{
new Thread(() =>
{
Thread.CurrentThread.IsBackground = true;
try
{
byte[] s = BitConverter.GetBytes(numOfElements); // [4]
for (int i = s.Length - 1; i >= 0; i--)
{
__AvatarData__.Insert(0, s[i]);
}
_tcp.write(__AvatarData__.ToArray());
}
catch (Exception e)
{
Console.WriteLine(e.Message);
Console.WriteLine(e.StackTrace);
}
}).Start();
}
}
colorBitmap.WritePixels(
new Int32Rect(0, 0, depthWidth, depthHeight),
this.displayFrame,
depthWidth * bytesPerPixel,
0);
}
}
}
/// <summary>
/// Converts the specified depth and body index frames to a bitmap and saves it to the specified location.
/// </summary>
/// <param name="depthFrame">The source depth frame.</param>
/// <param name="bodyIndexFrame">The source body index frame.</param>
/// <param name="destination">The destination path for the image. JPEG, PNG, GIF, BMP and TIFF formats are supported.</param>
/// <param name="width">The width of the image file.</param>
/// <param name="height">The height of the image file.</param>
/// <returns>True if the image was successfully saved. False otherwise.</returns>
public static async Task <bool> Save(this DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame, string path, int width, int height)
{
var bitmap = depthFrame.ToBitmap(bodyIndexFrame);
return(await _capture.Save(bitmap, path, width, height));
}
public void RecordFrame(DepthFrame frame)
{
if (!_isStarted)
throw new InvalidOperationException("Cannot record frames unless the KinectRecorder is started.");
if (frame != null)
{
_recordQueue.Enqueue(new RPDepthFrame(frame));
System.Diagnostics.Debug.WriteLine("+++ Enqueued Depth Frame ({0})", _recordQueue.Count);
}
else
{
System.Diagnostics.Debug.WriteLine("!!! FRAME SKIPPED (Depth in KinectRecorder)");
}
}
/// <summary>
/// Converts the specified depth frame to a bitmap image.
/// </summary>
/// <param name="frame">The specified depth frame.</param>
/// <returns>The bitmap representation of the specified depth frame.</returns>
public static WriteableBitmap ToBitmap(this DepthFrame frame)
{
_depthBitmapGenerator.Update(frame);
return(_depthBitmapGenerator.Bitmap);
}
public ImageSource ToBitmap(DepthFrame frame)
{
int width = frame.FrameDescription.Width;
int height = frame.FrameDescription.Height;
PixelFormat format = PixelFormats.Bgr32;
ushort minDepth = frame.DepthMinReliableDistance;
ushort maxDepth = frame.DepthMaxReliableDistance;
ushort[] pixelData = new ushort[width * height];
byte[] pixels = new byte[width * height * (format.BitsPerPixel + 7) / 8];
frame.CopyFrameDataToArray(pixelData);
int colorIndex = 0;
for (int depthIndex = 0; depthIndex < pixelData.Length; ++depthIndex)
{
ushort depth = pixelData[depthIndex];
byte intensity = (byte)(depth >= minDepth && depth <= maxDepth ? depth : 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);
}
/// <summary>
/// 計算該像素點座標之深度值(單位：m)
/// </summary>
internal float get_distance(DepthFrame depth, int x, int y)
{
float dist = depth.GetDistance(x, y);
return(dist);
}
void Update()
{
if (reader != null)
{
MultiSourceFrame frame = reader.AcquireLatestFrame();
if (frame != null)
{
using (ColorFrame colorFrame = frame.ColorFrameReference.AcquireFrame())
{
if (colorFrame != null)
{
colorFrame.CopyConvertedFrameDataToArray(colorData, ColorImageFormat.Rgba);
}
}
using (DepthFrame depthFrame = frame.DepthFrameReference.AcquireFrame())
{
if (depthFrame != null)
{
//Debug.Log ("bodyIndexFrame not null");
depthFrame.CopyFrameDataToArray(depthData);
}
}
using (BodyIndexFrame bodyIndexFrame = frame.BodyIndexFrameReference.AcquireFrame())
{
if (bodyIndexFrame != null)
{
//Debug.Log ("bodyIndexFrame not null");
bodyIndexFrame.CopyFrameDataToArray(bodyIndexData);
}
}
frame = null;
}
}
else
{
return;
}
MatUtils.copyToMat(colorData, outputMat);
MatUtils.copyToMat(colorData, rgbaMat);
// update mask image from bodyIndexData.
coordinateMapper.MapColorFrameToDepthSpace(depthData, depthSpacePoints);
for (int colorY = 0; colorY < colorFrameHeight; colorY++)
{
for (int colorX = 0; colorX < colorFrameWidth; colorX++)
{
int colorIndex = colorY * colorFrameWidth + colorX;
int depthX = (int)(depthSpacePoints[colorIndex].X);
int depthY = (int)(depthSpacePoints[colorIndex].Y);
if ((0 <= depthX) && (depthX < depthFrameWidth) && (0 <= depthY) && (depthY < depthFrameHeight))
{
int depthIndex = depthY * depthFrameWidth + depthX;
if (bodyIndexData[depthIndex] == 255)
{
maskData[colorIndex] = 0;
}
else
{
maskData[colorIndex] = 255;
}
}
}
}
MatUtils.copyToMat(maskData, maskMat);
if (filterType == FilterTypePreset.NONE)
{
rgbaMat.copyTo(outputMat, maskMat);
Imgproc.putText(outputMat, "Filter Type: NONE " + texture.width + "x" + texture.height, new Point(5, texture.height - 5), Imgproc.FONT_HERSHEY_PLAIN, 4.0, new Scalar(255, 0, 0, 255), 3);
}
else if (filterType == FilterTypePreset.SEPIA)
{
Core.transform(rgbaMat, rgbaMat, sepiaKernel);
rgbaMat.copyTo(outputMat, maskMat);
Imgproc.putText(outputMat, "Filter Type: SEPIA " + texture.width + "x" + texture.height, new Point(5, texture.height - 5), Imgproc.FONT_HERSHEY_PLAIN, 4.0, new Scalar(255, 0, 0, 255), 3);
}
else if (filterType == FilterTypePreset.PIXELIZE)
{
Imgproc.resize(rgbaMat, pixelizeIntermediateMat, pixelizeSize0, 0.1, 0.1, Imgproc.INTER_NEAREST);
Imgproc.resize(pixelizeIntermediateMat, rgbaMat, rgbaMat.size(), 0.0, 0.0, Imgproc.INTER_NEAREST);
rgbaMat.copyTo(outputMat, maskMat);
Imgproc.putText(outputMat, "Filter Type: PIXELIZE " + texture.width + "x" + texture.height, new Point(5, texture.height - 5), Imgproc.FONT_HERSHEY_PLAIN, 4.0, new Scalar(255, 0, 0, 255), 3);
}
else if (filterType == FilterTypePreset.COMIC)
{
comicFilter.Process(rgbaMat, rgbaMat);
rgbaMat.copyTo(outputMat, maskMat);
Imgproc.putText(outputMat, "Filter Type: COMIC " + texture.width + "x" + texture.height, new Point(5, texture.height - 5), Imgproc.FONT_HERSHEY_PLAIN, 4.0, new Scalar(255, 0, 0, 255), 3);
}
Utils.matToTexture2D(outputMat, texture);
}
/// <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)
{
return GreenScreen(colorFrame, depthFrame, bodyIndexFrame, Color.Transparent);
}
/// <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();
}
}
private void OnDepthFrameArrived(object sender, DepthFrameArrivedEventArgs e)
{
DepthFrameReference refer = e.FrameReference;
if (refer == null)
{
return;
}
DepthFrame frame = refer.AcquireFrame();
if (frame == null)
{
return;
}
using (frame)
{
FrameDescription frameDesc = frame.FrameDescription;
if (((frameDesc.Width * frameDesc.Height) == _depthData.Length) && (frameDesc.Width == _depthBitmap.PixelWidth) && (frameDesc.Height == _depthBitmap.PixelHeight))
{
//Copy Depth frame
frame.CopyFrameDataToArray(_depthData);
//Get min & max depth
ushort minDepth = frame.DepthMinReliableDistance;
ushort maxDepth = frame.DepthMaxReliableDistance;
//Adjust visualisation
int colorPixelIndex = 0;
for (int i = 0; i < _depthData.Length; ++i)
{
//Get Depth value
ushort depth = _depthData[i];
if (depth == 0)
{
_depthPixels[colorPixelIndex++] = 41;
_depthPixels[colorPixelIndex++] = 239;
_depthPixels[colorPixelIndex++] = 242;
}
else if (depth < minDepth || depth > maxDepth)
{
_depthPixels[colorPixelIndex++] = 25;
_depthPixels[colorPixelIndex++] = 0;
_depthPixels[colorPixelIndex++] = 255;
}
else
{
double gray = (Math.Floor((double)depth / 250) * 12.75);
_depthPixels[colorPixelIndex++] = (byte)gray;
_depthPixels[colorPixelIndex++] = (byte)gray;
_depthPixels[colorPixelIndex++] = (byte)gray;
}
//Increment
++colorPixelIndex;
}
//Copy output to bitmap
_depthBitmap.WritePixels(new Int32Rect(0, 0, frameDesc.Width, frameDesc.Height), _depthPixels, frameDesc.Width * _bytePerPixel, 0);
}
}
}
int avgPivot = 0; //現在張數
Mat RS_depth(DepthFrame DF)
{
Size RS_depthSize = new Size(1280, 720);
int SizeOfDepth = 1280 * 720;
Mat rtn = new Mat(RS_depthSize, DepthType.Cv8U, 1);
ushort[,] udepth = new ushort[SizeOfDepth, avgNum];
ushort[] ubuffer = new ushort[SizeOfDepth];
byte[] bdepth = new byte[SizeOfDepth];
//copy to buffer
DF.CopyTo(ubuffer);
for (int i = 0; i < SizeOfDepth; i++)
{
udepth[i, avgPivot] = ubuffer[i];
}
float max = 1000;
for (int i = 0; i < SizeOfDepth; i++)
{
ushort sum = 0;
int C = 0;
//---加總 所有這個pixel的buffer值
if (avgPivot < avgNum) //如果 現有張數不夠，則先直接平均
{
for (int n = 0; n <= avgPivot; n++) //pivot 是 index 所以要<=
{
if (udepth[i, n] > 0) //有值 才算 沒值跳過
{
sum += udepth[i, n];
C++;
}
}
}
else
{
for (int n = 0; n < avgNum; n++) //總共avgNum張
{
if (udepth[i, n] > 0) //有值 才算 沒值跳過
{
sum += udepth[i, n];
C++;
}
}
}
//---計算平均
if (C == 0)//全部都沒值 (不然除法會算錯)
{
ubuffer[i] = 0;
}
else//如果有值 就平均
{
ubuffer[i] = (ushort)(sum / C);
}
//---將值map成byte
if (ubuffer[i] > max)
{
bdepth[i] = 0;
}
else
{
bdepth[i] = (byte)(ubuffer[i] * (255.0f / max));
}
}//each pixel
avgPivot++;
if (avgPivot == avgNum)
{
avgPivot = 0;
}
Marshal.Copy(bdepth, 0, rtn.DataPointer, SizeOfDepth);
return(rtn);
}
private void MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
if (m_frameBool)
{
//pull multisource frame reference out
MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();
//Return on null
if (multiSourceFrame is null)
{
return;
}
//Calibration and full get sampled number of frames
if ((m_currentFrameType.Equals(FrameType.Calibration) || m_currentFrameType.Equals(FrameType.Full)))
{
using (DepthFrame depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame())
{
//Store one frame
m_tempDepthData[m_sampleIndex] = new ushort[m_depthFrameDescription.Width * m_depthFrameDescription.Height];
depthFrame.CopyFrameDataToArray(m_tempDepthData[m_sampleIndex]);
m_minDepth = depthFrame.DepthMinReliableDistance;
m_maxDepth = depthFrame.DepthMaxReliableDistance;
}
//...until all samples are acquired
if (m_sampleIndex == m_samplingRate - 1)
{
//Then clean the points
CleanDepth();
}
else
{
//Not done, get next sample
m_sampleIndex++;
return;
}
}
//Instantiate images
m_depthImage = new WriteableBitmap(m_depthFrameDescription.Width, m_depthFrameDescription.Height, 96, 96, PixelFormats.Bgr32, null);
m_colorImage = new WriteableBitmap(m_colorFrameDescription.Width, m_colorFrameDescription.Height, 96, 96, PixelFormats.Bgr32, null);
m_infraredImage = new WriteableBitmap(m_infraredFrameDescription.Width, m_infraredFrameDescription.Height, 96, 96, PixelFormats.Bgr32, null);
switch (m_currentFrameType)
{
case FrameType.Alignment:
using (DepthFrame depthframe = multiSourceFrame.DepthFrameReference.AcquireFrame())
{
depthframe.CopyFrameDataToArray(m_depthData);
m_maxDepth = depthframe.DepthMaxReliableDistance;
m_minDepth = depthframe.DepthMinReliableDistance;
ProcessDepth();
KinectFrameArgs args = new KinectFrameArgs(FrameType.Alignment, m_depthImage);
args.pointCloudV3 = m_cameraSpacePoints.Where(x => x.X != float.NegativeInfinity).Select(x => new Vector3(x.X, x.Y, x.Z)).ToArray();
FrameArrived.Invoke(HelperContext, args);
}
break;
case FrameType.Calibration:
ProcessDepth();
FrameArrived.Invoke(HelperContext, new KinectFrameArgs(FrameType.Calibration, m_depthImage));
break;
case FrameType.Full:
using (ColorFrame colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame())
using (InfraredFrame infraredFrame = multiSourceFrame.InfraredFrameReference.AcquireFrame())
{
ProcessDepth();
ProcessColor(colorFrame);
ProcessInfrared(infraredFrame);
KinectFrameArgs args = new KinectFrameArgs(FrameType.Full, m_depthImage, m_colorImage, m_infraredImage);
args.pointCloudCSP = m_cameraSpacePoints;
args.pointCloudV3 = m_cameraSpacePoints.Where(x => x.X != float.NegativeInfinity).Select(x => new Vector3(x.X, x.Y, x.Z)).ToArray();
FrameArrived.Invoke(HelperContext, args);
}
break;
}
m_frameBool = false;
}
else
{
return;
}
}
/// <summary>
/// Removes the background of the specified frames and generates a new bitmap (green-screen effect).
/// </summary>
/// <param name="bodyIndexFrame">The specified <see cref="BodyIndexFrame"/>.</param>
/// <param name="colorFrame">The specified <see cref="ColorFrame"/>.</param>
/// <param name="depthFrame">The specified <see cref="DepthFrame"/>.</param>
/// <returns>The bitmap representation of the generated frame.</returns>
public static WriteableBitmap GreenScreen(this BodyIndexFrame bodyIndexFrame, ColorFrame colorFrame, DepthFrame depthFrame)
{
_greenScreenBitmapGenerator.Update(colorFrame, depthFrame, bodyIndexFrame);
return(_greenScreenBitmapGenerator.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;
}
/// <summary>
/// Converts the specified depth and body index frames to a bitmap and saves it to the specified location.
/// </summary>
/// <param name="depthFrame">The source depth frame.</param>
/// <param name="bodyIndexFrame">The source body index frame.</param>
/// <param name="path">The destination path for the new image. JPEG, PNG, GIF, BMP and TIFF formats are supported.</param>
/// <returns>True if the image was successfully saved. False otherwise.</returns>
public static bool Save(this DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame, string path)
{
var bitmap = depthFrame.ToBitmap(bodyIndexFrame);
return(_capture.Save(bitmap, path));
}
/// <summary>
/// Initializes a new instance of the <see cref="ReplayDepthFrame"/> class
/// based on the specified <c>DepthFrame</c>.
/// </summary>
/// <param name="frame">The frame.</param>
internal ReplayDepthFrame(DepthFrame frame)
{
this.FrameType = FrameTypes.Depth;
this.RelativeTime = frame.RelativeTime;
this.DepthMinReliableDistance = frame.DepthMinReliableDistance;
this.DepthMaxReliableDistance = frame.DepthMaxReliableDistance;
this.Width = frame.FrameDescription.Width;
this.Height = frame.FrameDescription.Height;
this.BytesPerPixel = frame.FrameDescription.BytesPerPixel;
_frameData = new ushort[this.Width * this.Height];
frame.CopyFrameDataToArray(_frameData);
}
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>
/// Build depth bitmap.
/// </summary>
/// <param name="frame"></param>
/// <param name="bitmap"></param>
/// <param name="withLock"></param>
/// <param name="segmentation"></param>
public void BuildDepthBitmap(DepthFrame depthFrame, SmallFrameBitmap bitmap, bool withLock)
{
depthFrame.CopyFrameDataToArray(_depthData);
_displayFilter.Init(
DisplayFilterMode.Rainbow,
Frame.DEPTH_INFRARED_WIDTH,
Frame.DEPTH_INFRARED_HEIGHT,
0,
depthFrame.DepthMaxReliableDistance,
depthFrame.DepthMinReliableDistance
);
Array.Clear(_depthPixels, 0, _depthPixels.Length);
_displayFilter.Apply(_depthData, _depthPixels, null);
WriteableBitmap outBitmap = bitmap.Bitmap;
ValidateBitmap(outBitmap, Frame.DEPTH_INFRARED_WIDTH, Frame.DEPTH_INFRARED_HEIGHT);
CopyToDisplay(outBitmap, _depthPixels, withLock);
}
private void MapColorToDepth(DepthFrame depthFrame, Shared<Image> colorImage)
{
const int colorImageWidth = 1920;
const int colorImageHeight = 1080;
if (!this.configuration.OutputColorToCameraMapping && !this.configuration.OutputRGBD)
{
return;
}
ushort[] depthData = new ushort[depthFrame.FrameDescription.LengthInPixels];
depthFrame.CopyFrameDataToArray(depthData);
if (this.configuration.OutputColorToCameraMapping)
{
// Writing out a mapping from color space to camera space
CameraSpacePoint[] colorToCameraMapping = new CameraSpacePoint[colorImageWidth * colorImageHeight];
this.kinectSensor.CoordinateMapper.MapColorFrameToCameraSpace(depthData, colorToCameraMapping);
var time = this.pipeline.GetCurrentTimeFromElapsedTicks(depthFrame.RelativeTime.Ticks);
this.ColorToCameraMapper.Post(colorToCameraMapping, time);
}
if (this.configuration.OutputRGBD)
{
unsafe
{
DepthSpacePoint[] depthSpacePoints = new DepthSpacePoint[colorImageWidth * colorImageHeight];
this.kinectSensor.CoordinateMapper.MapColorFrameToDepthSpace(depthData, depthSpacePoints);
using (var rgbd = ImagePool.GetOrCreate(colorImageWidth, colorImageHeight, Imaging.PixelFormat.RGBA_64bpp))
{
byte* srcRow = (byte*)colorImage.Resource.ImageData.ToPointer();
byte* dstRow = (byte*)rgbd.Resource.ImageData.ToPointer();
int depthWidth = depthFrame.FrameDescription.Width;
int depthHeight = depthFrame.FrameDescription.Height;
for (int y = 0; y < colorImage.Resource.Height; y++)
{
byte* srcCol = srcRow;
ushort* dstCol = (ushort*)dstRow;
int offset = y * colorImageWidth;
for (int x = 0; x < colorImage.Resource.Width; x++)
{
dstCol[0] = (ushort)(srcCol[2] << 8);
dstCol[1] = (ushort)(srcCol[1] << 8);
dstCol[2] = (ushort)(srcCol[0] << 8);
DepthSpacePoint pt = depthSpacePoints[offset];
if (pt.X >= 0 && pt.X < depthWidth && pt.Y >= 0 && pt.Y < depthHeight)
{
dstCol[3] = depthData[((int)pt.Y * depthWidth) + (int)pt.X];
}
else
{
dstCol[3] = 0;
}
dstCol += 4;
srcCol += colorImage.Resource.BitsPerPixel / 8;
offset++;
}
srcRow += colorImage.Resource.Stride;
dstRow += rgbd.Resource.Stride;
}
var time = this.pipeline.GetCurrentTimeFromElapsedTicks(depthFrame.RelativeTime.Ticks);
this.RGBDImage.Post(rgbd, time);
}
}
}
}
/// <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);
}
/// <summary>
/// Initializes a new instance of the <see cref="ReplayDepthFrame"/> class
/// based on the specified <c>DepthFrame</c> and array of <c>ushort</c>.
/// </summary>
/// <param name="frame">The frame.</param>
/// <param name="frameData">The frame data.</param>
internal ReplayDepthFrame(DepthFrame frame, ushort[] frameData)
{
this.FrameType = FrameTypes.Depth;
this.RelativeTime = frame.RelativeTime;
this.DepthMinReliableDistance = frame.DepthMinReliableDistance;
this.DepthMaxReliableDistance = frame.DepthMaxReliableDistance;
this.Width = frame.FrameDescription.Width;
this.Height = frame.FrameDescription.Height;
this.BytesPerPixel = frame.FrameDescription.BytesPerPixel;
_frameData = frameData;
}
private void depthFrameReader_FrameArrived(object sender, DepthFrameArrivedEventArgs e)
{
using (DepthFrame depthFrame = e.FrameReference.AcquireFrame())
{
if (depthFrame != null)
{
using (KinectBuffer depthBuffer = depthFrame.LockImageBuffer())
{
if ((this.depthFrameDescription.Width * this.depthFrameDescription.Height) == (depthBuffer.Size / this.depthFrameDescription.BytesPerPixel))
{
int frame = windowSize / 2;
DepthSpacePoint pl = coordinateMapper.MapCameraPointToDepthSpace(leftHandPostition);
DepthSpacePoint pr = coordinateMapper.MapCameraPointToDepthSpace(rightHandPostition);
//Draw recordState
bool record = btn_record.Content.Equals("Stop");
if (record)
{
lbl_fpsDepth.Content = Convert.ToUInt64(lbl_fpsBody.Content) + 1;
}
// === Links
if (pl.X <= frame || pl.X >= depthFrameDescription.Width - frame || pl.Y <= frame || pl.Y >= depthFrameDescription.Height - frame)
{
pl = pl_old;
}
ProcessDepthFrameData(depthBuffer.UnderlyingBuffer, frame, 0, ushort.MaxValue, pl, record, true);
this.depthBitmapLeft.WritePixels(
new Int32Rect(0, 0, this.depthBitmapLeft.PixelWidth, this.depthBitmapLeft.PixelHeight),
this.depthPixels,
this.depthBitmapLeft.PixelWidth,
0);
using (DrawingContext dc = leftHandDrawingGroup.Open())
{
dc.DrawImage(LeftHandSource, new Rect(0.0, 0.0, depthFrameDescription.Width, depthFrameDescription.Height));
}
pl_old = pl;
// === Rechts
if (pr.X <= frame || pr.X >= depthFrameDescription.Width - frame || pr.Y <= frame || pr.Y >= depthFrameDescription.Height - frame)
{
pr = pr_old;
}
ProcessDepthFrameData(depthBuffer.UnderlyingBuffer, frame, 0, ushort.MaxValue, pr, record, false);
this.depthBitmapRight.WritePixels(
new Int32Rect(0, 0, this.depthBitmapRight.PixelWidth, this.depthBitmapRight.PixelHeight),
this.depthPixels,
this.depthBitmapRight.PixelWidth,
0);
using (DrawingContext dc = rightHandDrawingGroup.Open())
{
dc.DrawImage(RightHandSource, new Rect(0.0, 0.0, depthFrameDescription.Width, depthFrameDescription.Height));
}
pr_old = pr;
}
}
}
}
}
private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
if (!(KinectStreamerConfig.ProvideBodyData || KinectStreamerConfig.ProvideColorData || KinectStreamerConfig.ProvideDepthData))
{
return;
}
depthFrame = null;
colorFrame = null;
bodyFrame = null;
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();
bodyFrame = multiSourceFrame.BodyFrameReference.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) || (bodyFrame == null))
{
return;
}
// Process color stream if needed
if (KinectStreamerConfig.ProvideColorData)
{
ProcessColorData();
}
// Process depth frame if needed
if (KinectStreamerConfig.ProvideDepthData)
{
ProcessDepthData();
}
// Process body data if needed
if (KinectStreamerConfig.ProvideBodyData)
{
ProcessBodyData();
}
}
finally
{
if (depthFrame != null)
{
depthFrame.Dispose();
}
if (colorFrame != null)
{
colorFrame.Dispose();
}
if (bodyFrame != null)
{
bodyFrame.Dispose();
}
}
}
private void Reader_MultiSourceFrameArrived(MultiSourceFrameReader sender, MultiSourceFrameArrivedEventArgs e)
{
MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();
// If the Frame has expired by the time we process this event, return.
if (multiSourceFrame == null)
{
return;
}
DepthFrame depthFrame = null;
ColorFrame colorFrame = null;
InfraredFrame infraredFrame = null;
BodyFrame bodyFrame = null;
BodyIndexFrame bodyIndexFrame = null;
IBuffer depthFrameDataBuffer = null;
IBuffer bodyIndexFrameData = null;
// Com interface for unsafe byte manipulation
IBufferByteAccess bufferByteAccess = null;
switch (CurrentDisplayFrameType)
{
case DisplayFrameType.Infrared:
using (infraredFrame = multiSourceFrame.InfraredFrameReference.AcquireFrame())
{
ShowInfraredFrame(infraredFrame);
}
break;
case DisplayFrameType.Color:
using (colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame())
{
ShowColorFrame(colorFrame);
}
break;
case DisplayFrameType.Depth:
using (depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame())
{
ShowDepthFrame(depthFrame);
}
break;
case DisplayFrameType.BodyMask:
// Put in a try catch to utilise finally() and clean up frames
try
{
depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
bodyIndexFrame = multiSourceFrame.BodyIndexFrameReference.AcquireFrame();
colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
if ((depthFrame == null) || (colorFrame == null) || (bodyIndexFrame == null))
{
return;
}
// Access the depth frame data directly via LockImageBuffer to avoid making a copy
depthFrameDataBuffer = depthFrame.LockImageBuffer();
this.coordinateMapper.MapColorFrameToDepthSpaceUsingIBuffer(depthFrameDataBuffer, this.colorMappedToDepthPoints);
// Process Color
colorFrame.CopyConvertedFrameDataToBuffer(this.bitmap.PixelBuffer, ColorImageFormat.Bgra);
// Access the body index frame data directly via LockImageBuffer to avoid making a copy
bodyIndexFrameData = bodyIndexFrame.LockImageBuffer();
ShowMappedBodyFrame(depthFrame.FrameDescription.Width, depthFrame.FrameDescription.Height, bodyIndexFrameData, bufferByteAccess);
}
finally
{
if (depthFrame != null)
{
depthFrame.Dispose();
}
if (colorFrame != null)
{
colorFrame.Dispose();
}
if (bodyIndexFrame != null)
{
bodyIndexFrame.Dispose();
}
if (depthFrameDataBuffer != null)
{
// We must force a release of the IBuffer in order to ensure that we have dropped all references to it.
System.Runtime.InteropServices.Marshal.ReleaseComObject(depthFrameDataBuffer);
}
if (bodyIndexFrameData != null)
{
System.Runtime.InteropServices.Marshal.ReleaseComObject(bodyIndexFrameData);
}
if (bufferByteAccess != null)
{
System.Runtime.InteropServices.Marshal.ReleaseComObject(bufferByteAccess);
}
}
break;
case DisplayFrameType.BodyJoints:
using (bodyFrame = multiSourceFrame.BodyFrameReference.AcquireFrame())
{
ShowBodyJoints(bodyFrame);
}
break;
case DisplayFrameType.BackgroundRemoved:
// Put in a try catch to utilise finally() and clean up frames
try
{
depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
if ((depthFrame == null) || (colorFrame == null))
{
return;
}
depthFrame.CopyFrameDataToArray(depthFrameData);
this.coordinateMapper.MapColorFrameToDepthSpace(depthFrameData, this.colorMappedToDepthPoints);
// Process Color.
colorFrame.CopyConvertedFrameDataToBuffer(this.bitmap.PixelBuffer, ColorImageFormat.Bgra);
ShowMappedColorBackgroundRemoved(colorMappedToDepthPoints, depthFrameData, depthFrame.FrameDescription);
}
finally
{
if (depthFrame != null)
{
depthFrame.Dispose();
}
if (colorFrame != null)
{
colorFrame.Dispose();
}
}
break;
case DisplayFrameType.FaceOnColor:
using (colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame())
{
ShowColorFrame(colorFrame);
this.faceManager.DrawLatestFaceResults(this.FacePointsCanvas, this.faceFrameFeatures);
}
break;
case DisplayFrameType.FaceOnInfrared:
using (infraredFrame = multiSourceFrame.InfraredFrameReference.AcquireFrame())
{
ShowInfraredFrame(infraredFrame);
DrawFaceOnInfrared();
}
break;
case DisplayFrameType.FaceGame:
FaceGameLookUpdate();
break;
default:
break;
}
}
private void ProcessFrame(DepthFrame frame)
{
int width = frame.FrameDescription.Width;
int height = frame.FrameDescription.Height;
//ushort minDepth = 0;
ushort minDepth = frame.DepthMinReliableDistance;
ushort maxDepth = ushort.MaxValue;
//ushort maxDepth = frame.DepthMaxReliableDistance;
ushort[] depthData = new ushort[width * height];
int stride = (width * format.BitsPerPixel + 7) / 8;
byte[] pixelData = new byte[stride * height];
frame.CopyFrameDataToArray(depthData);
int colorIndex = 0;
for (int depthIndex = 0; depthIndex < depthData.Length; ++depthIndex)
{
ushort depth = depthData[depthIndex];
byte intensity = (byte)(depth >= minDepth && depth <= maxDepth ? depth : 0);
pixelData[colorIndex++] = intensity; // Blue
pixelData[colorIndex++] = intensity; // Green
pixelData[colorIndex++] = intensity; // Red
++colorIndex;
}
if (depthBitmap == null)
{
this.depthBitmap = new WriteableBitmap(displayWidth, displayHeight, 96.0, 96.0,format, null);
}
Random r = new Random();
r.NextBytes(pixelData);
depthBitmap.Lock();
Marshal.Copy(pixelData, 0, depthBitmap.BackBuffer, pixelData.Length);
depthBitmap.AddDirtyRect(new Int32Rect(0, 0, displayWidth, displayHeight));
depthBitmap.Unlock();
this.depthData = depthData;
}
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];
}
}
}
private void processDepthFrame(DepthFrame depthFrame, int depthWidth, int depthHeight)
{
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())
{
this.coordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
depthFrameData.UnderlyingBuffer,
depthFrameData.Size,
this.colorMappedToDepthPoints);
}
// We're done with the DepthFrame
depthFrame.Dispose();
depthFrame = null;
}
/// <summary>
/// Handles the color frame data arriving from the sensor
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private unsafe void MultiSourceFrameReader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
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
{
depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
colorFrame = multiSourceFrame.ColorFrameReference.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))
{
return;
}
this.multiSourceFrameReader.IsPaused = true;
// Process Depth
FrameDescription depthFrameDescription = depthFrame.FrameDescription;
FrameDescription colorFrameDescription = colorFrame.FrameDescription;
// Access the depth frame data directly via LockImageBuffer to avoid making a copy
using (KinectBuffer depthFrameData = depthFrame.LockImageBuffer())
{
this.coordinateMapper.MapDepthFrameToColorSpaceUsingIntPtr(depthFrameData.UnderlyingBuffer, depthFrameData.Size, this.depthMappedToColorPoints);
ushort maxDepth = ushort.MaxValue;
this.ProcessDepthFrameData(depthFrameData.UnderlyingBuffer, depthFrameData.Size, depthFrame.DepthMinReliableDistance, maxDepth);
}
// We're done with the DepthFrame
depthFrame.Dispose();
depthFrame = null;
BitmapSource colorSource = getColorImage(colorFrameDescription, colorFrame);
Bitmap colorBitmap = getBitmap(colorSource);
switch (state)
{
case State.DETECT_CONTROLS:
detectShapeCandidates(ref colorBitmap, false);
break;
case State.MAIN:
OpenCV(ref colorBitmap);
break;
}
writeToBackBuffer(ConvertBitmap(colorBitmap), this.colorBitmap);
colorBitmap.Dispose();
colorFrame.Dispose();
colorFrame = null;
calculateFps();
}
finally
{
if (depthFrame != null)
{
depthFrame.Dispose();
}
if (colorFrame != null)
{
colorFrame.Dispose();
}
this.multiSourceFrameReader.IsPaused = false;
}
}
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
}
}
// Nuitrack depth sensor module data update callback
private void onDepthSensorUpdate(DepthFrame depthFrame)
{
_depthFrame = depthFrame;
}
public System.Windows.Media.ImageSource ImageToImageSource(DepthFrame frame)
{
WPFPresenter presenter = new WPFPresenter(Factory, frame.Width, frame.Height);
ushort minValue;
ushort maxValue;
var image = frame.ToDirectCanvasImage(Factory, out minValue, out maxValue);
rawDepthLayer.CopyFromImage(image);
unpackEffect.MinThreshold = 100f;
unpackEffect.MaxThreshold = 1000f;
unpackEffect.MinValue = minValue;
unpackEffect.MaxValue = maxValue;
unpackEffect.TexSize = new DirectCanvas.Misc.Size(rawDepthLayer.Width, rawDepthLayer.Height);
rawDepthLayer.ApplyEffect(unpackEffect, presenter, true);
presenter.Present();
return presenter.ImageSource;
}
public Bitmap GreenScreenImpl(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 Bitmap(depthWidth, depthHeight, FORMAT);
}
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] = _colorData[colorIndex];
_displayPixels[displayIndex + 1] = _colorData[colorIndex + 1];
_displayPixels[displayIndex + 2] = _colorData[colorIndex + 2];
_displayPixels[displayIndex + 3] = 0xff;
}
}
}
}
BitmapData bmapdata = _bitmap.LockBits(new System.Drawing.Rectangle(0, 0, depthWidth, depthHeight), ImageLockMode.ReadWrite, _bitmap.PixelFormat);
IntPtr ptr = bmapdata.Scan0;
Marshal.Copy(_displayPixels, 0, ptr, _displayPixels.Length);
_bitmap.UnlockBits(bmapdata);
}
return(_bitmap);
}
/// <summary>
/// Removes the background of the specified frames and generates a new bitmap (green-screen effect).
/// </summary>
/// <param name="bodyIndexFrame">The specified <see cref="BodyIndexFrame"/>.</param>
/// <param name="colorFrame">The specified <see cref="ColorFrame"/>.</param>
/// <param name="depthFrame">The specified <see cref="DepthFrame"/>.</param>
/// <returns>The bitmap representation of the generated frame.</returns>
public static WriteableBitmap GreenScreen(this BodyIndexFrame bodyIndexFrame, ColorFrame colorFrame, DepthFrame depthFrame)
{
_greenScreenBitmapGenerator.Update(colorFrame, depthFrame, bodyIndexFrame);
return _greenScreenBitmapGenerator.Bitmap;
}
/// <summary>
/// Generates the point cloud
/// </summary>
/// <param name="df">The Depth Frame from the Kinect sensor</param>
/// <param name="bif">The Body Index Frame from the Kinect sensor</param>
/// <returns>The PointCloud generated from the frames</returns>
public PointCloud generate(DepthFrame df, BodyIndexFrame bif)
{
Log.Write("Creating point Cloud");
/*
* used to calculate centroid, as well as lowest x value for later on ->
* saves us looping later, though PointCloud methods do allow you to do that
*/
double xAccumulator = 0.0, yAccumulator = 0.0, zAccumulator = 0.0, xMean = 0.0, yMean = 0.0, zMean = 0.0, xMinimum = (1 / 0.0);
int depthFrameWidth = df.FrameDescription.Width;
int depthFrameHeight = df.FrameDescription.Height;
this.depthFrameData = new ushort[depthFrameWidth * depthFrameHeight];
this.bodyIndexFrameData = new byte[depthFrameWidth * depthFrameHeight];
this.cameraPoints = new CameraSpacePoint[depthFrameWidth * depthFrameHeight];
df.CopyFrameDataToArray(depthFrameData);
bif.CopyFrameDataToArray(bodyIndexFrameData);
coordinateMapper.MapDepthFrameToCameraSpace(depthFrameData, cameraPoints);
// Create new point cloud for storing points and operating on later
PointCloud pointCloud = new PointCloud();
int numberOfPoints = 0;
// loop over each row and column of the depth
for (int y = 0; y < depthFrameHeight; y++)
{
for (int x = 0; x < depthFrameWidth; x++)
{
// calculate index into depth array
int depthIndex = (y * depthFrameWidth) + x;
byte humanPoint = bodyIndexFrameData[depthIndex];
if (humanPoint == 0xff) // Check if human point empty
{
continue;
}
CameraSpacePoint p = this.cameraPoints[depthIndex];
if (!(Double.IsInfinity(p.X)) && !(Double.IsInfinity(p.Y)) && !(Double.IsInfinity(p.Z)))
{
if (p.X < depthLimit && p.Y < depthLimit && p.Z < depthLimit)
{
Point3D scaledPoint = new Point3D(p.X * unitScale, p.Y * unitScale, p.Z * unitScale);
pointCloud.addPoint(scaledPoint);
xAccumulator += scaledPoint.X;
yAccumulator += scaledPoint.Y;
zAccumulator += scaledPoint.Z;
numberOfPoints++;
if (scaledPoint.X < xMinimum)
{
xMinimum = scaledPoint.X;
}
}
}
} // end of for(int x..) loop over points
} // end of for(int y..) loop over points
xMean = xAccumulator / numberOfPoints;
yMean = yAccumulator / numberOfPoints;
zMean = zAccumulator / numberOfPoints;
// centroid calculated on the fly so we don't have to loop again unnecessarily
Point3D centroid = new Point3D(xMean, yMean, zMean);
pointCloud.subtractFromPointAxis(xMinimum, 0);
pointCloud.rotateOnSpot(180, PointCloud.Axis.Y, centroid);
Log.Write("Finished calculating point cloud");
return(pointCloud);
}
void GreenScreenMappingDepthToColorSplats(ref DepthFrame depthFrame, ref ColorFrame colorFrame, ref BodyIndexFrame bodyIndexFrame, int depthWidth, int depthHeight, int colorWidth, int colorHeight)
{
m_stopwatch.Restart();
using (KinectBuffer depthFrameData = depthFrame.LockImageBuffer()) {
// Need to know the color space point for each depth space point, but this is much less data
// and much faster to compute than mapping the other way
m_coordinateMapper.MapDepthFrameToColorSpaceUsingIntPtr(
depthFrameData.UnderlyingBuffer,
depthFrameData.Size,
m_depthToColorSpacePoints);
}
m_depthMapTimer.Update(m_stopwatch.ElapsedMilliseconds);
m_stopwatch.Restart();
// We're done with the DepthFrame
depthFrame.Dispose();
depthFrame = null;
lock (m_displayPixels) { // [KinectThread] avoid racing display buffer refresh with render (can cause missing images)
// have to clear the display pixels so we can copy only the BGRA image of the player(s)
Array.Clear(m_displayPixels, 0, m_displayPixels.Length);
unsafe {
fixed (byte* colorFrameDataPtr = &m_colorFrameData[0]) {
colorFrame.CopyConvertedFrameDataToIntPtr(new IntPtr(colorFrameDataPtr), (uint)m_colorFrameData.Length, ColorImageFormat.Bgra);
}
}
// done with the colorFrame
colorFrame.Dispose();
colorFrame = null;
m_colorCopyTimer.Update(m_stopwatch.ElapsedMilliseconds);
m_stopwatch.Restart();
// We'll access the body index data directly to avoid a copy
using (KinectBuffer bodyIndexData = bodyIndexFrame.LockImageBuffer()) {
unsafe {
byte* bodyIndexDataPointer = (byte*)bodyIndexData.UnderlyingBuffer;
uint bodyIndexDataLength = bodyIndexData.Size;
int colorMappedToDepthPointCount = m_colorToDepthSpacePoints.Length;
fixed (ColorSpacePoint* depthMappedToColorPointsPointer = m_depthToColorSpacePoints) {
fixed (byte* bitmapPixelsBytePointer = &m_displayPixels[0]) {
fixed (byte* sourcePixelsBytePointer = &m_colorFrameData[0]) {
uint* bitmapPixelsPointer = (uint*)bitmapPixelsBytePointer;
uint* sourcePixelsPointer = (uint*)sourcePixelsBytePointer;
// We don't go all the way to the edge of the depth buffer, to eliminate a chance
// that a splat will go outside the edge of the color buffer when mapped to color
// space. In the x direction this will never happen anyway since the depth FOV
// is so much narrower than the color FOV.
const int Margin = 2;
for (int y = Margin; y < depthHeight - Margin; y++) {
for (int x = 0; x < depthWidth; x++) {
// Scan forwards until we find a non-0xff value in the body index data.
int depthIndex = y * depthWidth + x;
if (bodyIndexDataPointer[depthIndex] != 0xff) {
int depthIndex2 = depthIndex;
// We found the beginning of a horizontal run of player pixels.
// Scan to the end.
int runWidth;
for (runWidth = 1; runWidth + x < depthWidth; runWidth++) {
depthIndex2++;
if (bodyIndexDataPointer[depthIndex2] == 0xff) {
break;
}
}
// Now splat from (x, y) to (x + runWidth, y)
float depthMappedToColorLeftX = depthMappedToColorPointsPointer[depthIndex].X;
float depthMappedToColorLeftY = depthMappedToColorPointsPointer[depthIndex].Y;
float depthMappedToColorRightX = depthMappedToColorPointsPointer[depthIndex2 - 1].X;
float depthMappedToColorRightY = depthMappedToColorPointsPointer[depthIndex2 - 1].Y;
// Now copy color pixels along that rectangle.
const int splatHMargin = 2; // X margin of splat rectangle in color pixels
const int splatVMargin = 3; // Y margin of splat rectangle in color pixels
int minX = (int)Math.Min(depthMappedToColorLeftX, depthMappedToColorRightX) - splatHMargin;
int minY = (int)Math.Min(depthMappedToColorLeftY, depthMappedToColorRightY) - splatVMargin;
int maxX = (int)Math.Max(depthMappedToColorLeftX, depthMappedToColorRightX) + splatHMargin;
int maxY = (int)Math.Max(depthMappedToColorLeftY, depthMappedToColorRightY) + splatVMargin;
// Some edge of screen situations can result in color space points that are negative or otherwise
// actually outside the color space coordinate range.
Clamp(ref minX, colorWidth - 1);
Clamp(ref minY, colorHeight - 1);
Clamp(ref maxX, colorWidth - 1);
Clamp(ref maxY, colorHeight - 1);
for (int colorY = minY; colorY < maxY; colorY++) {
int colorIndex = colorY * colorWidth + minX;
for (int colorX = minX; colorX < maxX; colorX++) {
bitmapPixelsPointer[colorIndex] = sourcePixelsPointer[colorIndex];
colorIndex++;
}
}
x += runWidth;
}
}
}
}
}
}
}
}
// Done with bodyIndexFrame
bodyIndexFrame.Dispose();
bodyIndexFrame = null;
}
m_colorScanTimer.Update(m_stopwatch.ElapsedMilliseconds);
m_stopwatch.Restart();
m_displayTexture.SetData(m_displayPixels);
m_textureSetDataTimer.Update(m_stopwatch.ElapsedMilliseconds);
m_stopwatch.Restart();
Spam.TopLine1 = string.Format("depth map: {0} msec; color copy: {1} msec; color scan: {2} msec; texture set: {3} msec",
m_depthMapTimer.Average,
m_colorCopyTimer.Average,
m_colorScanTimer.Average,
m_textureSetDataTimer.Average);
}
/// <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] = _colorData[colorIndex];
_displayPixels[displayIndex + 1] = _colorData[colorIndex + 1];
_displayPixels[displayIndex + 2] = _colorData[colorIndex + 2];
_displayPixels[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);
}
private void ShowDepthFrame(DepthFrame depthFrame)
{
bool depthFrameProcessed = false;
ushort minDepth = 0;
ushort maxDepth = 0;
if (depthFrame != null)
{
FrameDescription depthFrameDescription = depthFrame.FrameDescription;
// verify data and write the new infrared frame data to the display bitmap
if (((depthFrameDescription.Width * depthFrameDescription.Height)
== this.infraredFrameData.Length) &&
(depthFrameDescription.Width == this.bitmap.PixelWidth) &&
(depthFrameDescription.Height == this.bitmap.PixelHeight))
{
// Copy the pixel data from the image to a temporary array
depthFrame.CopyFrameDataToArray(this.depthFrameData);
minDepth = depthFrame.DepthMinReliableDistance;
maxDepth = depthFrame.DepthMaxReliableDistance;
//maxDepth = 8000;
depthFrameProcessed = true;
}
}
// we got a frame, convert and render
if (depthFrameProcessed)
{
ConvertDepthDataToPixels(minDepth, maxDepth);
RenderPixelArray(this.depthPixels);
}
}
/// <summary>
/// Handles the depth frame data arriving from the sensor
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void Reader_FrameArrived(object sender, DepthFrameArrivedEventArgs e)
{
bool depthFrameProcessed = false;
Console.WriteLine("frame arriving");
using (DepthFrame depthFrame = e.FrameReference.AcquireFrame())
{
if (depthFrame != null)
{
// the fastest way to process the body index data is to directly access
// the underlying buffer
using (Microsoft.Kinect.KinectBuffer depthBuffer = depthFrame.LockImageBuffer())
{
// verify data and write the color data to the display bitmap
if (((this.depthFrameDescription.Width * this.depthFrameDescription.Height) == (depthBuffer.Size / this.depthFrameDescription.BytesPerPixel)) &&
(this.depthFrameDescription.Width == this.depthBitmap.PixelWidth) && (this.depthFrameDescription.Height == this.depthBitmap.PixelHeight))
{
// 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
Console.WriteLine("depth processing");
this.ProcessDepthFrameData(depthBuffer.UnderlyingBuffer, depthBuffer.Size, depthFrame.DepthMinReliableDistance, maxDepth);
//checking if total dark pixels in frame exceed threshold
BackgroundWorker bw = new BackgroundWorker();
bw.DoWork += new DoWorkEventHandler(delegate(object o, DoWorkEventArgs args)
{
//HERE
if (numDarkPixels > threshold)
{
vibrate = true;
count++;
//Console.WriteLine(vibrate);
if (count == 10)
{
try
{
using (WebClient client = new WebClient())
{
var values = new NameValueCollection();
values.Add("vibrate", "true");
var response = client.UploadValues("http://localhost:3000/", "POST", values);
var responseString = Encoding.Default.GetString(response);
}
}
catch (Exception entry)
{
Console.WriteLine(entry);
}
count = 0;
}
//previousVal = numDarkPixels;
}
else if (numDarkPixels < threshold)
{
vibrate = false;
count = 0;
//previousVal = numDarkPixels;
}
});
bw.RunWorkerAsync();
depthFrameProcessed = true;
}
}
}
}
if (depthFrameProcessed)
{
this.RenderDepthPixels();
}
}
