如何在WPF画布上的Kinect空间中准确绘制3D点？

Question

I am developing a Kinect (v1) application where I want to be able to detect the floor position of the user relative to the user's head.

I want to draw a line in WPF on the canvas which goes from the head of the user, and ends at the point on the floor plane which is closest to the head point (this should be a completely vertical line).

I am however new to using WPF, canvas and Kinect, and I am having a scaling issue where the line ends much further down than it should (outside of the Kinect feed's grid). The reason I believe for it to be a scaling issue is because it is completely vertical and does move up and down correctly when I walk closer or further from the camera.

This is the main body of code for this operation:

SkeletonPoint headPos = currentSkeleton.Joints[JointType.Head].Position;
SkeletonPoint floorPoint = getSkeletonFloorPoint(headPos, floorPlane);     
CoordinateMapper mapper = new CoordinateMapper(KinectSensor);

ColorImagePoint colorHeadPoint = mapper.MapSkeletonPointToColorPoint(headPos, ColorImageFormat.RgbResolution640x480Fps30);
Point mappedHeadPoint = new Point(colorHeadPoint.X, colorHeadPoint.Y);


ColorImagePoint colorFloorPoint = mapper.MapSkeletonPointToColorPoint(floorPoint, ColorImageFormat.RgbResolution640x480Fps30);
Point mappedFloorPoint = new Point(colorFloorPoint.X, colorFloorPoint.Y);


drawUserPosition(userPos, mappedHeadPoint, mappedFloorPoint);

This the method which computes the floor point based on the head position:

private SkeletonPoint getSkeletonFloorPoint(SkeletonPoint skeletonHeadPos, Tuple<float, float, float, float> floorPlane)
{
    Point3D headPos = new Point3D(skeletonHeadPos.X, skeletonHeadPos.Y, skeletonHeadPos.Z);
    Point3D pointOnPlane = new Point3D(0, (double) -(floorPlane.Item2 / floorPlane.Item4), 0);
    Vector3D planeNorm = new Vector3D(floorPlane.Item1, floorPlane.Item2, floorPlane.Item3);
    Vector3D planeToHead = Point3D.Subtract(headPos, pointOnPlane);

    double dist = Vector3D.DotProduct(planeNorm, planeToHead);
    Point3D floorPoint = Vector3D.Add(Vector3D.Multiply(-dist, planeNorm), headPos);

    SkeletonPoint skeletonFloorPoint = new SkeletonPoint();
    skeletonFloorPoint.X = (float) floorPoint.X;
    skeletonFloorPoint.Y = (float) floorPoint.Y;
    skeletonFloorPoint.Z = (float) floorPoint.Z;

    return skeletonFloorPoint;
}

This sets the line position:

private void drawUserPosition(Line userPos, Point headPoint, Point floorPoint)
{
    userPos.X1 = headPoint.X;
    userPos.Y1 = headPoint.Y;
    userPos.X2 = floorPoint.X;
    userPos.Y2 = floorPoint.Y;

}

This is the XAML for the canvas:

<Canvas>
    <kt:KinectSkeletonViewer 
        KinectSensorManager="{Binding KinectSensorManager}"
        Visibility="{Binding KinectSensorManager.ColorStreamEnabled, Converter={StaticResource BoolToVisibilityConverter}}"
        Width="{Binding ElementName=ColorViewer,Path=ActualWidth}"
        Height="{Binding ElementName=ColorViewer,Path=ActualHeight}"
        ImageType="Color" />

    <Line X1="0" Y1="0" X2="0" Y2="0" Name="userPos" Stroke="Red" StrokeThickness="2" />

</Canvas>

As I said, I think the geometry is correct, I just don't have any experience with WPF so I am fairly certain that is where I am going wrong.

Thank you for any help, and if there are any details I can provide to make this easier to solve I will be happy to provide them.

Answer 1

Sorry, just a stupid mistake by me.

When trying to find an arbitrary point on the floor plane I accidentally flipped two values in a division:

Point3D pointOnPlane = new Point3D(0, (double) -(floorPlane.Item2 / floorPlane.Item4), 0);

This is the correct code:

Point3D pointOnPlane = new Point3D(0, (double) -(floorPlane.Item4 / floorPlane.Item2), 0);