/// <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);
*/
}
}
public Vector2 MapDepthPointToColorCoords(KinectInterop.SensorData sensorData, Vector2 depthPos, ushort depthVal)
{
Vector2 vPoint = Vector2.zero;
if (_coordinateMapper != null && _depthCameraIntrinsics != null && _colorCameraIntrinsics != null && depthPos != Vector2.zero)
{
System.Numerics.Vector2 depthPoint = new System.Numerics.Vector2(depthPos.x, depthPos.y);
System.Numerics.Vector3 depthSpace = _depthCameraIntrinsics.UnprojectFromFrame(depthPoint, (float)depthVal / 1000f);
System.Numerics.Vector3 colorSpace = _coordinateMapper.MapDepthSpacePointToColor(depthSpace);
System.Numerics.Vector2 colorPoint = _colorCameraIntrinsics.ProjectOntoFrame(colorSpace);
vPoint = new Vector2(colorPoint.X, colorPoint.Y);
}
return(vPoint);
}
public Vector2 MapSpacePointToDepthCoords(KinectInterop.SensorData sensorData, Vector3 spacePos)
{
Vector2 vPoint = Vector2.zero;
if (_depthCameraIntrinsics != null)
{
System.Numerics.Vector3 camPoint = new System.Numerics.Vector3(spacePos.x, spacePos.y, spacePos.z);
System.Numerics.Vector2 depthPoint = _depthCameraIntrinsics.ProjectOntoFrame(camPoint);
if (depthPoint.X >= 0 && depthPoint.X < sensorData.depthImageWidth &&
depthPoint.Y >= 0 && depthPoint.Y < sensorData.depthImageHeight)
{
vPoint = new Vector2(depthPoint.X, depthPoint.Y);
}
}
return(vPoint);
}
private void TrackHand(Joint handJoint, HandState state, CameraIntrinsics intrinsics, Func <Vector3, Vector3> coordinateTransformation, double xRatio, double yRatio)
{
if (handJoint.PositionTrackingState != TrackingState.Tracked)
{
return;
}
SolidColorBrush handBrush = null;
// add hand tracking
switch (state)
{
case HandState.Open:
handBrush = new SolidColorBrush(Colors.Blue);
break;
case HandState.Lasso:
handBrush = new SolidColorBrush(Colors.Green);
break;
case HandState.Closed:
handBrush = new SolidColorBrush(Colors.Red);
break;
}
if (handBrush != null)
{
var circle = new Ellipse();
circle.Width = 40;
circle.Height = 40;
circle.Opacity = 50;
circle.Fill = handBrush;
Children.Add(circle);
var colorSpace = coordinateTransformation(handJoint.Position);
var colorFramePoint = intrinsics.ProjectOntoFrame(colorSpace);
// TODO: attached propertis do no set the center of the elipse
SetLeft(circle, colorFramePoint.X * xRatio - 20);
SetTop(circle, colorFramePoint.Y * yRatio - 20);
}
}