/// <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); }
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); }