/// <summary>
/// Calculates the angle between the specified points.
/// </summary>
/// <param name="center">The center of the angle.</param>
/// <param name="start">The start of the angle.</param>
/// <param name="end">The end of the angle.</param>
/// <returns>The angle, in degrees.</returns>
public static double AngleBetween(this ColorSpacePoint center, ColorSpacePoint start, ColorSpacePoint end)
{
Vector3 first = start.ToVector3() - center.ToVector3();
Vector3 second = end.ToVector3() - center.ToVector3();
return Vector3.AngleBetween(first, second);
}
public void write(Int32 width, Int32 height, Int32 shortRange, Int32 longRange, ColorSpacePoint[,] shortRangeMap, ColorSpacePoint[,] longRangeMap)
{
if (File.Exists(filename))
{
var result = System.Windows.Forms.MessageBox.Show("File " + filename + " exists. Do you want to replace it?", "File exists", System.Windows.Forms.MessageBoxButtons.YesNo);
if (result == System.Windows.Forms.DialogResult.No)
{
return;
}
}
using (BinaryWriter coordinateMappingWriter = new BinaryWriter(File.Open(filename, FileMode.Create)))
{
coordinateMappingWriter.Write(width);
coordinateMappingWriter.Write(height);
coordinateMappingWriter.Write(shortRange);
coordinateMappingWriter.Write(longRange);
for (int i = 0; i < width; i++)
for (int j = 0; j < height; j++)
{
coordinateMappingWriter.Write(shortRangeMap[i, j].X);
coordinateMappingWriter.Write(shortRangeMap[i, j].Y);
}
for (int i = 0; i < width; i++)
for (int j = 0; j < height; j++)
{
coordinateMappingWriter.Write(longRangeMap[i, j].X);
coordinateMappingWriter.Write(longRangeMap[i, j].Y);
}
}
}
/// <summary>
/// Create a new <c>CustomJoint</c> object based on the supplied
/// <c>JointType</c> value.
/// </summary>
public CustomJoint(JointType type)
{
_jointType = type;
_position = new CameraSpacePoint();
_depthPosition = new DepthSpacePoint();
_colorPosition = new ColorSpacePoint();
_trackingState = TrackingState.NotTracked;
}
/// <summary>
/// 深度画像座標からカラー画像座標へ変換するやつ
/// </summary>
/// <param name="depthSpacePoint"></param>
/// <param name="depth"></param>
/// <returns></returns>
public ColorSpacePoint MapDepthPointToColorSpace(DepthSpacePoint depthSpacePoint, ushort depth)
{
int index = (int)(this.depthWidth * depthSpacePoint.Y + depthSpacePoint.X);
PointF lut = this.depthFrameToColorSpacfeTable[index];
ColorSpacePoint tempP = new ColorSpacePoint();
tempP.Y = lut.Y;
tempP.X = lut.X + (float)(this.D / depth);
return tempP;
}
/// <summary>
/// Copies face color points to the gpu
/// </summary>
/// <param name="context">Device context</param>
/// <param name="points">Points array</param>
/// <param name="elementCount">Number of elements to copy</param>
public void Copy(DeviceContext context, ColorSpacePoint[] points, int elementCount)
{
if (points.Length == 0)
return;
fixed (ColorSpacePoint* cptr = &points[0])
{
this.buffer.Upload(context, new IntPtr(cptr), points.Length * 8);
}
}
internal Finger(DepthPointEx point, CoordinateMapper coordinateMapper)
{
ushort depth = (ushort)point.Z;
DepthPoint = new DepthSpacePoint
{
X = point.X,
Y = point.Y
};
ColorPoint = coordinateMapper.MapDepthPointToColorSpace(DepthPoint, (ushort)point.Z);
CameraPoint = coordinateMapper.MapDepthPointToCameraSpace(DepthPoint, (ushort)point.Z);
}
/// <summary>
/// this function alignes the depth and color data
/// tracks the head joint
/// and cuts out a texture scaled by the distance of the person,
/// so it will only contain the head
/// than it saves the files to disk
/// and sets the loading state for the monitor
/// </summary>
/// <param name="body">Body.</param>
/// <param name="bodyObject">Body object.</param>
public void SetFace(Kinect.Body body, GameObject bodyObject)
{
Kinect.Joint headJoint = body.Joints[Kinect.JointType.Head];
if (headJoint.TrackingState == TrackingState.Tracked)
{
/// <summary>
/// get the color image coordinates for the head joint
/// </summary>
ColorSpacePoint ColorSpaceHead =
KinectSensor.GetDefault().CoordinateMapper.MapCameraPointToColorSpace(headJoint.Position);
/// <summary>
/// get the depth image coordinates for the head joint
/// </summary>
DepthSpacePoint depthPoint =
KinectSensor.GetDefault().CoordinateMapper.MapCameraPointToDepthSpace(headJoint.Position);
/// <summary>
/// use the x & y coordinates to locate the depth data
/// </summary>
FrameDescription depthDesc = KinectSensor.GetDefault().DepthFrameSource.FrameDescription;
int depthX = (int)Mathf.Floor(depthPoint.X + 0.5F);
int depthY = (int)Mathf.Floor(depthPoint.Y + 0.5F);
int depthIndex = (depthY * depthDesc.Width) + depthX;
//ushort depth = _depthData[depthIndex];
/// <remarks>
/// SET FACE IS COUSING AN ERROR SOMETIMES
///
/// IndexOutOfRangeException: Array index is out of range.
/// FaceSourceView.SetFace (Windows.Kinect.Body body, UnityEngine.GameObject bodyObject) (at Assets/KinectView/Scripts/msaw/FaceSourceView.cs:201)
/// FaceSourceView.Update () (at Assets/KinectView/Scripts/msaw/FaceSourceView.cs:143)
///
/// this error occures when you move super close to the canvas so i guess the head gets out of frame on top
/// or the kinect looses depth info when to close ...
/// </remarks>
ushort depth = _MultiManager.GetDepthData()[depthIndex];
int imageSize = (int)(FaceFrameSize / CalculatePixelWidth(depthDesc, depth));
int x = (int)(Mathf.Floor(ColorSpaceHead.X + 0.5F) - (imageSize / 2));
int y = (int)(Mathf.Floor(ColorSpaceHead.Y + 0.5F) - (imageSize / 2));
// _width = (float)imageSize;
// _height = (float)imageSize;
if (x > 0 && y > 0 && imageSize > 0 && x < _MultiManager.ColorWidth - imageSize && y < _MultiManager.ColorHeight - imageSize)
{
Texture2D colorSource = _MultiManager.GetColorTexture();
/// <remarks>
/// ERROR
/// Texture rectangle is out of bounds (867 + 263 > 1080)
/// UnityEngine.Texture2D:GetPixels(Int32, Int32, Int32, Int32)
/// FaceSourceView:SetFace(Body, GameObject) (at Assets/KinectView/Scripts/msaw/FaceSourceView.cs:224)
/// FaceSourceView:Update() (at Assets/KinectView/Scripts/msaw/FaceSourceView.cs:145)
///
/// tried to fix it with better if condition but did not help
/// && x <= _MultiManager.ColorWidth -(imageSize / 2) && y <= _MultiManager.ColorHeight -(imageSize / 2)
///
/// Texture rectangle is out of bounds (1859 + 72 > 1920)
/// tried to fix it now by removing the division on the imageSize
/// from -(imageSize / 2) to -(imageSize)
/// </remarks>
/// 1920x1080 color frame
/// 512x424 depth and infrared
Color[] pix = colorSource.GetPixels(x, y, imageSize, imageSize);
Texture2D destTex = new Texture2D(imageSize, imageSize);
destTex.SetPixels(pix);
destTex.Apply();
//gameObject.GetComponent<Renderer>().material.mainTexture = destTex;
/// <summary>
/// saving the destTex texture single frame to disk
/// </summary>
if (FaceImageNumber <= MaximumFaceImages)
{
if (_MonitorState._Display == MonitorState.Show.Nothing)
{
SaveTextureToFile(destTex, FaceImageNumber);
FaceImageNumber += 1;
}
}
else
{
//after images are saved tell the server to set the loading state
_MonitorState._Display = MonitorState.Show.StartLoading;
FaceImageNumber = 1;
//_MonitorState._Display = MonitorState.Show.FaceAnimation;
}
}
}
}
/// <summary>
/// Converts the specified ColorSpacePoint into a 3-D vector structure.
/// </summary>
/// <param name="point">The ColorSpacePoint to convert</param>
/// <returns>The corresponding 3-D vector.</returns>
public static Vector3D ToVector3(this ColorSpacePoint point)
{
return(new Vector3D(point.X, point.Y, 0.0));
}
/// <summary>
/// Updates the bitmap with new frame data.
/// </summary>
/// <param name="depthFrame">The specified depth frame.</param>
/// <param name="colorFrame">The specified color frame.</param>
/// <param name="bodyIndexFrame">The specified body index frame.</param>
public void Update(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
int colorWidth = colorFrame.FrameDescription.Width;
int colorHeight = colorFrame.FrameDescription.Height;
int depthWidth = depthFrame.FrameDescription.Width;
int depthHeight = depthFrame.FrameDescription.Height;
int bodyIndexWidth = bodyIndexFrame.FrameDescription.Width;
int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;
if (_displayPixels == null)
{
_depthData = new ushort[depthWidth * depthHeight];
_bodyData = new byte[depthWidth * depthHeight];
_colorData = new byte[colorWidth * colorHeight * Constants.BYTES_PER_PIXEL];
_displayPixels = new byte[depthWidth * depthHeight * Constants.BYTES_PER_PIXEL];
_colorPoints = new ColorSpacePoint[depthWidth * depthHeight];
Bitmap = new WriteableBitmap(depthWidth, depthHeight);
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();
}
}
static void reader_FrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
var reference = e.FrameReference.AcquireFrame();
using (var colorFrame = reference.ColorFrameReference.AcquireFrame())
{
if (colorFrame != null)
{
ColorFrameReady(colorFrame);
}
}
using (var bodyFrame = reference.BodyFrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
var _bodies = new Body[bodyFrame.BodyFrameSource.BodyCount];
bodyFrame.GetAndRefreshBodyData(_bodies);
_headFound = false;
foreach (var body in _bodies)
{
if (body.IsTracked)
{
Joint head = body.Joints[JointType.Head];
if (head.TrackingState == TrackingState.NotTracked)
continue;
_headFound = true;
_headPosition = Sensor.CoordinateMapper.MapCameraPointToColorSpace(head.Position);
}
}
UpdateZoomPosition();
}
}
}
/// <summary>
/// Generic function for filling display buffer and transforming depth data to colorspace
/// </summary>
/// <param name="sourceBuffer"></param>
/// <param name="outputBuffer"></param>
/// <param name="bodyData"></param>
/// <param name="colorPoints"></param>
public void Apply(ushort[] sourceBuffer, byte[] outputBuffer, byte[] bodyData, ColorSpacePoint [] colorPoints=null)
{
int displayIndex = 0;
for (int sourceIndex = 0; sourceIndex < sourceBuffer.Length; ++sourceIndex)
{
if (colorPoints != null)
{
ColorSpacePoint colorPoint = colorPoints[sourceIndex];
// make sure the depth pixel maps to a valid point in color space
if (float.IsInfinity(colorPoint.X) || float.IsInfinity(colorPoint.Y))
continue;
int colorX = (int)Math.Floor(colorPoint.X + 0.5);
int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX < 0) || (colorX >= _displayWidth) || (colorY < 0) || (colorY >= _displayHeight))
{
continue;
}
displayIndex = ((colorY * _displayWidth) + colorX) * _bytesPerPixel;
}
ApplyColor(outputBuffer, displayIndex, sourceBuffer, sourceIndex, bodyData);
if (colorPoints == null)
displayIndex += 4;
}
}
/// <summary>
/// action called by the anonymous event handler for every frame arriving
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
/// <param name="form"></param>
private static void MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e, Display form)
{
// get the current acquired frames
var multiFrames = e.FrameReference.AcquireFrame();
// automatic resource management with acquired color, body, depth frames
using (var color_frame = multiFrames.ColorFrameReference.AcquireFrame())
using (var body_frame = multiFrames.BodyFrameReference.AcquireFrame())
using (var depth_frame = multiFrames.DepthFrameReference.AcquireFrame())
{
// all the frames have to be acquired
if (color_frame != null && body_frame != null && depth_frame != null)
{
var color_frame_width = color_frame.FrameDescription.Width;
var color_frame_height = color_frame.FrameDescription.Height;
var color_frame_pixels = new byte[color_frame_width * color_frame_height * 4];
var bitmap = new Bitmap(color_frame_width, color_frame_height, PixelFormat.Format32bppRgb);
color_frame.CopyConvertedFrameDataToArray(color_frame_pixels, ColorImageFormat.Bgra);
var bitmapData = bitmap.LockBits(new Rectangle(0, 0, bitmap.Width, bitmap.Height),
ImageLockMode.WriteOnly, bitmap.PixelFormat);
Marshal.Copy(color_frame_pixels, 0, bitmapData.Scan0, color_frame_pixels.Length);
bitmap.UnlockBits(bitmapData);
// wrap the bitmap data into an image object(1920 x 1080)
Image <Bgr, byte> bgr = new Image <Bgr, byte>(bitmap);
// resize the image into 960 x 540
CvInvoke.Resize(bgr, display_kinect_image, new Size(640, 360));
//var depth_width = depth_frame.FrameDescription.Width;
//var depth_height = depth_frame.FrameDescription.Height;
//var depthData = new ushort[depth_width * depth_height];
//depth_frame.CopyFrameDataToArray(depthData);
var bodies = new Body[kinect_sensor.BodyFrameSource.BodyCount];
body_frame.GetAndRefreshBodyData(bodies);
var body = bodies.Where(bd => bd.IsTracked).FirstOrDefault();
form.Program_status = "Detecting Body";
if (!played_detecting_body)
{
form.play_audio(Properties.Resources.detecting_body);
played_detecting_body = true;
}
if (body != null)
{
form.Program_status = "Body Tracked";
head = body.Joints[JointType.Head].Position;
neck = body.Joints[JointType.Neck].Position;
shoulder_center = body.Joints[JointType.SpineShoulder].Position;
hip_center = body.Joints[JointType.SpineBase].Position;
wrist_left = body.Joints[JointType.WristLeft].Position;
wrist_right = body.Joints[JointType.WristRight].Position;
shoulder_left = body.Joints[JointType.ShoulderLeft].Position;
shoulder_right = body.Joints[JointType.ShoulderRight].Position;
elbow_left = body.Joints[JointType.ElbowLeft].Position;
hip_left = body.Joints[JointType.HipLeft].Position;
knee_left = body.Joints[JointType.KneeLeft].Position;
foot_left = body.Joints[JointType.FootLeft].Position;
elbow_right = body.Joints[JointType.ElbowRight].Position;
hip_right = body.Joints[JointType.HipRight].Position;
knee_right = body.Joints[JointType.KneeRight].Position;
foot_right = body.Joints[JointType.FootRight].Position;
//ankle_left = body.Joints[JointType.AnkleLeft].Position;
//ankle_right = body.Joints[JointType.AnkleRight].Position;
var body_joints = body.Joints.Values;
var body_joints_Trked = new Joint[body_joints.Count()];
var body_joints_Infrd = new Joint[body_joints.Count()];
var body_joints_CamSpacePts_Trked = new CameraSpacePoint[body_joints.Count()];
var body_joints_ClrSpacePts_Trked = new ColorSpacePoint[body_joints.Count()];
var body_joints_CamSpacePts_Infrd = new CameraSpacePoint[body_joints.Count()];
var body_joints_ClrSpacePts_Infrd = new ColorSpacePoint[body_joints.Count()];
var untracked_joints = new List <Joint>(0);
int i = 0, j = 0;
foreach (var joint in body_joints)
{
if (joint.TrackingState == TrackingState.Tracked)
{
body_joints_Trked[i] = joint;
body_joints_CamSpacePts_Trked[i] = joint.Position;
i++;
}
else if (joint.TrackingState == TrackingState.Inferred)
{
body_joints_Infrd[i] = joint;
body_joints_CamSpacePts_Infrd[j] = joint.Position;
j++;
}
else
{
untracked_joints.Add(joint);
}
}
kinect_sensor.CoordinateMapper.MapCameraPointsToColorSpace(body_joints_CamSpacePts_Trked, body_joints_ClrSpacePts_Trked);
kinect_sensor.CoordinateMapper.MapCameraPointsToColorSpace(body_joints_CamSpacePts_Infrd, body_joints_ClrSpacePts_Infrd);
DrawingHelper.draw_tracked_joints(body_joints_ClrSpacePts_Trked, display_kinect_image);
DrawingHelper.draw_inferred_joints(body_joints_ClrSpacePts_Infrd, display_kinect_image);
if (!HighKnees.ready_for_tutoring)
{
if (HighKnees.check_for_tutoring(form))
{
HighKnees.ready_for_tutoring = true;
}
}
else
{
if (!HighKnees.finished_first_quarter)
{
form.Program_status = "First-Quarter Tutoring";
HighKnees.first_quarter_tutoring(form);
}
else if (!HighKnees.finished_second_quarter)
{
form.Program_status = "Second-Quarter Tutoring";
HighKnees.second_quarter_tutoring(form);
}
else if (!HighKnees.finished_third_quarter)
{
form.Program_status = "Third-Quarter Tutoring";
form.Text_box_status = "Now, see if you can curl your right arm, raise your left knee and slightly bend your left arm simultaneously then switch to another side, which is similar";
if (!HighKnees.played_third_quarter_tutoring)
{
form.play_audio(Properties.Resources.third_quarter_tutoring);
HighKnees.played_third_quarter_tutoring = true;
}
HighKnees.third_quarter_tutoring(form);
if (HighKnees.finished_third_quarter)
{
System.Media.SystemSounds.Beep.Play();
}
}
else if (!HighKnees.finished_fourth_quarter)
{
form.Program_status = "Fourth-Quarter Tutoring";
HighKnees.fourth_quarter_tutoring(form);
if (HighKnees.finished_fourth_quarter)
{
System.Media.SystemSounds.Beep.Play();
}
}
else
{
form.Program_status = "Free-mode";
form.Text_box_status = "Now you are in exercising mode. Practice more to gain a better skill! Remember to alternate your arms back and forth.";
if (!HighKnees.played_free_mode)
{
form.play_audio(Properties.Resources.free_mode);
HighKnees.played_free_mode = true;
}
if (!HighKnees.finish_first_part_exercise)
{
if (HighKnees.third_quarter_tutoring(form))
{
HighKnees.finish_first_part_exercise = true;
HighKnees.count++;
System.Media.SystemSounds.Beep.Play();
form.Set_counter = HighKnees.count;
HighKnees.finish_second_part_exercise = false;
}
}
else if (!HighKnees.finish_second_part_exercise)
{
if (HighKnees.fourth_quarter_tutoring(form))
{
HighKnees.finish_second_part_exercise = true;
HighKnees.count++;
System.Media.SystemSounds.Beep.Play();
form.Set_counter = HighKnees.count;
HighKnees.finish_first_part_exercise = false;
}
}
}
}
}
else
{
HighKnees.ready_for_tutoring = false;
HighKnees.played_check_for_tutoring = false;
}
}
form.setImage = display_kinect_image;
}
}
public static Point GetPoint(this ColorSpacePoint colorSpacePoint)
{
return(new Point(colorSpacePoint.X, colorSpacePoint.Y));
}
// TODO: don't sent alpha
private void ProcessPointCloudColors()
{
byte[] pointCloudColors = new byte[depthArray.Length * 4];
ColorSpacePoint[] colorSpacePoints = new ColorSpacePoint[depthArray.Length];
var colorWidth = ColorFrameDescription.Width;
var colorHeight = ColorFrameDescription.Height;
CoordinateMapper.MapDepthFrameToColorSpace(depthArray, colorSpacePoints);
float maxX = 0;
float maxY = 0;
for (int i = 0; i < depthArray.Length; i++)
{
var point = colorSpacePoints[i];
if (GeometryHelper.IsValidPoint(point))
{
maxX = point.X > maxX ? point.X : maxX;
maxY = point.Y > maxY ? point.Y : maxY;
int colorX = (int) Math.Floor(point.X + 0.5);
int colorY = (int)Math.Floor(point.Y + 0.5);
if ((colorX > 0 && colorX < colorWidth) && (colorY > 0 && colorY < colorHeight))
{
int index = (int)(((colorWidth*colorY)+colorX) * 4);
pointCloudColors[i * 4] = colorPixels[index];
pointCloudColors[i * 4 + 1] = colorPixels[index + 1];
pointCloudColors[i * 4 + 2] = colorPixels[index + 2];
pointCloudColors[i * 4 + 3] = colorPixels[index + 3];
}
//int index = ((int)point.X + DepthFrameDescription.Width / 2) +
// ((int)point.Y + DepthFrameDescription.Height / 2) * DepthFrameDescription.Width;
//int index = (int)((point.X + DepthFrameDescription.Width / 2) * ColorFrameDescription.BytesPerPixel +
// (point.Y+ DepthFrameDescription.Height/2) * stride);
}
}
coloredPointCloudStreamMessage = new ColoredPointCloudStreamMessage(FullPointCloud, pointCloudColors);
}
private void ProcessDepthMapping(MultiFrame multiFrame)
{
// if depth data was not run
if (multiFrame.DepthBytes == null) multiFrame.DepthBytes = new byte[depthWidth * depthHeight * 4 * 2];
// --- depth mapping from kinect
long ticks1 = DateTime.Now.Ticks;
ColorSpacePoint[] depthMapping = new ColorSpacePoint[depthWidth * depthHeight];
coordMapper.MapDepthFrameToColorSpace(multiFrame.DepthData, depthMapping);
Utils.UpdateTimer("(DepthMappingCoord)", ticks1);
// --- mapped depthAsColor -> color
long ticks2 = DateTime.Now.Ticks;
unsafe
{
fixed (byte* fixedDepthMapped = multiFrame.DepthBytes)
fixed (byte* fixedColorData = multiFrame.ColorData)
{
byte* ptrDepthMapped = fixedDepthMapped;
ptrDepthMapped += depthByteSize * 4; // joined
byte* ptrColorData = fixedColorData;
int countMapped = 0;
int countNotMapped = 0;
for (int i = 0; i < depthByteSize; i++)
{
// checking infinity before adding + 0.5f is about 5x faster (!!)
float xTmp = depthMapping[i].X;
float yTmp = depthMapping[i].Y;
int x = float.IsInfinity(xTmp) ? -1 : (int)(xTmp + 0.5f);
int y = float.IsInfinity(yTmp) ? -1 : (int)(yTmp + 0.5f);
if (x >= 0 && x < colorWidth && y >= 0 && y < colorHeight)
{
int idxBase = x * 4 + y * colorWidth * 4;
*ptrDepthMapped++ = ptrColorData[idxBase];
*ptrDepthMapped++ = ptrColorData[idxBase + 1];
*ptrDepthMapped++ = ptrColorData[idxBase + 2];
*ptrDepthMapped++ = ptrColorData[idxBase + 3];
countMapped++;
}
else
{
*ptrDepthMapped++ = 0;
*ptrDepthMapped++ = 0;
*ptrDepthMapped++ = 0;
*ptrDepthMapped++ = 0;
countNotMapped++;
}
}
}
}
Utils.UpdateTimer("(DepthMappingBytes)", ticks2);
}
private void MapColorToDepth(ushort[] depthFrameData)
{
if (rawColorBitmap != null)
{
var pixelBytes = colorFrameDescription.BytesPerPixel;
byte[] rawColorPixels = new byte[rawColorBitmap.PixelHeight * rawColorBitmap.PixelWidth * pixelBytes];
rawColorBitmap.CopyPixels(rawColorPixels, rawColorBitmap.BackBufferStride, 0);
byte[] colorPixels = new byte[colorBitmap.PixelHeight * colorBitmap.PixelWidth * pixelBytes];
ColorSpacePoint[] colorPoints = new ColorSpacePoint[depthFrameDescription.Height * depthFrameDescription.Width];
kinectSensor.CoordinateMapper.MapDepthFrameToColorSpace(depthFrameData, colorPoints);
for (var id = 0; id < colorPoints.Length; id++)
{
if (colorPoints[id].X < 0 || colorPoints[id].X >= colorFrameDescription.Width ||
colorPoints[id].Y < 0 || colorPoints[id].Y >= colorFrameDescription.Height)
continue;
for (var i = 0; i < pixelBytes; i++)
colorPixels[id * pixelBytes + i] =
rawColorPixels[(Int32)colorPoints[id].Y * rawColorBitmap.PixelWidth * pixelBytes + (Int32)colorPoints[id].X * pixelBytes + i];
}
colorBitmap.WritePixels(new Int32Rect(0, 0, colorBitmap.PixelWidth, colorBitmap.PixelHeight),
colorPixels, colorBitmap.BackBufferStride, 0);
colorBitmap.Lock();
colorBitmap.AddDirtyRect(new Int32Rect(0, 0, colorBitmap.PixelWidth, colorBitmap.PixelHeight));
colorBitmap.Unlock();
}
}
/// <summary>
/// Update the joint position based on the referenced <c>IJoint</c>.
/// </summary>
public virtual void Update(IJoint joint)
{
if (this.JointType != joint.JointType)
throw new Exception("Cannot Update with Joint of a different Type");
_position = joint.Position;
_depthPosition = new DepthSpacePoint();
_colorPosition = new ColorSpacePoint();
_trackingState = joint.TrackingState;
}
/// <summary>
/// Initializes a new instance of the MainWindow class.
/// </summary>
public MainWindow()
{
//Depth pixe> FILE
string winDir = System.Environment.GetEnvironmentVariable("windir");
writer.WriteLine("START");
// get the kinectSensor object
this.kinectSensor = KinectSensor.GetDefault();
// get the coordinate mapper
coordinateMapper = this.kinectSensor.CoordinateMapper;
// open the reader for the depth frames
this.depthFrameReader = this.kinectSensor.DepthFrameSource.OpenReader();
this.bodyFrameReader = this.kinectSensor.BodyFrameSource.OpenReader();
// wire handler for frame arrival
this.depthFrameReader.FrameArrived += this.Depth_Reader_FrameArrived;
if (this.bodyFrameReader != null)
{
Console.Write("yO");
this.bodyFrameReader.FrameArrived += this.Body_Reader_FrameArrived;
}
// get FrameDescription from DepthFrameSource
this.depthFrameDescription = this.kinectSensor.DepthFrameSource.FrameDescription;
// get size of joint space
this.displayWidth = depthFrameDescription.Width;
this.displayHeight = depthFrameDescription.Height;
// open the reader for the body frames
this.bodyFrameReader = this.kinectSensor.BodyFrameSource.OpenReader();
// allocate space to put the pixels being received and converted
this.depthPixels = new byte[this.depthFrameDescription.Width * this.depthFrameDescription.Height];
this.hist = new int[this.depthPixels.Length];
// create the bitmap to display
this.depthBitmap = new WriteableBitmap(this.depthFrameDescription.Width, this.depthFrameDescription.Height, 96.0, 96.0, PixelFormats.Gray8, null);
// set IsAvailableChanged event notifier
this.kinectSensor.IsAvailableChanged += this.Sensor_IsAvailableChanged;
// open the sensor
this.kinectSensor.Open();
// set the status text
this.StatusText = this.kinectSensor.IsAvailable ? Properties.Resources.RunningStatusText
: Properties.Resources.NoSensorStatusText;
// use the window object as the view model in this simple example
this.DataContext = this;
this.ctr = 0;
this.handpoint = coordinateMapper.MapCameraPointToColorSpace(lefthandpoint);
this.lefthandpoint = this.lefthand.Position;
// initialize the components (controls) of the window
this.InitializeComponent();
}
public static bool IsValidPoint(ColorSpacePoint point)
{
return IsNumber(point.X) && IsNumber(point.Y);
}
private void DrawPoint(WriteableBitmap bitmap, ColorSpacePoint p)
{
bitmap.DrawLineAa(Math.Max((int)p.X - 5, 0), Math.Max((int)p.Y - 5, 0), Math.Min((int)p.X + 5, colorWidth - 1), Math.Min((int)p.Y + 5, colorHeight - 1), Colors.Cyan, 20);
}
private async Task DrawDepthCoodinate()
{
// Depth画像の解像度でデータを作る
var colorImageBuffer = new byte[depthFrameDesc.LengthInPixels *
colorFrameDesc.BytesPerPixel];
// 変換処理が重いため、非同期に行う
await Task.Factory.StartNew( () =>
{
// Depth座標系に対応するカラー座標系の一覧を取得する
var colorSpace = new ColorSpacePoint[depthFrameDesc.LengthInPixels];
kinect.CoordinateMapper.MapDepthFrameToColorSpace( depthBuffer, colorSpace );
// 並列で処理する
Parallel.For( 0, depthFrameDesc.LengthInPixels, i =>
{
int colorX = (int)colorSpace[i].X;
int colorY = (int)colorSpace[i].Y;
if ( (colorX < 0) || (colorFrameDesc.Width <= colorX) ||
(colorY < 0) || (colorFrameDesc.Height <= colorY) ) {
return;
}
// カラー座標系のインデックス
int colorIndex = (colorY * colorFrameDesc.Width) + colorX;
int bodyIndex = bodyIndexBuffer[i];
// 人を検出した位置だけ色を付ける
if ( bodyIndex == 255 ) {
return;
}
// カラー画像を設定する
int colorImageIndex = (int)(i * colorFrameDesc.BytesPerPixel);
int colorBufferIndex = (int)(colorIndex * colorFrameDesc.BytesPerPixel);
colorImageBuffer[colorImageIndex + 0] = colorBuffer[colorBufferIndex + 0];
colorImageBuffer[colorImageIndex + 1] = colorBuffer[colorBufferIndex + 1];
colorImageBuffer[colorImageIndex + 2] = colorBuffer[colorBufferIndex + 2];
} );
} );
// ビットマップにする
var colorBitmap = new WriteableBitmap( depthFrameDesc.Width,
depthFrameDesc.Height );
var stream = colorBitmap.PixelBuffer.AsStream();
stream.Write( colorImageBuffer, 0, colorImageBuffer.Length );
colorBitmap.Invalidate();
ImageColor.Source = colorBitmap;
}
static void Main()
{
Application.EnableVisualStyles();
Application.SetCompatibleTextRenderingDefault(false);
RenderForm form = new RenderForm("Kinect hd face sample with color map");
RenderDevice device = new RenderDevice(SharpDX.Direct3D11.DeviceCreationFlags.BgraSupport | DeviceCreationFlags.Debug);
RenderContext context = new RenderContext(device);
DX11SwapChain swapChain = DX11SwapChain.FromHandle(device, form.Handle);
VertexShader vertexShader = ShaderCompiler.CompileFromFile<VertexShader>(device, "ColorHdFaceView.fx", "VS");
PixelShader pixelShader = ShaderCompiler.CompileFromFile<PixelShader>(device, "ColorHdFaceView.fx", "PS");
HdFaceIndexBuffer faceIndexBuffer = new HdFaceIndexBuffer(device, 1);
DynamicHdFaceStructuredBuffer faceVertexBuffer = new DynamicHdFaceStructuredBuffer(device, 1);
DynamicRgbSpaceFaceStructuredBuffer faceRgbBuffer = new DynamicRgbSpaceFaceStructuredBuffer(device, 1);
KinectSensor sensor = KinectSensor.GetDefault();
sensor.Open();
cbCamera camera = new cbCamera();
camera.Projection = Matrix.PerspectiveFovLH(1.57f * 0.5f, 1.3f, 0.01f, 100.0f);
camera.View = Matrix.Translation(0.0f, 0.0f, 0.5f);
camera.Projection.Transpose();
camera.View.Transpose();
ConstantBuffer<cbCamera> cameraBuffer = new ConstantBuffer<cbCamera>(device);
cameraBuffer.Update(context, ref camera);
bool doQuit = false;
bool doUpload = false;
KinectBody[] bodyFrame = null;
KinectSensorBodyFrameProvider provider = new KinectSensorBodyFrameProvider(sensor);
form.KeyDown += (sender, args) => { if (args.KeyCode == Keys.Escape) { doQuit = true; } };
FaceModel currentFaceModel = new FaceModel();
FaceAlignment currentFaceAlignment = new FaceAlignment();
SingleHdFaceProcessor hdFaceProcessor = new SingleHdFaceProcessor(sensor);
hdFaceProcessor.HdFrameReceived += (sender, args) => { currentFaceModel = args.FaceModel; currentFaceAlignment = args.FaceAlignment; doUpload = true; };
bool uploadColor = false;
ColorRGBAFrameData currentData = null;
DynamicColorRGBATexture colorTexture = new DynamicColorRGBATexture(device);
KinectSensorColorRGBAFrameProvider colorProvider = new KinectSensorColorRGBAFrameProvider(sensor);
colorProvider.FrameReceived += (sender, args) => { currentData = args.FrameData; uploadColor = true; };
provider.FrameReceived += (sender, args) =>
{
bodyFrame = args.FrameData;
var body = bodyFrame.TrackedOnly().ClosestBodies().FirstOrDefault();
if (body != null)
{
hdFaceProcessor.AssignBody(body);
}
else
{
hdFaceProcessor.Suspend();
}
};
context.Context.Rasterizer.State = device.RasterizerStates.WireFrame;
RenderLoop.Run(form, () =>
{
if (doQuit)
{
form.Dispose();
return;
}
if (doUpload)
{
var vertices = currentFaceModel.CalculateVerticesForAlignment(currentFaceAlignment).ToArray();
var vertRgb = new ColorSpacePoint[vertices.Length];
sensor.CoordinateMapper.MapCameraPointsToColorSpace(vertices, vertRgb);
faceVertexBuffer.Copy(context, vertices);
faceRgbBuffer.Copy(context, vertRgb);
doUpload = false;
}
if (uploadColor)
{
colorTexture.Copy(context, currentData);
uploadColor = false;
}
context.Context.ClearRenderTargetView(swapChain.RenderView, SharpDX.Color.Black);
if (hdFaceProcessor.IsValid)
{
context.RenderTargetStack.Push(swapChain);
context.Context.VertexShader.SetShaderResource(0, faceVertexBuffer.ShaderView);
context.Context.VertexShader.SetShaderResource(1, faceRgbBuffer.ShaderView);
context.Context.VertexShader.SetConstantBuffer(0, cameraBuffer.Buffer);
context.Context.PixelShader.SetShaderResource(0, colorTexture.ShaderView);
context.Context.PixelShader.SetSampler(0, device.SamplerStates.LinearClamp);
//Draw lines
context.Context.PixelShader.Set(pixelShader);
context.Context.VertexShader.Set(vertexShader);
//Attach index buffer, null topology since we fetch
faceIndexBuffer.AttachWithLayout(context);
faceIndexBuffer.Draw(context, 1);
context.RenderTargetStack.Pop();
}
swapChain.Present(0, SharpDX.DXGI.PresentFlags.None);
});
swapChain.Dispose();
context.Dispose();
device.Dispose();
colorProvider.Dispose();
colorTexture.Dispose();
cameraBuffer.Dispose();
faceIndexBuffer.Dispose();
faceVertexBuffer.Dispose();
faceRgbBuffer.Dispose();
provider.Dispose();
pixelShader.Dispose();
vertexShader.Dispose();
sensor.Close();
}
private void RefreshData(ushort[] depthData, int colorWidth, int colorHeight)
{
var frameDesc = _Sensor.DepthFrameSource.FrameDescription;
ColorSpacePoint[] colorSpace = new ColorSpacePoint[depthData.Length];
CameraSpacePoint[] cameraSpace = new CameraSpacePoint[depthData.Length];
_Mapper.MapDepthFrameToColorSpace(depthData, colorSpace);
_Mapper.MapDepthFrameToCameraSpace(depthData, cameraSpace);
for (int y = 0; y < frameDesc.Height; y += _DownsampleSize)
{
for (int x = 0; x < frameDesc.Width; x += _DownsampleSize)
{
int indexX = x / _DownsampleSize;
int indexY = y / _DownsampleSize;
int smallIndex = (indexY * (frameDesc.Width / _DownsampleSize)) + indexX;
int bigIndex = y * frameDesc.Width + x;
_Vertices[smallIndex].x = -cameraSpace[bigIndex].X;
_Vertices[smallIndex].y = cameraSpace[bigIndex].Y;
_Vertices[smallIndex].z = cameraSpace[bigIndex].Z;
// Update UV mapping with CDRP
var colorSpacePoint = colorSpace[bigIndex];
_UV[smallIndex] = new Vector2(colorSpacePoint.X / colorWidth, colorSpacePoint.Y / colorHeight);
}
}
_Mesh.vertices = _Vertices;
_Mesh.uv = _UV;
_Mesh.triangles = _Triangles;
_Mesh.RecalculateNormals();
bool sendMesh = false;
lock (readyToSendLock)
{
sendMesh = readyToSendMesh;
}
if (sendMesh)
{
var depthWidth = frameDesc.Width / _DownsampleSize;
var depthHeight = frameDesc.Height / _DownsampleSize;
var depthCount = depthWidth * depthHeight;
// Breite und Höhe Tiefendaten schicken
SendDataToClient(BitConverter.GetBytes(depthWidth));
SendDataToClient(BitConverter.GetBytes(depthHeight));
// Punktdaten und UV-Daten sammeln und als einzelne Nachricht schicken
// 5 Floats (X, Y, Z, U, V)
int byteCount = 5 * sizeof(float) * depthCount;
byte[] dataToSend = new byte[byteCount];
for (int i = 0; i < depthCount; i++)
{
Buffer.BlockCopy(BitConverter.GetBytes(_Vertices[i].x), 0, dataToSend, i * 5 * sizeof(float), sizeof(float));
Buffer.BlockCopy(BitConverter.GetBytes(_Vertices[i].y), 0, dataToSend, (i * 5 + 1) * sizeof(float), sizeof(float));
Buffer.BlockCopy(BitConverter.GetBytes(_Vertices[i].z), 0, dataToSend, (i * 5 + 2) * sizeof(float), sizeof(float));
Buffer.BlockCopy(BitConverter.GetBytes(_UV[i].x), 0, dataToSend, (i * 5 + 3) * sizeof(float), sizeof(float));
Buffer.BlockCopy(BitConverter.GetBytes(_UV[i].y), 0, dataToSend, (i * 5 + 4) * sizeof(float), sizeof(float));
}
SendDataToClient(dataToSend.ToArray());
// Farbdaten mittels JPG komprimieren und als einzelne Nachricht schicken
byte[] colorDataCopy = new byte[colorData.Length];
byte[] jpgData;
Buffer.BlockCopy(colorData, 0, colorDataCopy, 0, colorData.Length);
// Der nachfolgende Code zur JPG-Kompression vertauscht Rot- und Blaukanal, deswegen tauschen wir ihn hier auch schonmal
for (int y = 0; y < colorHeight; y++)
{
for (int x = 0; x < colorWidth; x++)
{
int index = y * colorWidth * 4 + x * 4;
// Vertausche erstes Byte (Rot) und drittes Byte (Blau)
byte temp = colorDataCopy[index];
colorDataCopy[index] = colorDataCopy[index + 2];
colorDataCopy[index + 2] = temp;
}
}
try
{
// Quelle: https://stackoverflow.com/questions/11730373/byte-array-to-bitmap-image
Bitmap bitmap = new Bitmap(colorWidth, colorHeight, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
BitmapData bmData = bitmap.LockBits(new Rectangle(0, 0, colorWidth, colorHeight), ImageLockMode.ReadWrite, bitmap.PixelFormat);
Marshal.Copy(colorDataCopy, 0, bmData.Scan0, colorWidth * colorHeight * 4);
bitmap.UnlockBits(bmData);
using (var ms = new MemoryStream())
{
bitmap.Save(ms, ImageFormat.Jpeg);
jpgData = ms.ToArray();
textInGui.text = "JPG length: " + jpgData.Length;
// Länge der Daten schicken
SendDataToClient(BitConverter.GetBytes(jpgData.Length));
SendDataToClient(jpgData);
}
lock (readyToSendLock)
{
readyToSendMesh = false;
}
} catch (Exception e)
{
textInGui.text = e.Message;
}
}
}
unsafe void UpdateFusionFrame()
{
//KinectのDepthImageデータからFusionのDepthImageFrameを作成
reconstruction.DepthToDepthFloatFrame( depthImagePixels, depthImageFrame,
FusionDepthProcessor.DefaultMinimumDepth, FusionDepthProcessor.DefaultMaximumDepth, true );
//depthImageFrameに平滑化をかけてsmoothDepthFrameに格納
reconstruction.SmoothDepthFloatFrame( depthImageFrame, smoothDepthImageFrame, SmoothingKernelWidth, SmoothingDistanceThreshold );
//色情報を取得するためにDepthImage各点のColorImageでの位置を計算
ColorSpacePoint[] points = new ColorSpacePoint[depthWidth * depthHeight];
coordinateMapper.MapDepthFrameToColorSpace( depthImagePixels, points );
//colorImageFrameBufferに色情報を格納
int[] colorImageFrameBuffer = new int[depthWidth * depthHeight];
fixed ( byte* ptrColorImagePixels = colorImagePixels ) {
int* rawColorPixels = (int*)ptrColorImagePixels;
Parallel.For( 0, depthHeight,
y =>
{
for ( int x = 0; x < depthWidth; x++ ) {
int index = y * depthWidth + x;
ColorSpacePoint point = points[index];
int colorX = (int)(Math.Ceiling( point.X ));
int colorY = (int)(Math.Ceiling( point.Y ));
if ( (colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight) ) {
colorImageFrameBuffer[index] = rawColorPixels[colorY * colorWidth + colorX];
}
}
} );
}
//colorImageFrameBufferからFusionのcolorImageFrameを作成
colorImageFrame.CopyPixelDataFrom( colorImageFrameBuffer );
//現在のカメラ位置や角度の情報を計算に反映させるため、worldToCameraTransformを最新に更新
worldToCameraTransform = reconstruction.GetCurrentWorldToCameraTransform();
//最新のsmoothDepthImageFrame,colorImageFrameをFusionのReconstructionオブジェクトに反映
float alignmentEnergy = 0.0f;
bool ret = reconstruction.ProcessFrame( smoothDepthImageFrame, colorImageFrame, FusionDepthProcessor.DefaultAlignIterationCount,
FusionDepthProcessor.DefaultIntegrationWeight, FusionDepthProcessor.DefaultColorIntegrationOfAllAngles,
out alignmentEnergy, worldToCameraTransform );
//反映結果がエラーならカウントし、カウンタが100を越えたら初めからやり直す
if ( !ret ) {
if ( ++errorCount>=100 ) {
errorCount = 0;
reset();
}
}
//結果を格納
result();
}
/// <summary>
/// Handles the arrival of each skeleton frame
/// </summary?
private void Reader_FrameArrivedSkel(object sender, BodyFrameArrivedEventArgs e)
{
BodyFrameReference frameReference = e.FrameReference;
try
{
BodyFrame frame = frameReference.AcquireFrame();
if (frame != null)
{
// BodyFrame is IDisposable
using (frame)
{
this.framesSinceUpdate++;
using (DrawingContext dc = this.drawingGroup.Open())
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Transparent, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
// The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
// As long as those body objects are not disposed and not set to null in the array,
// those body objects will be re-used.
frame.GetAndRefreshBodyData(this.bodies);
foreach (Body body in this.bodies)
{
if (body.IsTracked)
{
// this.DrawClippedEdges(body, dc);
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
// convert the joint points to depth (display) space
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
ColorSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToColorSpace(joints[jointType].Position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
this.DrawBody(joints, jointPoints, dc);
this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
//save the skel coords if the option is selected
if (skelRecO == true)
{
long milliseconds = DateTime.Now.Ticks / TimeSpan.TicksPerMillisecond;
saveSkelInformation(milliseconds, body);
saveTimeStamp("time_skel", milliseconds);
//Save current frame time stamp
}
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
}
}
}
catch (Exception ex)
{
Console.WriteLine(ex.Message);
// ignore if the frame is no longer available
}
}
/// <summary>
/// Calculates the angle and updates the arc according to the specifed points.
/// </summary>
/// <param name="start">The starting point.</param>
/// <param name="middle">The middle point.</param>
/// <param name="end">The end point.</param>
/// <param name="desiredRadius">The desired arc radius.</param>
public void Update(ColorSpacePoint start, ColorSpacePoint middle, ColorSpacePoint end, double desiredRadius = 0)
{
Update(start.ToVector3(), middle.ToVector3(), end.ToVector3(), desiredRadius);
}
/// <summary>
/// Calculates the angle and updates the arc according to the specifed points.
/// </summary>
/// <param name="start">The starting point.</param>
/// <param name="middle">The middle point.</param>
/// <param name="end">The end point.</param>
/// <param name="desiredRadius">The desired arc radius.</param>
public void Update(ColorSpacePoint start, ColorSpacePoint middle, ColorSpacePoint end, double desiredRadius = 0)
{
Update(start.ToVector3(), middle.ToVector3(), end.ToVector3(), desiredRadius);
}
private unsafe void getSkinColor(ColorSpacePoint leftHand, ColorSpacePoint rightHand)
{
byte bl, gl, rl, al, br, gr, rr, ar;
int handLeftOffset = ((colorFrameDescription.Width * ((int)leftHand.Y) + ((int)leftHand.X)) * BytesPerPixel);
int handRightOffset = ((colorFrameDescription.Width * ((int)rightHand.Y) + ((int)rightHand.X)) * BytesPerPixel);
if (handLeftOffset > 0 && handRightOffset > 0 && handLeftOffset < 8294400 && handRightOffset < 8294400)
{
bl = colorFrameData[handLeftOffset];
gl = colorFrameData[handLeftOffset + 1];
rl = colorFrameData[handLeftOffset + 2];
al = colorFrameData[handLeftOffset + 3];
br = colorFrameData[handRightOffset];
gr = colorFrameData[handRightOffset + 1];
rr = colorFrameData[handRightOffset + 2];
ar = colorFrameData[handRightOffset + 3];
var color = new System.Windows.Media.Color();
color.B = (byte)((bl + br) / 2);
color.G = (byte)((gl + gr) / 2);
color.R = (byte)((rl + rr) / 2);
color.A = (byte)((al + ar) / 2);
skinColorHue = System.Drawing.Color.FromArgb(color.A, color.R, color.G, color.B).GetHue();
this.ellipseSkinColor.Fill = new System.Windows.Media.SolidColorBrush(color);
}
}
private void DrawWorksapces(DrawingContext dc)
{
CoordinateMapper coordinateMapper = kinectStreamer.CoordinateMapper;
foreach (Workspace workspace in WorkspaceList)
{
Pen pen = WorkspaceActiveMap[workspace] ? activeWorkspacePen : passiveWorkspacePen;
CameraSpacePoint p0 = Point3DtoCameraSpacePoint(workspace.FittedVertices[0]);
CameraSpacePoint p1 = Point3DtoCameraSpacePoint(workspace.FittedVertices[1]);
CameraSpacePoint p2 = Point3DtoCameraSpacePoint(workspace.FittedVertices[2]);
CameraSpacePoint p3 = Point3DtoCameraSpacePoint(workspace.FittedVertices[3]);
ColorSpacePoint[] colorPoints = new ColorSpacePoint[4];
coordinateMapper.MapCameraPointsToColorSpace(new[] { p0, p1, p2, p3 }, colorPoints);
for (int i = 0; i < colorPoints.Length; i++)
{
dc.DrawLine(pen,
new Point(colorPoints[i % colorPoints.Length].X, colorPoints[i % colorPoints.Length].Y),
new Point(colorPoints[(i + 1) % colorPoints.Length].X, colorPoints[(i + 1) % colorPoints.Length].Y));
}
}
}
private unsafe bool isSkinColor(ColorSpacePoint point)
{
int offset = ((colorFrameDescription.Width * ((int)point.Y) + ((int)point.X)) * BytesPerPixel);
if (offset > 0 && offset < 8294400)
{
byte b = colorFrameData[offset];
byte g = colorFrameData[offset + 1];
byte r = colorFrameData[offset + 2];
byte a = colorFrameData[offset + 3];
var color = System.Drawing.Color.FromArgb(r, g, b);
var hue = color.GetHue(); //290 - 340
//var sat = color.GetSaturation(); //0-100
//var bri = color.GetBrightness(); //0-100
//if (hue > skinColorHue - colorThreshold && hue < skinColorHue + colorThreshold)
if (hue > 280 && hue < 350)
return true;
return false;
}
return false;
}
/// <summary>
/// Converts the specified ColorSpacePoint into a 2-D point.
/// </summary>
/// <param name="position">The ColorSpacePoint to convert.</param>
/// <returns>The corresponding 2-D point.</returns>
public static Point ToPoint(this ColorSpacePoint position)
{
return(new Point(position.X, position.Y));
}
/// <summary>
/// Draws a body joint
/// </summary>
/// <param name="joint">Joint of the body</param>
/// <param name="radius">Circle radius</param>
/// <param name="fill">Fill color</param>
/// <param name="borderWidth">Thickness of the border</param>
/// <param name="border">Color of the boder</param>
private void DrawJoint(Joint joint, double radius, SolidColorBrush fill, double borderWidth, SolidColorBrush border)
{
if (joint.TrackingState != TrackingState.Tracked)
{
return;
}
// Map the CameraPoint to ColorSpace so they match
ColorSpacePoint colorPoint = _kinect.CoordinateMapper.MapCameraPointToColorSpace(joint.Position);
// Create the UI element based on the parameters
Ellipse el = new Ellipse();
el.Fill = fill;
el.Stroke = border;
el.StrokeThickness = borderWidth;
el.Width = el.Height = radius;
Ellipse cb = new Ellipse();
cb.Fill = fill;
cb.Stroke = border;
el.StrokeThickness = borderWidth;
cb.Height = radius;
cb.Width = radius;
//hand
Ellipse te = new Ellipse();
te.Fill = fill;
te.Stroke = border;
te.Height = radius;
te.Width = radius;
te.Opacity = 0.25f;
TextBlock tb = new TextBlock();
tb.Text = joint.Position.Y.ToString();
tb.FontSize = 34;
tb.Width = cb.Width;
tb.Height = cb.Height;
//bar path
if (trackingBarPath)
{
Canvas.SetLeft(te, colorPoint.X / 2 - radius / 2);
Canvas.SetTop(te, colorPoint.Y / 2 - radius / 2);
if (recording)
{
myCanvas.Children.Add(te);
}
if (statMap["feetDistance"])
{
distanceText.Text = "INIT: " + Math.Round(feetData[0], 2).ToString() + " " +
Math.Round(feetData[1], 2).ToString() + " " +
Math.Round(feetData[2], 2).ToString() + " CURRENT: " +
Math.Round(feetDistance, 2).ToString() + " " +
Math.Round(distanceInFront, 2).ToString() + " " +
Math.Round(distanceInBack, 2).ToString() + " r: " +
Math.Round(Math.Abs(feetData[0] - feetDistance) / feetData[0], 2) * 100 + "%, " +
Math.Round(Math.Abs(feetData[1] - distanceInFront) / feetData[1], 2) * 100 + "%, " +
Math.Round(Math.Abs(feetData[2] - distanceInBack) / feetData[2], 2) * 100 + "%";
}
testWindow.Content = myCanvas;
testWindow.Show();
}
// Add the Ellipse to the canvas
//SkeletonCanvas.Children.Add(el);
SkeletonCanvas.Children.Add(cb);
//SkeletonCanvas.Children.Add(tb);
// Avoid exceptions based on bad tracking
if (float.IsInfinity(colorPoint.X) || float.IsInfinity(colorPoint.X))
{
return;
}
// Allign ellipse on canvas (Divide by 2 because image is only 50% of original size)
//Canvas.SetLeft(el, colorPoint.X / 2);
//Canvas.SetTop(el, colorPoint.Y / 2);
//if (joint.JointType == JointType.FootLeft)
//{
// //TextBlock tb = new TextBlock();
// //tb.Text = joint.Position.Y.ToString();
// //tb.FontSize = 34;
// //tb.Width = cb.Width;
// //tb.Height = cb.Height;
// Line line = new Line();
// line.X1 = colorPoint.X / 2 - radius / 2;
// line.Y1 = colorPoint.Y / 2 - radius / 2;
// line.X2 = colorPoint.X / 2 - radius / 2;
// line.Y2 = colorPoint.Y / 2 - radius / 2;
// line.StrokeThickness = 50;
// SolidColorBrush color = new SolidColorBrush();
// color.Color = Colors.Yellow;
// line.Stroke = color;
// SkeletonCanvas.Children.Add(line);
//}
Canvas.SetLeft(cb, colorPoint.X / 2 - radius / 2);
Canvas.SetTop(cb, colorPoint.Y / 2 - radius / 2);
}
/// <summary>
/// Calculates the angle between the specified points.
/// </summary>
/// <param name="center">The center of the angle.</param>
/// <param name="start">The start of the angle.</param>
/// <param name="end">The end of the angle.</param>
/// <returns>The angle, in degrees.</returns>
public static double AngleBetween(this ColorSpacePoint center, ColorSpacePoint start, ColorSpacePoint end)
{
Vector3D first = start.ToVector3() - center.ToVector3();
Vector3D second = end.ToVector3() - center.ToVector3();
return(Vector3D.AngleBetween(first, second));
}
public (Texture2D, Texture2D, DepthSpacePoint[], bool) CreateMapperTextures(Matrix projMatrix, Viewport viewport)
{
var w = viewport.Width;
var h = viewport.Height;
var colorPoints = new ColorSpacePoint[w * h];
var depthPoints = new DepthSpacePoint[w * h];
var colorMapper = new Texture2D(gdevice, w, h, false, SurfaceFormat.Vector2); // two channel float32
var depthMapper = new Texture2D(gdevice, w, h, false, SurfaceFormat.Vector2);
var targetProjPlane = viewport.Project(new Vector3(0, 0, TARGET_PLANE_M),
projMatrix, Matrix.Identity, Matrix.Identity).Z;
var cameraPoints = new CameraSpacePoint[w * h];
for (int y = 0; y < h; y++)
{
for (int x = 0; x < w; x++)
{
var point = viewport.Unproject(new Vector3(x, y, targetProjPlane),
projMatrix, Matrix.Identity, Matrix.Identity);
cameraPoints[y * w + x] = new CameraSpacePoint
{
X = point.X,
Y = point.Y,
Z = -point.Z
};
}
}
//sometimes the coordinate mapper returns invalid buffers, retry until a valid buffer is returned
int colorTimeoutCounter = 0;
do
{
kinect.CoordinateMapper.MapCameraPointsToColorSpace(cameraPoints, colorPoints);
colorTimeoutCounter++;
} while (float.IsInfinity(colorPoints[w * h / 2 + w / 2].X) && colorTimeoutCounter < 20);
int depthTimeoutCounter = 0;
do
{
kinect.CoordinateMapper.MapCameraPointsToDepthSpace(cameraPoints, depthPoints);
depthTimeoutCounter++;
} while (float.IsInfinity(depthPoints[w * h / 2 + w / 2].X) && depthTimeoutCounter < 20);
var colorStartX = (colorFrameSize.Width - w) / 2;
var colorStartY = (colorFrameSize.Height - h) / 2;
var depthStartX = (depthFrameSize.Width - w) / 2;
var depthStartY = (depthFrameSize.Height - h) / 2;
for (int i = 0; i < colorPoints.Length; i++)
{
colorPoints[i].X /= colorFrameSize.Width;
colorPoints[i].Y /= colorFrameSize.Height;
depthPoints[i].X /= depthFrameSize.Width;
depthPoints[i].Y /= depthFrameSize.Height;
}
colorMapper.SetData(colorPoints);
depthMapper.SetData(depthPoints);
return(colorMapper, depthMapper, depthPoints,
colorTimeoutCounter < 20 && depthTimeoutCounter < 20);
}
private void HandleBodyFrame(BodyFrame bodyFrame)
{
if (bodyFrame == null)
{
return;
}
if (bodies == null)
{
bodies = new Body[bodyFrame.BodyCount];
}
bodyFrame.GetAndRefreshBodyData(bodies);
var jmjBody = bodies.FirstOrDefault(b => b.IsTracked); // we get the first tracker body
if (jmjBody == null)
{
HideLaser();
this.currentViewModel.Status = "NO BODY";
return;
}
ShowLaser();
posMappedLeft = this.kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(jmjBody.Joints[JointType.HandLeft].Position);
this.currentViewModel.LeftHandX = posMappedLeft.X;
this.currentViewModel.LeftHandY = posMappedLeft.Y;
posMappedRight = this.kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(jmjBody.Joints[JointType.HandRight].Position);
this.currentViewModel.RightHandX = posMappedRight.X;
this.currentViewModel.RightHandY = posMappedRight.Y;
switch (jmjBody.HandLeftState)
{
case HandState.Open:
this.currentViewModel.LeftHandColor = openedHandColor;
break;
case HandState.Closed:
this.currentViewModel.LeftHandColor = closedHandColor;
break;
default:
this.currentViewModel.LeftHandColor = unusableHandColor;
break;
}
switch (jmjBody.HandRightState)
{
case HandState.Open:
this.currentViewModel.RightHandColor = openedHandColor;
break;
case HandState.Closed:
this.currentViewModel.RightHandColor = closedHandColor;
break;
default:
this.currentViewModel.RightHandColor = unusableHandColor;
break;
}
this.currentViewModel.LeftHandStatus = jmjBody.HandLeftState.ToString();
this.currentViewModel.RightHandStatus = jmjBody.HandRightState.ToString();
if (jmjBody.Joints[JointType.ShoulderLeft].Position.Z - jmjBody.Joints[JointType.HandLeft].Position.Z > 0.40)
{
PlayLaserUnderLeftHand(posMappedLeft.X, posMappedLeft.Y, jmjBody.HandLeftState);
this.currentViewModel.LeftHandTorsoDistance = "PLAY";
}
else
{
leftHandAdapter.ReleaseLaser();
this.currentViewModel.LeftHandTorsoDistance = "";
}
if (jmjBody.Joints[JointType.ShoulderRight].Position.Z - jmjBody.Joints[JointType.HandRight].Position.Z > 0.40)
{
PlayLaserUnderRightHand(posMappedRight.X, posMappedRight.Y, jmjBody.HandRightState);
this.currentViewModel.RightHandTorsoDistance = "PLAY";
}
else
{
rightHandAdapter.ReleaseLaser();
this.currentViewModel.RightHandTorsoDistance = "";
}
}
void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
var reference = e.FrameReference.AcquireFrame();
// Color
using (var frame = reference.ColorFrameReference.AcquireFrame())
{
if (frame != null)
{
//将图像转入writeablebitmmap
FrameDescription colorFrameDescription = frame.FrameDescription;
using (KinectBuffer colorBuffer = frame.LockRawImageBuffer())
{
this.colorBitmap.Lock();
// verify data and write the new color frame data to the display bitmap
if ((colorFrameDescription.Width == this.colorBitmap.PixelWidth) && (colorFrameDescription.Height == this.colorBitmap.PixelHeight))
{
frame.CopyConvertedFrameDataToIntPtr(
this.colorBitmap.BackBuffer,
(uint)(colorFrameDescription.Width * colorFrameDescription.Height * 4),
ColorImageFormat.Bgra);
this.colorBitmap.AddDirtyRect(new Int32Rect(0, 0, this.colorBitmap.PixelWidth, this.colorBitmap.PixelHeight));
}
this.colorBitmap.Unlock();
}
if (_mode == CameraMode.Color)
{
camera.Source = frame.ToBitmap();
}
}
}
// Depth
using (var frame = reference.DepthFrameReference.AcquireFrame())
{
if (frame != null)
{
if (_mode == CameraMode.Depth)
{
camera.Source = frame.ToBitmap();
}
}
}
// Infrared
using (var frame = reference.InfraredFrameReference.AcquireFrame())
{
if (frame != null)
{
if (_mode == CameraMode.Infrared)
{
camera.Source = frame.ToBitmap();
}
}
}
// Body
using (var frame = reference.BodyFrameReference.AcquireFrame())
{
if (frame != null)
{
canvas.Children.Clear();
_bodies = new Body[frame.BodyFrameSource.BodyCount];
frame.GetAndRefreshBodyData(_bodies);
foreach (var body in _bodies)
{
if (body.IsTracked)
{
// COORDINATE MAPPING
foreach (Joint joint in body.Joints.Values)
{
if (joint.TrackingState == TrackingState.Tracked)
{
// 3D space point
CameraSpacePoint jointPosition = joint.Position;
// 2D space point
System.Windows.Point point = new System.Windows.Point();
if (_mode == CameraMode.Color)
{
ColorSpacePoint colorPoint = _sensor.CoordinateMapper.MapCameraPointToColorSpace(jointPosition);
point.X = float.IsInfinity(colorPoint.X) ? 0 : colorPoint.X / 2;
point.Y = float.IsInfinity(colorPoint.Y) ? 0 : colorPoint.Y / 2;
//获取肩肘腕，脊椎中间坐标
switch (joint.JointType)
{
case JointType.WristLeft:
WristLeft.X = point.X;
WristLeft.Y = point.Y;
break;
case JointType.ElbowLeft:
ElbowLeft.X = point.X;
ElbowLeft.Y = point.Y;
break;
case JointType.ShoulderLeft:
ShoulderLeft.X = point.X;
ShoulderLeft.Y = point.Y;
break;
case JointType.SpineShoulder:
SpineShoulder.X = point.X;
SpineShoulder.Y = point.Y;
break;
case JointType.ShoulderRight:
ShoulderRight.X = point.X;
ShoulderRight.Y = point.Y;
break;
case JointType.ElbowRight:
ElbowRight.X = point.X;
ElbowRight.Y = point.Y;
break;
case JointType.WristRight:
WristRight.X = point.X;
WristRight.Y = point.Y;
break;
}
}
else if (_mode == CameraMode.Depth || _mode == CameraMode.Infrared) // Change the Image and Canvas dimensions to 512x424
{
DepthSpacePoint depthPoint = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(jointPosition);
point.X = float.IsInfinity(depthPoint.X) ? 0 : depthPoint.X / 2;
point.Y = float.IsInfinity(depthPoint.Y) ? 0 : depthPoint.Y / 2;
}
// Draw 红色圆，直径10
Ellipse ellipse = new Ellipse
{
Fill = System.Windows.Media.Brushes.Red,
Width = 10,
Height = 10
};
Canvas.SetLeft(ellipse, point.X - ellipse.Width / 2);
Canvas.SetTop(ellipse, point.Y - ellipse.Height / 2);
canvas.Children.Add(ellipse);
}
}
if (elbowrightflag)
{
DrawingLine(ShoulderRight, ElbowRight); //连接肩肘
DrawingLine(ElbowRight, WristRight); //连接肘腕
//计算肘角度并添加到canvas
rightelbowangle = GetAngle(ShoulderRight, ElbowRight, WristRight);
Addangle(rightelbowangle, ElbowRight);
}
if (shoulderrightflag)
{
DrawingLine(ShoulderRight, ElbowRight); //连接肩肘
DrawingLine(SpineShoulder, ShoulderRight); //连接肩和肩中
//计算肩角度并添加到canvas
rightshoulderangle = GetAngle(SpineShoulder, ShoulderRight, ElbowRight);
Addangle(rightshoulderangle, ShoulderRight);
}
}
}
}
}
}
public BitmapSource GreenScreenCustom(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
{
//
ushort[] _depthData = null;
byte[] _bodyData = null;
byte[] _colorData = null;
byte[] _displayPixels = null;
ColorSpacePoint[] _colorPoints = null;
CoordinateMapper _coordinateMapper = _sensor.CoordinateMapper;
int BYTES_PER_PIXEL = (PixelFormats.Bgr32.BitsPerPixel + 7) / 8;
PixelFormat FORMAT = PixelFormats.Bgra32;
double DPI = 96.0;
WriteableBitmap _bitmap = null;
//
int colorWidth = colorFrame.FrameDescription.Width;
int colorHeight = colorFrame.FrameDescription.Height;
int depthWidth = depthFrame.FrameDescription.Width;
int depthHeight = depthFrame.FrameDescription.Height;
int bodyIndexWidth = bodyIndexFrame.FrameDescription.Width;
int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;
if (_displayPixels == null)
{
_depthData = new ushort[depthWidth * depthHeight];
_bodyData = new byte[depthWidth * depthHeight];
_colorData = new byte[colorWidth * colorHeight * BYTES_PER_PIXEL];
_displayPixels = new byte[depthWidth * depthHeight * BYTES_PER_PIXEL];
_colorPoints = new ColorSpacePoint[depthWidth * depthHeight];
_bitmap = new WriteableBitmap(depthWidth, depthHeight, DPI, DPI, FORMAT, null);
}
if (((depthWidth * depthHeight) == _depthData.Length) && ((colorWidth * colorHeight * BYTES_PER_PIXEL) == _colorData.Length) && ((bodyIndexWidth * bodyIndexHeight) == _bodyData.Length))
{
depthFrame.CopyFrameDataToArray(_depthData);
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(_colorData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Bgra);
}
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
_coordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);
Array.Clear(_displayPixels, 0, _displayPixels.Length);
Array.Clear(rarray_final, 0, rarray_final.Length);
Array.Clear(garray_final, 0, rarray_final.Length);
Array.Clear(barray_final, 0, rarray_final.Length);
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
int depthIndex = (y * depthWidth) + x;
byte player = _bodyData[depthIndex];
if (player != 0xff)
{
ColorSpacePoint colorPoint = _colorPoints[depthIndex];
int colorX = (int)Math.Floor(colorPoint.X + 0.5);
int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
{
int colorIndex = ((colorY * colorWidth) + colorX) * BYTES_PER_PIXEL;
int displayIndex = depthIndex * BYTES_PER_PIXEL;
_displayPixels[displayIndex + 0] = _colorData[colorIndex];
_displayPixels[displayIndex + 1] = _colorData[colorIndex + 1];
_displayPixels[displayIndex + 2] = _colorData[colorIndex + 2];
_displayPixels[displayIndex + 3] = 0xff;
barray_final[y, x] = _colorData[colorIndex];
garray_final[y, x] = _colorData[colorIndex + 1];
rarray_final[y, x] = _colorData[colorIndex + 2];
// For ROS
_finaldisplayPixels[displayIndex + 0] = _colorData[colorIndex + 2];
_finaldisplayPixels[displayIndex + 1] = _colorData[colorIndex + 1];
_finaldisplayPixels[displayIndex + 2] = _colorData[colorIndex];
_finaldisplayPixels[displayIndex + 3] = 0xff;
}
}
}
}
_bitmap.Lock();
Marshal.Copy(_displayPixels, 0, _bitmap.BackBuffer, _displayPixels.Length);
_bitmap.AddDirtyRect(new Int32Rect(0, 0, depthWidth, depthHeight));
_bitmap.Unlock();
}
return(_bitmap);
}
/// <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();
}
}
void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
var reference = e.FrameReference.AcquireFrame();
// Color
using (var frame = reference.ColorFrameReference.AcquireFrame())
{
if (frame != null)
{
if (_mode == CameraMode.Color)
{
camera.Source = frame.ToBitmap();
}
}
}
// Depth
using (var frame = reference.DepthFrameReference.AcquireFrame())
{
if (frame != null)
{
if (_mode == CameraMode.Depth)
{
camera.Source = frame.ToBitmap();
}
}
}
// Infrared
using (var frame = reference.InfraredFrameReference.AcquireFrame())
{
if (frame != null)
{
if (_mode == CameraMode.Infrared)
{
camera.Source = frame.ToBitmap();
}
}
}
// Body
using (var frame = reference.BodyFrameReference.AcquireFrame())
{
if (frame != null)
{
canvas.Children.Clear();
_bodies = new Body[frame.BodyFrameSource.BodyCount];
frame.GetAndRefreshBodyData(_bodies);
foreach (var body in _bodies)
{
if (body.IsTracked)
{
// COORDINATE MAPPING
foreach (Joint joint in body.Joints.Values)
{
if (joint.TrackingState == TrackingState.Tracked)
{
// 3D space point
CameraSpacePoint jointPosition = joint.Position;
// 2D space point
Point point = new Point();
if (_mode == CameraMode.Color)
{
ColorSpacePoint colorPoint = _sensor.CoordinateMapper.MapCameraPointToColorSpace(jointPosition);
point.X = float.IsInfinity(colorPoint.X) ? 0 : (1366 * colorPoint.X) / 1920;
point.Y = float.IsInfinity(colorPoint.Y) ? 0 : (768 * colorPoint.Y) / 1080;
}
else if (_mode == CameraMode.Depth || _mode == CameraMode.Infrared) // Change the Image and Canvas dimensions to 512x424
{
DepthSpacePoint depthPoint = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(jointPosition);
point.X = float.IsInfinity(depthPoint.X) ? 0 : depthPoint.X;
point.Y = float.IsInfinity(depthPoint.Y) ? 0 : depthPoint.Y;
}
// Draw
Ellipse ellipse = new Ellipse
{
Fill = Brushes.Red,
Width = 15,
Height = 15
};
Canvas.SetLeft(ellipse, point.X - ellipse.Width / 2);
Canvas.SetTop(ellipse, point.Y - ellipse.Height / 2);
canvas.Children.Add(ellipse);
// CHECK 3____________________________________
FLX = camera_intrinsics.FocalLengthX;
FLY = camera_intrinsics.FocalLengthY;
string FLX2 = Convert.ToString(FLX);
string FLY2 = Convert.ToString(FLY);
string[] CI = { "FLX", FLX2, "FLY", FLY2 };
System.IO.File.WriteAllLines(@"D:\ALL.txt", CI);
//int abcdefgh = 8;
//mapper0.GetDepthCameraIntrinsics.FocalLengthX;
}
}
}
}
}
}
}
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;
}
private void Reader_BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
bool dataReceived = false;
if (e.FrameReference != null)
{
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
bodyFrame.GetAndRefreshBodyData(this.bodies);
dataReceived = true;
}
}
}
if (!dataReceived)
{
return;
}
for (int i = 0; i < this.bodyCount; i++)
{
Body body = this.bodies[i];
if (body.IsTracked && engagement.engagedBodies.Contains(body.TrackingId))
{
CameraSpacePoint handLeftPoint = body.Joints[JointType.HandLeft].Position;
CameraSpacePoint handRightPoint = body.Joints[JointType.HandRight].Position;
CameraSpacePoint spineBase = body.Joints[JointType.SpineBase].Position;
// Console.WriteLine(body.HandLeftConfidence);
ColorSpacePoint handLeft = coordinateMapper.MapCameraPointToColorSpace(handLeftPoint);
ColorSpacePoint handRight = coordinateMapper.MapCameraPointToColorSpace(handRightPoint);
double angle = ColourGestures.DetectColourGestures(handLeftPoint, handRightPoint);
Point last = new Point(handLeft.X, handLeft.Y);
if (previousPoints.ContainsKey(body.TrackingId) && previousPoints[body.TrackingId] != null && previousPoints[body.TrackingId].Points.Count > 0)
{
last = previousPoints[body.TrackingId].Points[previousPoints[body.TrackingId].Points.Count - 1];
}
double notJoinedLeftX = (last.X + ((handLeftPoint.X - spineBase.X) * drawArea.ActualWidth - last.X));
double notJoinedLeftY = (last.Y + ((spineBase.Y - handLeftPoint.Y) * drawArea.ActualHeight - last.Y));
Point notJoinedLeft = new Point(notJoinedLeftX + drawArea.ActualWidth / 2, notJoinedLeftY + drawArea.ActualHeight);
notJoinedPrevPos = notJoinedLeft;
Console.WriteLine("my calcualtaion " + notJoinedLeft.X + " " + notJoinedLeft.Y);
Ellipse colourSwatch = new Ellipse()
{
HorizontalAlignment = HorizontalAlignment.Left,
Height = 35,
Width = 35,
Margin = new Thickness(5),
Stroke = Brushes.Black,
StrokeThickness = 3
};
if (!colourSwatches.ContainsKey(body.TrackingId))
{
colourSwatches.Add(body.TrackingId, colourSwatch);
}
drawArea.Children.Remove(colourSwatches[body.TrackingId]);
if (angle > -1)
{
Color newColour = ColourGestures.HsvToRgb(angle, 1, 1);
colours[body.TrackingId] = new SolidColorBrush(newColour);
colourSwatches[body.TrackingId].Fill = colours[body.TrackingId];
drawArea.Children.Add(colourSwatches[body.TrackingId]);
startStopGestures.penUp[body.TrackingId] = true;
}
if (engagement.Draw(body.TrackingId) == HandType.LEFT)
{
startStopGestures.DetectLeftGestures(body, handLeftPoint);
startStopGestures.DetectStopGesture(body, handLeftPoint, handRightPoint);
Point currentPoint = new Point(handLeft.X, handLeft.Y);
float closenessFactor = handLeftPoint.Z / 4;
if (closenessFactor > 1)
{
closenessFactor = 1;
}
if (currentPoint.X < Double.PositiveInfinity && currentPoint.X > Double.NegativeInfinity &&
currentPoint.Y < Double.PositiveInfinity && currentPoint.Y > Double.NegativeInfinity)
{
currentPoint.X = (1 - closenessFactor) * currentPoint.X + (notJoinedLeft.X * (closenessFactor));
currentPoint.Y = (1 - closenessFactor) * currentPoint.Y + (notJoinedLeft.Y * (closenessFactor));
Draw(currentPoint, body.TrackingId, handLeftPoint.Z);
Canvas.SetTop(colourSwatches[body.TrackingId], handLeft.Y - 70);
Canvas.SetLeft(colourSwatches[body.TrackingId], handLeft.X + 10);
}
}
if (engagement.Draw(body.TrackingId) == HandType.RIGHT)
{
startStopGestures.DetectRightGestures(body, handRightPoint);
startStopGestures.DetectStopGesture(body, handRightPoint, handLeftPoint);
Point currentPoint = new Point(handRight.X, handRight.Y);
if (currentPoint.X < Double.PositiveInfinity && currentPoint.X > Double.NegativeInfinity &&
currentPoint.Y < Double.PositiveInfinity && currentPoint.Y > Double.NegativeInfinity)
{
Draw(currentPoint, body.TrackingId, handRightPoint.Z);
Canvas.SetTop(colourSwatches[body.TrackingId], handRight.Y - 70);
Canvas.SetLeft(colourSwatches[body.TrackingId], handRight.X - 10);
}
}
}
else if (body.IsTracked)
{
if (cursors.ContainsKey(body.TrackingId))
{
drawArea.Children.Remove(cursors[body.TrackingId]);
cursors.Remove(body.TrackingId);
rotateTransforms.Remove(body.TrackingId);
}
if (colourSwatches.ContainsKey(body.TrackingId))
{
drawArea.Children.Remove(colourSwatches[body.TrackingId]);
colourSwatches.Remove(body.TrackingId);
}
if (previousPoints.ContainsKey(body.TrackingId))
{
if (previousPoints[body.TrackingId] != null)
{
FinishSegment(body.TrackingId);
}
previousPoints.Remove(body.TrackingId);
}
}
if (this.faceFrameSources[i].IsTrackingIdValid)
{
if (this.faceFrameResults[i] != null)
{
this.DrawFaceFrameResults(i, this.faceFrameResults[i]);
}
}
else
{
if (this.bodies[i].IsTracked)
{
this.faceFrameSources[i].TrackingId = this.bodies[i].TrackingId;
}
}
}
}
private void BodyReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
bool dataReceived = false;
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
this.bodies = new Body[bodyFrame.BodyCount];
bodyFrame.GetAndRefreshBodyData(bodies);
// Console.WriteLine(canvas.Width + " " + canvas.Height);
Body body = bodies.Where(b => b.IsTracked).FirstOrDefault();
if (body != null)
{
using (DrawingContext dc = this.drawingGroup.Open())
{
Joint handRight = body.Joints[JointType.HandTipRight];
Joint handLeft = body.Joints[JointType.HandLeft];
CameraSpacePoint handRightPosition = handRight.Position;
ColorSpacePoint handRightPoint = this.kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(handRightPosition);
CameraSpacePoint handLeftPosition = handLeft.Position;
ColorSpacePoint handLeftPoint = this.kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(handLeftPosition);
/*Microsoft.Research.Kinect.Nui.Vector vector = new Microsoft.Research.Kinect.Nui.Vector();
* vector.X = ScaleVector(1024, handright.Position.X);
* vector.Y = ScaleVector(800, -handright.Position.Y);
* vector.Z = handright.Position.Z;
*
* handright.Position = vector;
*
* int topofscreen;
* int leftofscreen;
*
* leftofscreen = Convert.ToInt32(handRightPoint.X);
* topofscreen = Convert.ToInt32(handRightPoint.Y);
*
* DepthSpacePoint handPt = kinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(handRightPosition);
*
* Point relativePoint = new Point (handRightPoint.X-350, handRightPoint.Y-120 );
*
* SetCursorPos((int)relativePoint.X, (int)relativePoint.Y);
*
*/
//SetCursorPos(leftofscreen, topofscreen);
if (fpaint == true)
{
double x = handRightPoint.X;
double y = handRightPoint.Y;
if ((body.HandRightState == HandState.Closed /*|| body.HandRightState==HandState.Lasso */ || body.HandRightState == HandState.Unknown))
{
fgpoly = true;
Console.Write(x + " " + y + "\n");
if (x <= (Canvas.GetLeft(Black) + Black.Width) && x >= Canvas.GetLeft(Black) && y <= (Canvas.GetTop(Black) + Black.Height) && y >= Canvas.GetTop(Black))
{
Console.Write(fcolor + "\n");
fcolor = 0;
newpoly();
}
else if (x <= (Canvas.GetLeft(Red) + Red.Width) && x >= Canvas.GetLeft(Red) && y <= (Canvas.GetTop(Red) + Red.Height) && y >= Canvas.GetTop(Red))
{
fcolor = 1;
newpoly();
}
else if (x <= (Canvas.GetLeft(Green) + Green.Width) && x >= Canvas.GetLeft(Green) && y <= (Canvas.GetTop(Green) + Green.Height) && y >= Canvas.GetTop(Green))
{
fcolor = 2;
newpoly();
}
else if (x <= (Canvas.GetLeft(Purple) + Purple.Width) && x >= Canvas.GetLeft(Purple) && y <= (Canvas.GetTop(Purple) + Purple.Height) && y >= Canvas.GetTop(Purple))
{
fcolor = 3;
newpoly();
}
else if (x <= (Canvas.GetLeft(Yellow) + Yellow.Width) && x >= Canvas.GetLeft(Yellow) && y <= (Canvas.GetTop(Yellow) + Yellow.Height) && y >= Canvas.GetTop(Yellow))
{
fcolor = 4;
newpoly();
}
else if (x <= (Canvas.GetLeft(Blue) + Blue.Width) && x >= Canvas.GetLeft(Blue) && y <= (Canvas.GetTop(Blue) + Blue.Height) && y >= Canvas.GetTop(Blue))
{
fcolor = 5;
newpoly();
}
else if (x <= (Canvas.GetLeft(eraser) + eraser.Width) && x >= Canvas.GetLeft(eraser) && y <= (Canvas.GetTop(eraser) + eraser.Height) && y >= Canvas.GetTop(eraser))
{
fcolor = 6;
newpoly();
}
else if (x <= 1779)
{
if (dppaint != null)
{
dppaint.Points.Add(new Point {
X = x, Y = y
});
}
}
}
else if (handRight.TrackingState != TrackingState.NotTracked && body.HandRightState != HandState.Closed)
{
if (fgpoly == true)
{
newpoly();
fgpoly = false;
}
}
if (handRight.TrackingState != TrackingState.NotTracked)
{
if (!double.IsInfinity(x - (fpoint.Width / 2.0)) && !double.IsInfinity(y - (fpoint.Height / 2.0)))
{
fpoint.Visibility = Visibility.Visible;
Canvas.SetLeft(fpoint, x - (fpoint.Width / 2.0));
Canvas.SetTop(fpoint, y - (fpoint.Height / 2.0));
//dc.DrawEllipse(paint.Stroke, null, new Point(handRightPoint.X, handRightPoint.Y), 10, 10);
//img.PointToScreen(new Point(handRightPoint.X, handRightPoint.Y));
}
}
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.colorFrameDescription.Width, this.colorFrameDescription.Height));
}
else
{
Point right = new Point(handRightPoint.X, handRightPoint.Y);
Point left = new Point(handLeftPoint.X, handLeftPoint.Y);
double distance = Point.Subtract(left, right).Length;
// if(handRight.TrackingState != TrackingState.NotTracked && Button.)
if (distance <= 200 && handLeft.TrackingState != TrackingState.NotTracked && handRight.TrackingState != TrackingState.NotTracked && body.HandLeftState == HandState.Closed && body.HandRightState != HandState.Closed)
{
// Console.Write(x + " " + Canvas.GetLeft(circle) + "\n");
// if (!float.IsInfinity(x) && !float.IsInfinity(y))
//{
// DRAW!
//trail.Points.Add(new Point { X = x, Y = y });
// right = new Point(handRightPoint.X, handRightPoint.Y);
// left = new Point(handLeftPoint.X, handLeftPoint.Y);
//distance = Point.Subtract(left, right).Length;
if (((Canvas.GetLeft(circle) + circle.Width) - left.X) <= circle.Width + 100 && (right.X - (Canvas.GetLeft(circle))) <= circle.Width + 100 && Math.Abs((Canvas.GetTop(circle) + circle.Height / 2.0) - left.Y) <= circle.Height / 2.0 && Math.Abs(right.Y - (Canvas.GetTop(circle) + circle.Height / 2.0)) <= circle.Height / 2.0 || cirf == true)
{
cirf = true;
if (temp != 0)
{
if (circle.Width + (temp - distance) >= 10)
{
circle.Width += (temp - distance);
}
if (circle.Height + (temp - distance) >= 10)
{
circle.Height += (temp - distance);
}
}
temp = distance;
}
else if (((Canvas.GetLeft(ellipse) + ellipse.Width) - left.X) <= ellipse.Width + 100 && (right.X - (Canvas.GetLeft(ellipse) + ellipse.Width)) <= ellipse.Width + 100 && Math.Abs((Canvas.GetTop(ellipse) + ellipse.Height / 2.0) - left.Y) <= ellipse.Height / 2.0 && Math.Abs(right.Y - (Canvas.GetTop(ellipse) + ellipse.Height / 2.0)) <= circle.Height / 2.0 || ellf == true)
{
ellf = true;
if (temp != 0)
{
if (ellipse.Width + (temp - distance) >= 10)
{
ellipse.Width += (temp - distance);
}
if (ellipse.Height + (temp - distance) >= 10)
{
ellipse.Height += (temp - distance);
}
}
temp = distance;
}
else if (((Canvas.GetLeft(rec) + rec.Width) - left.X) <= rec.Width + 100 && (right.X - (Canvas.GetLeft(rec) + rec.Width)) <= rec.Width + 100 && Math.Abs((Canvas.GetTop(rec) + rec.Height / 2.0) - left.Y) <= rec.Height / 2.0 && Math.Abs(right.Y - (Canvas.GetTop(rec) + rec.Height / 2.0)) <= rec.Height / 2.0 || recf == true)
{
recf = true;
if (temp != 0)
{
if (rec.Width + (temp - distance) >= 10)
{
rec.Width += (temp - distance);
}
if (rec.Height + (temp - distance) >= 10)
{
rec.Height += (temp - distance);
}
}
temp = distance;
}
else if (((Canvas.GetLeft(square) + square.Width) - left.X) <= square.Width + 100 && (right.X - (Canvas.GetLeft(square) + square.Width)) <= square.Width + 100 && Math.Abs((Canvas.GetTop(square) + square.Height / 2.0) - left.Y) <= square.Height / 2.0 && Math.Abs(right.Y - (Canvas.GetTop(square) + square.Height / 2.0)) <= square.Height / 2.0 || squrf == true)
{
squrf = true;
if (temp != 0)
{
if (square.Width + (temp - distance) >= 10)
{
square.Width += (temp - distance);
}
if (square.Height + (temp - distance) >= 10)
{
square.Height += (temp - distance);
}
}
temp = distance;
}
}
else if (handRight.TrackingState != TrackingState.NotTracked && body.HandRightState == HandState.Closed)
{
float x = handRightPoint.X;
float y = handRightPoint.Y;
Console.Write(x + " " + Canvas.GetLeft(circle) + "\n");
// if (!float.IsInfinity(x) && !float.IsInfinity(y))
//{
// DRAW!
//trail.Points.Add(new Point { X = x, Y = y });
if ((x < (Canvas.GetLeft(circle) + circle.Width) && x > Canvas.GetLeft(circle) && y < (Canvas.GetTop(circle) + circle.Height) && y > Canvas.GetTop(circle)) || cirf == true)
{
cirf = true;
Canvas.SetLeft(circle, x - (circle.Width / 2.0));
Canvas.SetTop(circle, y - circle.Height / 2.0);
}
else if ((x < (Canvas.GetLeft(ellipse) + ellipse.Width) && x > Canvas.GetLeft(ellipse) && y < (Canvas.GetTop(ellipse) + ellipse.Height) && y > Canvas.GetTop(ellipse)) || ellf == true)
{
ellf = true;
Canvas.SetLeft(ellipse, x - ellipse.Width / 2.0);
Canvas.SetTop(ellipse, y - ellipse.Height);
}
else if ((x < (Canvas.GetLeft(rec) + rec.Width) && x > Canvas.GetLeft(rec) && y < (Canvas.GetTop(rec) + rec.Height) && y > Canvas.GetTop(rec)) || recf == true)
{
recf = true;
Canvas.SetLeft(rec, x - rec.Width / 2.0);
Canvas.SetTop(rec, y - rec.Height / 2.0);
}
else if ((x < (Canvas.GetLeft(square) + square.Width) && x > Canvas.GetLeft(square) && y < (Canvas.GetTop(square) + square.Height) && y > Canvas.GetTop(square)) || squrf == true)
{
squrf = true;
Canvas.SetLeft(square, x - square.Width / 2.0);
Canvas.SetTop(square, y - square.Height);
}
//}
}
else
{
cirf = squrf = recf = ellf = false;
temp = 0;
}
}
}
}
}
}
}
public unsafe void Update(ushort[] depthData, byte[] bodyIndexData, Body body)
{
double handLength = 0.0;
double barLength = 0.0;
double barHeight = 0.0;
double angle = 0.0;
Joint shoulderLeft = body.Joints[JointType.ShoulderLeft];
Joint shoulderRight = body.Joints[JointType.ShoulderRight];
Joint chest = body.Joints[JointType.SpineShoulder];
Joint waist = body.Joints[JointType.SpineBase];
Joint elbowLeft = body.Joints[JointType.ElbowLeft];
Joint elbowRight = body.Joints[JointType.ElbowRight];
Joint handLeft = body.Joints[JointType.HandLeft];
Joint handRight = body.Joints[JointType.HandRight];
Joint footLeft = body.Joints[JointType.FootLeft];
Joint footRight = body.Joints[JointType.FootRight];
if (waist.TrackingState == TrackingState.NotTracked)
{
return;
}
if (waist.Position.Z < 2.0f || waist.Position.Z > 4.5f)
{
return;
}
if (shoulderLeft.TrackingState != TrackingState.NotTracked && shoulderRight.TrackingState != TrackingState.NotTracked &&
elbowLeft.TrackingState != TrackingState.NotTracked && elbowRight.TrackingState != TrackingState.NotTracked &&
handLeft.TrackingState != TrackingState.NotTracked && handRight.TrackingState != TrackingState.NotTracked)
{
handLength =
shoulderLeft.Position.Length(shoulderRight.Position) +
shoulderLeft.Position.Length(elbowLeft.Position) +
shoulderRight.Position.Length(elbowRight.Position) +
elbowLeft.Position.Length(handLeft.Position) +
elbowRight.Position.Length(handRight.Position);
}
CoordinateMapper.MapColorFrameToDepthSpace(depthData, _depthPoints);
fixed(DepthSpacePoint *colorMappedToDepthPointsPointer = _depthPoints)
{
int minimumX = int.MaxValue;
int maximumX = int.MinValue;
int minimumY = int.MaxValue;
int maximumY = int.MinValue;
ushort minimumDistance = 0;
ushort maximumdistance = 0;
for (int colorIndex = 0; colorIndex < _depthPoints.Length; ++colorIndex)
{
float colorMappedToDepthX = colorMappedToDepthPointsPointer[colorIndex].X;
float colorMappedToDepthY = colorMappedToDepthPointsPointer[colorIndex].Y;
if (!float.IsNegativeInfinity(colorMappedToDepthX) &&
!float.IsNegativeInfinity(colorMappedToDepthY))
{
int depthX = (int)(colorMappedToDepthX + 0.5f);
int depthY = (int)(colorMappedToDepthY + 0.5f);
if ((depthX >= 0) && (depthX < DepthWidth) && (depthY >= 0) && (depthY < DepthHeight))
{
int depthIndex = (depthY * DepthWidth) + depthX;
ushort depth = depthData[depthIndex];
if (bodyIndexData[depthIndex] != 0xff)
{
if (depthX < minimumX)
{
minimumX = depthX;
minimumY = depthY;
minimumDistance = depth;
}
if (depthX > maximumX)
{
maximumX = depthX;
maximumY = depthY;
maximumdistance = depth;
}
continue;
}
}
}
}
DepthSpacePoint depthMinimum = new DepthSpacePoint
{
X = minimumX,
Y = minimumY
};
DepthSpacePoint depthMaximum = new DepthSpacePoint
{
X = maximumX,
Y = maximumY
};
CameraSpacePoint cameraMinimum = CoordinateMapper.MapDepthPointToCameraSpace(depthMinimum, minimumDistance);
CameraSpacePoint cameraMaximum = CoordinateMapper.MapDepthPointToCameraSpace(depthMaximum, maximumdistance);
ColorSpacePoint colorMinimum = CoordinateMapper.MapDepthPointToColorSpace(depthMinimum, minimumDistance);
ColorSpacePoint colorMaximum = CoordinateMapper.MapDepthPointToColorSpace(depthMaximum, maximumdistance);
CameraSpacePoint cameraTrail = new CameraSpacePoint
{
X = (cameraMinimum.X + cameraMaximum.X) / 2f,
Y = (cameraMinimum.Y + cameraMaximum.Y) / 2f,
Z = (cameraMinimum.Z + cameraMaximum.Z) / 2f
};
ColorSpacePoint colorTrail = new ColorSpacePoint
{
X = (colorMinimum.X + colorMaximum.X) / 2f,
Y = (colorMinimum.Y + colorMaximum.Y) / 2f
};
DepthSpacePoint depthTrail = new DepthSpacePoint
{
X = (depthMinimum.X + depthMaximum.X) / 2f,
Y = (depthMinimum.Y + depthMaximum.Y) / 2f
};
CameraSpacePoint feet = new CameraSpacePoint
{
X = (footLeft.Position.X + footRight.Position.X) / 2f,
Y = (footLeft.Position.Y + footRight.Position.Y) / 2f,
Z = (footLeft.Position.Z + footRight.Position.Z) / 2f
};
CameraSpacePoint projection = new CameraSpacePoint
{
X = cameraTrail.X,
Y = feet.Y,
Z = cameraTrail.Z
};
barLength = cameraMinimum.Length(cameraMaximum);
barHeight = cameraTrail.Length(projection);
angle = cameraMinimum.Angle(cameraMaximum, new CameraSpacePoint {
X = cameraMaximum.X, Y = cameraMinimum.Y, Z = (cameraMaximum.Z + cameraMinimum.Z) / 2f
});
if (angle > 180.0)
{
angle = 360.0 - angle;
}
if (cameraMinimum.Y < cameraMaximum.Y)
{
angle = -angle;
}
if (barLength > handLength && Math.Abs(angle) < 25.0)
{
BarDetectionResult result = new BarDetectionResult
{
Minimum = new MultiPoint
{
CameraPoint = cameraMinimum,
ColorPoint = colorMinimum,
DepthPoint = depthMinimum
},
Maximum = new MultiPoint
{
CameraPoint = cameraMaximum,
ColorPoint = colorMaximum,
DepthPoint = depthMaximum
},
Trail = new MultiPoint
{
CameraPoint = cameraTrail,
ColorPoint = colorTrail,
DepthPoint = depthTrail
},
BarHeight = barHeight + HEIGHT_DIFFERENCE,
BarLength = barLength,
Angle = angle
};
BarDetected?.Invoke(this, result);
}
}
}
private bool checkStart(ColorSpacePoint joint_head, ColorSpacePoint joint_handLeft, ColorSpacePoint joint_handRight, int i)
{
//通过判断左手或右手关节点与头结点的距离是否小于7厘米来触发变脸；单位：毫米
if (startChangeFace[i] == false)
{
startChangeFace[i] = ((Math.Abs(joint_handLeft.X - joint_head.X) < 70 && Math.Abs(joint_handLeft.Y - joint_head.Y) < 70) ||
(Math.Abs(joint_handRight.X - joint_head.X) < 70 && Math.Abs(joint_handRight.Y - joint_head.Y) < 70));
}
return(false);
}
public Point MapJointToColor(KinectBase.Joint joint, bool undoTransform)
{
//TODO: Update this so it takes a joint array instead of a single joint (this is supposed to be more efficient for the Kinect 2)
Point mappedPoint = new Point(0, 0);
Point3D transformedPosition = joint.Position;
if (undoTransform)
{
Matrix3D inverseTransform = skeletonTransformation;
inverseTransform.Invert();
transformedPosition = inverseTransform.Transform(transformedPosition);
}
//Setup the Kinect v2 objects to do the transformation
CameraSpacePoint[] skelPoints = new CameraSpacePoint[1];
skelPoints[0] = new CameraSpacePoint();
skelPoints[0].X = (float)transformedPosition.X;
skelPoints[0].Y = (float)transformedPosition.Y;
skelPoints[0].Z = (float)transformedPosition.Z;
ColorSpacePoint[] points = new ColorSpacePoint[1];
kinect.CoordinateMapper.MapCameraPointsToColorSpace(skelPoints, points);
//Convert back to the base object type
mappedPoint.X = points[0].X;
mappedPoint.Y = points[0].Y;
return mappedPoint;
}
/// <summary>
/// 人が映った画素領域だけを映した BitmapSource を取得します。
/// </summary>
/// <param name="colorFrame">
/// カラー画像フレーム。
/// </param>
/// <param name="depthFrame">
/// 深度フレーム。
/// </param>
/// <param name="bodyIndexFrame">
/// BodyIndex フレーム。
/// </param>
/// <returns>
/// 人が映った画素領域だけを映した BitmapSource。
/// </returns>
BitmapSource GetUserMaskImage(ColorFrame colorFrame,
DepthFrame depthFrame,
BodyIndexFrame bodyIndexFrame)
{
byte[] colors = new byte[this.colorFrameDescription.Width
* this.colorFrameDescription.Height
* this.colorFrameDescription.BytesPerPixel];
colorFrame.CopyConvertedFrameDataToArray(colors, this.colorImageFormat);
ushort[] depths = new ushort[this.depthFrameDescription.Width
* this.depthFrameDescription.Height];
depthFrame.CopyFrameDataToArray(depths);
byte[] bodyIndexes = new byte[this.bodyIndexFrameDescription.Width
* this.bodyIndexFrameDescription.Height];
bodyIndexFrame.CopyFrameDataToArray(bodyIndexes);
//人が映っているだけの画像を表す byte 配列を用意して 0 で初期化する。
byte[] bodyColors = new byte[this.bodyIndexFrameDescription.Width
* this.bodyIndexFrameDescription.Height
* this.colorFrameDescription.BytesPerPixel];
Array.Clear(bodyColors, 0, bodyColors.Length);
//カラー画像と深度画像の座標を対応させる(= マッピングする)必要がある。
//ColorSpacePoint はカラー画像の座標を示す構造体。
ColorSpacePoint[] colorSpacePoints
= new ColorSpacePoint[this.bodyIndexFrameDescription.Width
* this.bodyIndexFrameDescription.Height];
this.kinect.CoordinateMapper.MapDepthFrameToColorSpace(depths, colorSpacePoints);
for (int y = 0; y < this.depthFrameDescription.Height; y++)
{
for (int x = 0; x < this.depthFrameDescription.Width; x++)
{
int depthsIndex = y * this.depthFrameDescription.Width + x;
if (bodyIndexes[depthsIndex] != 255)
{
//対応するカラー画像の座標を取得し、
//カラー画像座標系の範囲内に収まるかを判定する。
ColorSpacePoint colorSpacePoint = colorSpacePoints[depthsIndex];
int colorX = (int)colorSpacePoint.X;
int colorY = (int)colorSpacePoint.Y;
if ((colorSpacePoint.X >= 0) &&
(colorSpacePoint.X < this.colorFrameDescription.Width) &&
(colorSpacePoint.Y >= 0) &&
(colorSpacePoint.Y < this.colorFrameDescription.Height))
{
//対応するカラー画像の画素から色情報を取得して、
//新しいカラー画像のデータに与える。
int colorsIndex =
(colorY * this.colorFrameDescription.Width + colorX) * 4;
int bodyColorsIndex = depthsIndex * 4;
bodyColors[bodyColorsIndex] = colors[colorsIndex];
bodyColors[bodyColorsIndex + 1] = colors[colorsIndex + 1];
bodyColors[bodyColorsIndex + 2] = colors[colorsIndex + 2];
bodyColors[bodyColorsIndex + 3] = 255;
}
}
}
}
BitmapSource bitmapSource
= BitmapSource.Create(depthFrameDescription.Width,
depthFrameDescription.Height,
96,
96,
PixelFormats.Bgra32,
null,
bodyColors,
depthFrameDescription.Width * 4);
return(bitmapSource);
}
public SpacePointBase(ColorSpacePoint pt) : this(pt.X, pt.Y, SpaceMode.Color)
{
}
/// <summary>
/// Updates the position of the cursor.
/// </summary>
/// <param name="point">The specified point in the color space.</param>
/// <param name="ratioX">The specified horizontal scale scale ratio.</param>
/// <param name="ratioY">The specified vertical scale ratio.</param>
public void Update(ColorSpacePoint point, double ratioX = 1.0, double ratioY = 1.0)
{
Update(point.X * ratioX, point.Y * ratioY);
}
/// <summary>
/// Procedure invoked by Kinect when new data are available
/// </summary>
private void MultisourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
if (KinectSensor == null || Reader == null)
{
return;
}
// acquire frame data
MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();
// if the Frame has expired by the time we process this event, return.
if (multiSourceFrame == null)
{
return;
}
// Continue only if buffer is empty
if (!Buffer.IsEmpty())
{
return;
}
// declare variables for data from sensor
ColorFrame colorFrame = null;
DepthFrame depthFrame = null;
InfraredFrame infraredFrame = null;
byte[] colorFrameData = null;
ushort[] depthData = null;
ushort[] infraredData = null;
DepthSpacePoint[] pointsFromColorToDepth = null;
ColorSpacePoint[] pointsFromDepthToColor = null;
CameraSpacePoint[] cameraSpacePointsFromDepthData = null;
try
{
// get frames from sensor
colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
infraredFrame = multiSourceFrame.InfraredFrameReference.AcquireFrame();
// If any frame has expired by the time we process this event, return.
if (colorFrame == null || depthFrame == null || infraredFrame == null)
{
return;
}
// use frame data to fill arrays
colorFrameData = new byte[ColorFrameDescription.LengthInPixels * 4];
depthData = new ushort[DepthFrameDescription.LengthInPixels];
infraredData = new ushort[InfraredFrameDescription.LengthInPixels];
colorFrame.CopyConvertedFrameDataToArray(colorFrameData, ColorImageFormat.Bgra);
depthFrame.CopyFrameDataToArray(depthData);
infraredFrame.CopyFrameDataToArray(infraredData);
pointsFromColorToDepth = new DepthSpacePoint[ColorFrameDescription.LengthInPixels];
pointsFromDepthToColor = new ColorSpacePoint[DepthFrameDescription.LengthInPixels];
cameraSpacePointsFromDepthData = new CameraSpacePoint[DepthFrameDescription.LengthInPixels];
using (KinectBuffer depthFrameData = depthFrame.LockImageBuffer())
{
CoordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
depthFrameData.UnderlyingBuffer,
depthFrameData.Size,
pointsFromColorToDepth);
CoordinateMapper.MapDepthFrameToColorSpaceUsingIntPtr(
depthFrameData.UnderlyingBuffer,
depthFrameData.Size,
pointsFromDepthToColor);
CoordinateMapper.MapDepthFrameToCameraSpaceUsingIntPtr(
depthFrameData.UnderlyingBuffer,
depthFrameData.Size,
cameraSpacePointsFromDepthData);
}
}
finally
{
// dispose frames so that Kinect can continue processing
colorFrame?.Dispose();
depthFrame?.Dispose();
infraredFrame?.Dispose();
// send data futher
if (
colorFrameData != null &&
depthData != null &&
infraredData != null &&
cameraSpacePointsFromDepthData != null
)
{
// store data to buffer and notify processing thread
Buffer.Store(
new KinectData(
colorFrameData,
depthData,
infraredData,
cameraSpacePointsFromDepthData,
pointsFromColorToDepth,
pointsFromDepthToColor
)
);
TrackingManager.SendKinectUpdate();
}
}
}
/// <summary>
/// Copies face color points to the gpu
/// </summary>
/// <param name="context">Device context</param>
/// <param name="points">Points array</param>
public void Copy(DeviceContext context, ColorSpacePoint[] points)
{
Copy(context, points, points.Length);
}
/// <summary>
/// 触发变脸的方法
/// </summary>
/// <param name="joint_head"></param>
/// <param name="joint_handLeft"></param>
/// <param name="joint_handRight"></param>
/// <returns></returns>
private bool changeFaceMethod(ColorSpacePoint joint_head, ColorSpacePoint joint_handLeft, ColorSpacePoint joint_handRight)
{
//通过判断左手或右手关节点与头结点的距离是否小于7厘米来触发变脸；单位：毫米
bool isOverMatrix = ((Math.Abs(joint_handLeft.X - joint_head.X) < 70 && Math.Abs(joint_handLeft.Y - joint_head.Y) < 70) ||
(Math.Abs(joint_handRight.X - joint_head.X) < 70 && Math.Abs(joint_handRight.Y - joint_head.Y) < 70));
return(isOverMatrix);
}
public ColorPointCloud(ColorFrame iColorFrame, DepthFrame iDepthFrame, BodyIndexFrame iBodyIndexFrame, CoordinateMapper iCoordinateMapper)
{
TimeStamp = DateTime.Now;
Width = iDepthFrame.FrameDescription.Width;
Height = iDepthFrame.FrameDescription.Height;
var depthData = new ushort[Width * Height];
//BodyData = new int[Width * Height];
iDepthFrame.CopyFrameDataToArray(depthData);
var bodyData = new byte[Width * Height];
iBodyIndexFrame.CopyFrameDataToArray(bodyData);
var colorWidth = iColorFrame.FrameDescription.Width;
var colorHeight = iColorFrame.FrameDescription.Height;
var colorData = new byte[colorWidth * colorHeight * KinectConstants.BYTES_PER_PIXEL];
if (iColorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
iColorFrame.CopyRawFrameDataToArray(colorData);
}
else
{
iColorFrame.CopyConvertedFrameDataToArray(colorData, ColorImageFormat.Bgra);
}
var colorPoints = new ColorSpacePoint[Width * Height];
iCoordinateMapper.MapDepthFrameToColorSpace(depthData, colorPoints);
var displayPixels = new byte[Width * Height * KinectConstants.BYTES_PER_PIXEL];
Array.Clear(displayPixels, 0, displayPixels.Length);
var bodyDataBool = new bool[Width * Height];
var colorDataWithoutAlpha = new byte[Width * Height * 3];
for (var y = 0; y < Height; ++y)
{
for (var x = 0; x < Width; ++x)
{
var depthIndex = (y * Width) + x;
var player = bodyData[depthIndex];
if (player != 0xff)
{
bodyDataBool[depthIndex] = true;
var colorPoint = colorPoints[depthIndex];
var colorX = (int)Math.Floor(colorPoint.X + 0.5);
var colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
{
var colorIndex = ((colorY * colorWidth) + colorX) * KinectConstants.BYTES_PER_PIXEL;
var displayIndex = depthIndex * KinectConstants.BYTES_PER_PIXEL;
displayPixels[displayIndex + 0] = colorData[colorIndex]; // B
displayPixels[displayIndex + 1] = colorData[colorIndex + 1]; // G
displayPixels[displayIndex + 2] = colorData[colorIndex + 2]; // R
displayPixels[displayIndex + 3] = 0xff; // Alpha
var colorDataIndex = depthIndex * 3;
colorDataWithoutAlpha[colorDataIndex + 0] = colorData[colorIndex + 0];
colorDataWithoutAlpha[colorDataIndex + 1] = colorData[colorIndex + 1];
colorDataWithoutAlpha[colorDataIndex + 2] = colorData[colorIndex + 2];
}
}
}
}
DisplayPixels = displayPixels;
BodyData = bodyDataBool;
DepthData = depthData;
ColorData = colorDataWithoutAlpha;
}
private bool checkEnd(ColorSpacePoint joint_head, ColorSpacePoint joint_handLeft, ColorSpacePoint joint_handRight, int i)
{
//通过判断左手或右手关节点与头结点的距离是否小于7厘米来触发变脸；单位：毫米
if (startChangeFace[i] == true)
{
return(-(joint_handLeft.Y - joint_head.Y) > 100 && -(joint_handRight.Y - joint_head.Y) > 100);
}
else
{
return(false);
}
}
public static void DrawLine(this Canvas canvas, Joint first, Joint second, KinectSensor _sensor)
{
if (first.TrackingState == TrackingState.NotTracked || second.TrackingState == TrackingState.NotTracked)
{
return;
}
//first = first.ScaleTo(canvas.ActualWidth, canvas.ActualHeight);
//second = second.ScaleTo(canvas.ActualWidth, canvas.ActualHeight);
// 3D space point
// CameraSpacePoint jointPosition = joint.Position;
CameraSpacePoint firstPosition = first.Position;
CameraSpacePoint secondPosition = second.Position;
// 2D space point
//Point point = new Point();
Point firstPoint = new Point();
Point secondPoint = new Point();
ColorSpacePoint firstColorPoint = _sensor.CoordinateMapper.MapCameraPointToColorSpace(firstPosition);
ColorSpacePoint secondColorPoint = _sensor.CoordinateMapper.MapCameraPointToColorSpace(secondPosition);
//DepthSpacePoint depthPoint = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(jointPosition);
// point.X = float.IsInfinity(colorPoint.X) ? 0 : colorPoint.X;
firstPoint.X = float.IsInfinity(firstColorPoint.X) ? 0 : firstColorPoint.X;
secondPoint.X = float.IsInfinity(secondColorPoint.X) ? 0 : secondColorPoint.X;
// point.X = float.IsInfinity(depthPoint.X) ? 0 : depthPoint.X;
//point.Y = float.IsInfinity(colorPoint.Y) ? 0 : colorPoint.Y;
firstPoint.Y = float.IsInfinity(firstColorPoint.Y) ? 0 : firstColorPoint.Y;
secondPoint.Y = float.IsInfinity(secondColorPoint.Y) ? 0 : secondColorPoint.Y;
// point.Y = float.IsInfinity(depthPoint.Y) ? 0 : depthPoint.Y;
// Draw
Ellipse ellipse = new Ellipse
{
Fill = Brushes.Red,
Width = 6,
Height = 6
};
/* Canvas.SetLeft(ellipse, point.X - ellipse.Width / 2);
* Canvas.SetTop(ellipse, point.Y - ellipse.Height / 2);
*
* canvas.Children.Add(ellipse); */
/* Line line = new Line
* {
* X1 = first.Position.X,
* Y1 = first.Position.Y,
* X2 = second.Position.X,
* Y2 = second.Position.Y,
* StrokeThickness = 8,
* Stroke = new SolidColorBrush(Colors.Blue)
* }; */
Line line = new Line
{
X1 = firstPoint.X,
Y1 = firstPoint.Y,
X2 = secondPoint.X,
Y2 = secondPoint.Y,
StrokeThickness = 8,
Stroke = new SolidColorBrush(Colors.Blue)
};
canvas.Children.Add(line);
}
private void RefreshData(ushort[] depthData)
{
var frameDesc = _Sensor.DepthFrameSource.FrameDescription;
ColorSpacePoint[] colorSpace = new ColorSpacePoint[depthData.Length];
_Mapper.MapDepthFrameToColorSpace(depthData, colorSpace);
Hashtable depthCount = new Hashtable();
if (_flagDeskDepth)
{
int maxCount = 0;
for (int i = 0; i < depthData.Length; i++)
{
if (depthCount.ContainsKey(depthData[i]))
{
if ((int)depthData[i] != 0)
{
depthCount[depthData[i]] = (int)depthCount[depthData[i]] + 1;
}
if ((int)depthCount[depthData[i]] > maxCount)
{
maxCount = (int)depthCount[depthData[i]];
_maxCountKey = depthData[i];
}
}
else
{
depthCount.Add(depthData[i], 1);
}
}
_flagDeskDepth = false;
Debug.Log(_maxCountKey);
//Debug.Log("Max" + maxCountKey);
}
//foreach (ushort key in depthCount.Keys)
//{
// if((int)depthCount[key] > maxCount)
// {
// maxCount = (int)depthCount[key];
// maxCountKey = key;
// }
//}
//Debug.Log(maxCountKey);
//設定存放depth的Mat大小
_Depth = new Mat(frameDesc.Height / _DownsampleSize, frameDesc.Width / _DownsampleSize, CvType.CV_8UC1);
for (int y = 0; y < frameDesc.Height; y += _DownsampleSize)
{
for (int x = 0; x < frameDesc.Width; x += _DownsampleSize)
{
int indexX = x / _DownsampleSize;
int indexY = y / _DownsampleSize;
int smallIndex = (indexY * (frameDesc.Width / _DownsampleSize)) + indexX;
double avg = GetAvg(depthData, x, y, frameDesc.Width, frameDesc.Height);
//avg = avg * _DepthScale;
if (indexX == 64 && indexY == 53 && _kinectDistance != null)
{
_kinectDistance.text = "distance: " + avg;
}
if (x == 0 && y == 0)
{
Distance_UpLeft = avg;
}
if (x == (frameDesc.Width - _DownsampleSize) && y == 0)
{
Distance_UpRight = avg;
}
if (x == 0 && y == (frameDesc.Height - _DownsampleSize))
{
Distance_DownLeft = avg;
}
if (x == (frameDesc.Width - _DownsampleSize) && y == (frameDesc.Height - _DownsampleSize))
{
Distance_DonwRight = avg;
}
//距離1000mm正負200mm
//avg = (avg > (double)(_maxCountKey + _binaryIndex)) ? 0 : 255;
avg = 255 - (avg / 4000 * 255);
avg = (avg == 255) ? 0 : avg;
_Vertices[smallIndex].z = (float)avg;
_Depth.put(y / _DownsampleSize, frameDesc.Width / _DownsampleSize - x / _DownsampleSize, avg);
// Update UV mapping with CDRP
//var colorSpacePoint = colorSpace[(y * frameDesc.Width) + x];
// _UV[smallIndex] = new Vector2(colorSpacePoint.X / colorWidth, colorSpacePoint.Y / colorHeight);
}
}
}
private void BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
if (null == e.FrameReference)
{
return;
}
// Remove all the old points and clipping lines
while (0 < KinectCanvas.Children.Count)
{
KinectCanvas.Children.RemoveAt(0);
}
// If you do not dispose of the frame, you never get another one...
using (BodyFrame _BodyFrame = e.FrameReference.AcquireFrame()) {
if (null == _BodyFrame)
{
return;
}
Body[] _Bodies = new Body[_BodyFrame.BodyFrameSource.BodyCount];
_BodyFrame.GetAndRefreshBodyData(_Bodies);
foreach (Body _Body in _Bodies)
{
if (_Body.IsTracked)
{
foreach (Joint _Joint in _Body.Joints.Values)
{
if (TrackingState.Tracked == _Joint.TrackingState)
{
Ellipse _Ellipse = new Ellipse();
_Ellipse.Stroke = Brushes.Green;
_Ellipse.Fill = Brushes.Green;
_Ellipse.Width = 20;
_Ellipse.Height = 20;
ColorSpacePoint _ColorSpacePoint = Sensor.CoordinateMapper.MapCameraPointToColorSpace(_Joint.Position);
Canvas.SetLeft(_Ellipse, _ColorSpacePoint.X);
Canvas.SetTop(_Ellipse, _ColorSpacePoint.Y);
KinectCanvas.Children.Add(_Ellipse);
}
}
if (FrameEdges.Top == (FrameEdges.Top & _Body.ClippedEdges))
{
CreateClippingLine(0, 0, KinectCanvas.ActualWidth, 0);
}
if (FrameEdges.Left == (FrameEdges.Left & _Body.ClippedEdges))
{
CreateClippingLine(0, 0, 0, KinectCanvas.ActualHeight);
}
if (FrameEdges.Bottom == (FrameEdges.Bottom & _Body.ClippedEdges))
{
CreateClippingLine(0, KinectCanvas.ActualHeight, KinectCanvas.ActualWidth, KinectCanvas.ActualHeight);
}
if (FrameEdges.Right == (FrameEdges.Right & _Body.ClippedEdges))
{
CreateClippingLine(KinectCanvas.ActualWidth, 0, KinectCanvas.ActualWidth, KinectCanvas.ActualHeight);
}
}
}
}
}
static void Main()
{
Application.EnableVisualStyles();
Application.SetCompatibleTextRenderingDefault(false);
RenderForm form = new RenderForm("Kinect multiple hd faces projected to rgb");
RenderDevice device = new RenderDevice(SharpDX.Direct3D11.DeviceCreationFlags.BgraSupport | DeviceCreationFlags.Debug);
RenderContext context = new RenderContext(device);
DX11SwapChain swapChain = DX11SwapChain.FromHandle(device, form.Handle);
VertexShader vertexShader = ShaderCompiler.CompileFromFile<VertexShader>(device, "ProjectedTextureHdFaceView.fx", "VS_Simple");
VertexShader vertexShaderIndexed = ShaderCompiler.CompileFromFile<VertexShader>(device, "ProjectedTextureHdFaceView.fx", "VS_Indexed");
PixelShader pixelShader = ShaderCompiler.CompileFromFile<PixelShader>(device, "ProjectedTextureHdFaceView.fx", "PS");
int maxFaceCount = Consts.MaxBodyCount;
int faceVertexCount = (int)Microsoft.Kinect.Face.FaceModel.VertexCount;
var vertRgbTempBuffer = new ColorSpacePoint[faceVertexCount];
ColorSpacePoint[] facePoints = new ColorSpacePoint[faceVertexCount * maxFaceCount];
DX11StructuredBuffer lookupBuffer = DX11StructuredBuffer.CreateDynamic<uint>(device, maxFaceCount);
//Note : since in this case we use instancing, we only need a buffer for a single face
HdFaceIndexBuffer faceIndexBuffer = new HdFaceIndexBuffer(device, 1);
DynamicRgbSpaceFaceStructuredBuffer faceRgbBuffer = new DynamicRgbSpaceFaceStructuredBuffer(device, maxFaceCount);
KinectSensor sensor = KinectSensor.GetDefault();
sensor.Open();
bool doQuit = false;
bool invalidateFace = false;
KinectSensorBodyFrameProvider provider = new KinectSensorBodyFrameProvider(sensor);
BodyTrackingProcessor bodyTracker = new BodyTrackingProcessor();
MultipleHdFaceProcessor multiFace = new MultipleHdFaceProcessor(sensor, bodyTracker, maxFaceCount);
form.KeyDown += (sender, args) => { if (args.KeyCode == Keys.Escape) { doQuit = true; } };
bool uploadColor = false;
ColorRGBAFrameData currentData = null;
DynamicColorRGBATexture colorTexture = new DynamicColorRGBATexture(device);
KinectSensorColorRGBAFrameProvider colorProvider = new KinectSensorColorRGBAFrameProvider(sensor);
colorProvider.FrameReceived += (sender, args) => { currentData = args.FrameData; uploadColor = true; };
provider.FrameReceived += (sender, args) =>
{
bodyTracker.Next(args.FrameData);
};
multiFace.OnFrameResultsChanged += (sender, args) =>
{
invalidateFace = true;
};
RenderLoop.Run(form, () =>
{
if (doQuit)
{
form.Dispose();
return;
}
if (invalidateFace)
{
int offset = 0;
foreach (var data in multiFace.CurrentResults)
{
var vertices = data.FaceModel.CalculateVerticesForAlignment(data.FaceAlignment).ToArray();
sensor.CoordinateMapper.MapCameraPointsToColorSpace(vertices, vertRgbTempBuffer);
Array.Copy(vertRgbTempBuffer, 0, facePoints, offset, faceVertexCount);
offset += faceVertexCount;
}
faceRgbBuffer.Copy(context, facePoints, multiFace.CurrentResults.Count * faceVertexCount);
invalidateFace = false;
}
if (uploadColor)
{
colorTexture.Copy(context, currentData);
uploadColor = false;
}
context.Context.ClearRenderTargetView(swapChain.RenderView, SharpDX.Color.Black);
context.RenderTargetStack.Push(swapChain);
context.Context.Rasterizer.State = device.RasterizerStates.BackCullSolid;
context.Context.OutputMerger.BlendState = device.BlendStates.Disabled;
device.Primitives.ApplyFullTri(context, colorTexture.ShaderView);
device.Primitives.FullScreenTriangle.Draw(context);
if (multiFace.CurrentResults.Count > 0)
{
context.Context.VertexShader.SetShaderResource(0, faceRgbBuffer.ShaderView);
context.Context.PixelShader.SetSampler(0, device.SamplerStates.LinearClamp);
context.Context.PixelShader.SetShaderResource(0, colorTexture.ShaderView);
if (multiFace.CurrentResults.Count > 1)
{
uint[] buffer = new uint[multiFace.CurrentResults.Count];
for (uint i = 0; i < multiFace.CurrentResults.Count; i++)
{
buffer[i] = (uint)((i + 1) % multiFace.CurrentResults.Count);
}
lookupBuffer.WriteData(context, buffer);
context.Context.VertexShader.Set(vertexShaderIndexed);
context.Context.VertexShader.SetShaderResource(1, lookupBuffer.ShaderView);
}
else
{
context.Context.VertexShader.Set(vertexShader);
}
context.Context.PixelShader.Set(pixelShader);
//Attach index buffer, null topology since we fetch
faceIndexBuffer.AttachWithLayout(context);
faceIndexBuffer.DrawInstanced(context, multiFace.CurrentResults.Count);
}
context.RenderTargetStack.Pop();
swapChain.Present(0, SharpDX.DXGI.PresentFlags.None);
});
swapChain.Dispose();
context.Dispose();
device.Dispose();
colorProvider.Dispose();
colorTexture.Dispose();
faceIndexBuffer.Dispose();
faceRgbBuffer.Dispose();
provider.Dispose();
pixelShader.Dispose();
vertexShader.Dispose();
vertexShaderIndexed.Dispose();
lookupBuffer.Dispose();
sensor.Close();
}
/// <summary>
/// Kinectにつないでる時だけ使えるコンストラクタ
/// </summary>
/// <param name="originalCoordinateMapper"></param>
/// <param name="depthWidth"></param>
/// <param name="depthHeight"></param>
public LocalCoordinateMapper(CoordinateMapper originalCoordinateMapper, int depthWidth, int depthHeight)
{
this.depthWidth = depthWidth;
this.depthHeight = depthHeight;
this.depthFrameToCameraSpaceTable = originalCoordinateMapper.GetDepthFrameToCameraSpaceTable();
int length = depthWidth * depthHeight;
ushort[] depthBuffer = new ushort[length];
ColorSpacePoint[] colorSpacePoints = new ColorSpacePoint[length];
this.depthFrameToColorSpacfeTable = new PointF[length];
int depth = 1500; // なんでもいい
for (int i = 0; i < length; i++)
{
depthBuffer[i] = (ushort)depth;
}
originalCoordinateMapper.MapDepthFrameToColorSpace(depthBuffer, colorSpacePoints);
for (int i = 0; i < length; i++)
{
PointF tempP = new PointF();
tempP.X = (float)(colorSpacePoints[i].X - D / depth);
tempP.Y = colorSpacePoints[i].Y;
this.depthFrameToColorSpacfeTable[i] = tempP;
}
Debug.WriteLine(1);
}
//Processes the Frame data from the Kinect camera.
//Since events are called synchronously, this would bottleneck and cause an issue with framerate
//By threading, we process the info on seperate threads, allowing execution to coninue with the rest of the game
private void ProcessRGBVideo(ColorFrameReference aReference, BodyIndexFrameReference bifRef, DepthFrameReference depthRef)
{
using (ColorFrame colorImageFrame = aReference.AcquireFrame())
{
if (colorImageFrame != null)
{
using (BodyIndexFrame bodyIndexFrame = bifRef.AcquireFrame())
{
if (bodyIndexFrame != null)
{
using (DepthFrame depthFrame = depthRef.AcquireFrame())
{
if (depthFrame != null)
{
int depthHeight = depthFrame.FrameDescription.Height;
int depthWidth = depthFrame.FrameDescription.Width;
int colorHeight = colorImageFrame.FrameDescription.Height;
int colorWidth = colorImageFrame.FrameDescription.Width;
ushort[] _depthData = new ushort[depthFrame.FrameDescription.Width * depthFrame.FrameDescription.Height];
byte[] _bodyData = new byte[bodyIndexFrame.FrameDescription.Width * bodyIndexFrame.FrameDescription.Height];
byte[] _colorData = new byte[colorImageFrame.FrameDescription.Width * colorImageFrame.FrameDescription.Height * 4];
ColorSpacePoint[] _colorPoints = new ColorSpacePoint[depthWidth * depthHeight];
depthFrame.CopyFrameDataToArray(_depthData);
bodyIndexFrame.CopyFrameDataToArray(_bodyData);
colorImageFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Rgba);
iSensor.CoordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);
Color[] color = new Color[depthWidth * depthHeight];
Color c;
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
int depthIndex = (y * depthHeight) + x;
byte player = _bodyData[depthIndex];
// Check whether this pixel belong to a human!!!
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);
int colorIndex = ((colorY * colorWidth) + colorX);
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
{
int displayIndex = colorIndex * 4;
c = new Color(_colorData[displayIndex + 0], _colorData[displayIndex + 1], _colorData[displayIndex + 2], 0xff);
color[depthIndex] = c;
}
}
}
}
if (iGraphicsDevice.IsDisposed) return;
var video = new Texture2D(iGraphicsDevice, depthWidth, depthHeight);
video.SetData(color);
lock (iVideoLock)
{
iRGBVideo = video;
}
}
}
}
}
}
}
}
public static System.Drawing.PointF projectedPoint(ColorSpacePoint[,] shortRange , ColorSpacePoint[,] longRange, int short_range, int long_range, Point3 p )
{
int x = (int)p.X;
int y = (int)p.Y;
if (x < 0 || x >= 512) return new System.Drawing.PointF();
if (y < 0 || y >= 424) return new System.Drawing.PointF();
var shortP = shortRange[x, y];
var longP = longRange[x, y];
float shortP_X = shortP.X;
float longP_X = longP.X;
var b = ( longP_X * long_range - shortP_X * short_range) / ( long_range - short_range );
var a = (shortP_X - b) * short_range ;
var p_x = a / p.Z + b;
float shortP_Y = shortP.Y;
float longP_Y = longP.Y;
b = (longP_Y * long_range - shortP_Y * short_range) / (long_range - short_range);
a = (shortP_Y - b) * short_range;
var p_y = a / p.Z + b;
return new System.Drawing.PointF(p_x, p_y);
}
