Картографирование координат Kinect V2

Question

У меня проблемы с правильным нанесением структуры скелета поверх тела субъекта. (BodyFrame с ColorFrame). Я попытался поэкспериментировать с DepthSpacePoint и ColorSpacePoint, но все еще не смог получить правильное выравнивание скелета по телу.

Вот изображение неправильного вывода: скелет над выводом тела

Это код для рисования скелета:

bool dataReceived = false;


if (bodyFrame != null)
{
    if (this.bodies == null)
    {
        this.bodies = new Body[bodyFrame.BodyCount];
    }

    bodyFrame.GetAndRefreshBodyData(this.bodies);
    dataReceived = true;
}


if (dataReceived)
{
    using (DrawingContext dc = this.drawingGroup.Open())
    {
        dc.DrawRectangle(Brushes.Transparent, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));

        foreach (Body body in this.bodies)
        {
            Pen drawPen = this.bodyColor;

            if (body.IsTracked)
            {
                IReadOnlyDictionary<JointType, Joint> joints = body.Joints;

                Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();

                foreach (JointType jointType in joints.Keys)
                {
                    CameraSpacePoint position = joints[jointType].Position;
                    if (position.Z < 0)
                    {
                        position.Z = InferredZPositionClamp;
                    }

                    DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
                    //ColorSpacePoint colorSpacePoint = this.coordinateMapper.MapCameraPointToColorSpace(position);
                    jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
                }

                this.DrawBody(joints, jointPoints, dc, drawPen);

                if (!this.status_ready)
                {
                    this.DrawHand(body.HandLeftState, body.HandRightState, jointPoints[JointType.HandLeft], dc, joints);
                    this.DrawHand(body.HandLeftState, body.HandRightState, jointPoints[JointType.HandRight], dc, joints);
                }
            }
        }
        this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
    }
}

Это код для рисования цветного изображения:

FrameDescription colorFrameDescription = colorFrame.FrameDescription;

using (KinectBuffer colorBuffer = colorFrame.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))
    {
        colorFrame.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();
}