public Vector3 MapDepthPointToSpaceCoords(KinectInterop.SensorData sensorData, Vector2 depthPos, ushort depthVal) { Vector3 vPoint = Vector3.zero; if (_depthCameraIntrinsics != null && depthPos != Vector2.zero) { System.Numerics.Vector2 depthPoint = new System.Numerics.Vector2(depthPos.x, depthPos.y); System.Numerics.Vector3 camPoint = _depthCameraIntrinsics.UnprojectFromFrame(depthPoint, (float)depthVal / 1000f); vPoint = new Vector3(camPoint.X, camPoint.Y, camPoint.Z); } return(vPoint); }
/// <summary> /// For DEBUG purposes only. Implementation / Output may change in the future. /// </summary> public void FillBodies(IEnumerable <Body> bodies, CameraIntrinsics cameraIntrinsics, Func <Vector3, Vector3> coordinateTransformation) { Children.Clear(); if (bodies == null) { return; } foreach (var body in bodies.Where(b => b.IsTracked)) { var brush = new SolidColorBrush(Colors.Green); var xRatio = ActualWidth / cameraIntrinsics.FrameWidth; var yRatio = ActualHeight / cameraIntrinsics.FrameHeight; //create skeleton foreach (var bone in body.CreateSkeleton().Where(bone => bone.TrackingState == TrackingState.Tracked)) { var colorSpace = coordinateTransformation(bone.Joint1.Position); /* * var origColorFramePoint = cameraIntrinsics.OriginalIntrinsics.ProjectOntoFrame(colorSpace); * var distortedOrig = cameraIntrinsics.OriginalIntrinsics.DistortPoint(origColorFramePoint); */ var colorFramePoint = cameraIntrinsics.ProjectOntoFrame(colorSpace); var unprojectedPoint = cameraIntrinsics.UnprojectFromFrame(colorFramePoint, colorSpace.Z); var line = new Line(); line.StrokeThickness = 4; line.Stroke = brush; line.X1 = colorFramePoint.X * xRatio; line.Y1 = colorFramePoint.Y * yRatio; colorSpace = coordinateTransformation(bone.Joint2.Position); colorFramePoint = cameraIntrinsics.ProjectOntoFrame(colorSpace); line.X2 = colorFramePoint.X * xRatio; line.Y2 = colorFramePoint.Y * yRatio; Children.Add(line); } // track hands TrackHand(body.Joints[JointType.HandRight], body.HandStateRight, cameraIntrinsics, coordinateTransformation, xRatio, yRatio); TrackHand(body.Joints[JointType.HandLeft], body.HandStateLeft, cameraIntrinsics, coordinateTransformation, xRatio, yRatio); // clipped edges /* * DrawClipEdge(body.ClippedEdges & FrameEdges.Top); * DrawClipEdge(body.ClippedEdges & FrameEdges.Bottom); * DrawClipEdge(body.ClippedEdges & FrameEdges.Left); * DrawClipEdge(body.ClippedEdges & FrameEdges.Right); */ } }