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