C# DepthImageFrame.MapFromSkeletonPoint方法代码示例

        /// <summary>
        /// 距離データをカラー画像に変換する
        /// </summary>
        /// <param name="kinect"></param>
        /// <param name="depthFrame"></param>
        /// <returns></returns>
        private void HeightMeasure( KinectSensor kinect, DepthImageFrame depthFrame, SkeletonFrame skeletonFrame )
        {
            ColorImageStream colorStream = kinect.ColorStream;
              DepthImageStream depthStream = kinect.DepthStream;

              // トラッキングされている最初のスケルトンを取得する
              // インデックスはプレイヤーIDに対応しているのでとっておく
              Skeleton[] skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
              skeletonFrame.CopySkeletonDataTo( skeletons );

              int playerIndex = 0;
              for ( playerIndex = 0; playerIndex < skeletons.Length; playerIndex++ ) {
            if ( skeletons[playerIndex].TrackingState == SkeletonTrackingState.Tracked ) {
              break;
            }
              }
              if ( playerIndex == skeletons.Length ) {
            return;
              }

              // トラッキングされている最初のスケルトン
              Skeleton skeleton = skeletons[playerIndex];

              // 実際のプレイヤーIDは、スケルトンのインデックス+1
              playerIndex++;

              // 頭、足先がトラッキングされていない場合は、そのままRGBカメラのデータを返す
              Joint head = skeleton.Joints[JointType.Head];
              Joint leftFoot = skeleton.Joints[JointType.FootLeft];
              Joint rightFoot = skeleton.Joints[JointType.FootRight];
              if ( (head.TrackingState != JointTrackingState.Tracked) ||
               (leftFoot.TrackingState != JointTrackingState.Tracked) ||
               (rightFoot.TrackingState != JointTrackingState.Tracked) ) {
             return;
              }

              // 距離カメラのピクセルごとのデータを取得する
              short[] depthPixel = new short[depthFrame.PixelDataLength];
              depthFrame.CopyPixelDataTo( depthPixel );

              // 距離カメラの座標に対応するRGBカメラの座標を取得する(座標合わせ)
              ColorImagePoint[] colorPoint = new ColorImagePoint[depthFrame.PixelDataLength];
              kinect.MapDepthFrameToColorFrame( depthStream.Format, depthPixel,
            colorStream.Format, colorPoint );

              // 頭のてっぺんを探す
              DepthImagePoint headDepth = depthFrame.MapFromSkeletonPoint( head.Position );
              int top = 0;
              for ( int i = 0; (headDepth.Y - i) > 0; i++ ) {
            // 一つ上のY座標を取得し、プレイヤーがいなければ、現在の座標が最上位
            int index = ((headDepth.Y - i) * depthFrame.Width) + headDepth.X;
            int player = depthPixel[index] & DepthImageFrame.PlayerIndexBitmask;
            if ( player == playerIndex ) {
              top = i;
            }
              }

              // 頭のてっぺんを3次元座標に戻し、足の座標(下にあるほう)を取得する
              // この差分で身長を測る
              head.Position = depthFrame.MapToSkeletonPoint( headDepth.X, headDepth.Y - top );
              Joint foot = (leftFoot.Position.Y < rightFoot.Position.Y) ? leftFoot : rightFoot;

              // 背丈を表示する
              DrawMeasure( kinect, colorStream, head, foot );
        }

        private Point GetPosition2DLocation(DepthImageFrame depthFrame, SkeletonPoint skeletonPoint)
        {
            DepthImagePoint depthPoint = depthFrame.MapFromSkeletonPoint(skeletonPoint);

            ColorImagePoint colorPoint = depthFrame.MapToColorImagePoint(depthPoint.X, depthPoint.Y, this._sensor.ColorStream.Format);

            // map back to skeleton.Width & skeleton.Height
            //return new Point(
            //    (int)(this.RenderSize.Width * colorPoint.X / this._sensor.ColorStream.FrameWidth),
            //    (int)(this.RenderSize.Height * colorPoint.Y / this._sensor.ColorStream.FrameHeight));
            return new Point(
                (int)(canvas1.Width * colorPoint.X / this._sensor.ColorStream.FrameWidth),
                (int)(canvas1.Height * colorPoint.Y / this._sensor.ColorStream.FrameHeight));
        }

        private double GetRealLength(DepthImageFrame depthFrame, SkeletonPoint skeletonPoint1,SkeletonPoint skeletonPoint2)
        {
            DepthImagePoint depthPoint1 = depthFrame.MapFromSkeletonPoint(skeletonPoint1);
            DepthImagePoint depthPoint2 = depthFrame.MapFromSkeletonPoint(skeletonPoint2);

            double depth = (depthPoint1.Depth+depthPoint2.Depth)/2;
            //double lengthPixel = Math.Sqrt((depthPoint1.X-depthPoint2.X)^2+(depthPoint1.Y-depthPoint2.Y)^2);

            double lengthPixel = Math.Abs(depthPoint1.Y - depthPoint2.Y);
            double frameLengthReal = 2 * depth * Math.Tan(57 / 2 * Math.PI / 180);

            // lengthReal/frameLengthReal = lengthPixel/frameLengthPixel
            double lengthReal = lengthPixel * frameLengthReal / depthFrame.Height;
            return lengthReal;
        }

        /// <summary>
        /// Check queue if right hand and elbow positions are stable
        /// under the condition that Status is DepthDetecting.
        /// If is stable, the Status will be changed to next after doing depth calibration.
        /// If not, nothing will be changed.
        /// </summary>
        public void DoDepthDetecting(KinectSensor sensor, DepthImageFrame depthFrame)
        {
            if (Status == MainStatus.DepthDetecting)
            {
                if (IsStable(rightHandQueue, DEPTH_HAND_TOLERANCE, DEPTH_HAND_COUNT))
                {
                    // if is stable, calibrate depth according to avg of position near hand
                    SkeletonPoint handPoint = rightHandQueue.Last().Position;
                    DepthImagePoint centerDepthPoint = sensor.MapSkeletonPointToDepth(
                        handPoint, sensor.DepthStream.Format);
                    int[] depthArr = new int[DEPTH_NEAR_COUNT * 4];
                    int index = 0;
                    for (int i = 0; i < DEPTH_NEAR_COUNT; ++i)
                    {
                        // top
                        SkeletonPoint topSke = depthFrame.MapToSkeletonPoint(
                            centerDepthPoint.X - DEPTH_NEAR_RADIUS + (int)(DEPTH_SPAN * i),
                            centerDepthPoint.Y - DEPTH_NEAR_RADIUS);
                        depthArr[index++] = depthFrame.MapFromSkeletonPoint(topSke).Depth;
                        // bottom
                        SkeletonPoint bottomSke = depthFrame.MapToSkeletonPoint(
                            centerDepthPoint.X - DEPTH_NEAR_RADIUS + (int)(DEPTH_SPAN * i),
                            centerDepthPoint.Y + DEPTH_NEAR_RADIUS);
                        depthArr[index++] = depthFrame.MapFromSkeletonPoint(bottomSke).Depth;
                        // left
                        SkeletonPoint leftSke = depthFrame.MapToSkeletonPoint(
                            centerDepthPoint.X - DEPTH_NEAR_RADIUS,
                            centerDepthPoint.Y - DEPTH_NEAR_RADIUS + (int)(DEPTH_SPAN * i));
                        depthArr[index++] = depthFrame.MapFromSkeletonPoint(leftSke).Depth;
                        // right
                        SkeletonPoint rightSke = depthFrame.MapToSkeletonPoint(
                            centerDepthPoint.X + DEPTH_NEAR_RADIUS,
                            centerDepthPoint.Y - DEPTH_NEAR_RADIUS + (int)(DEPTH_SPAN * i));
                        depthArr[index++] = depthFrame.MapFromSkeletonPoint(rightSke).Depth;
                    }
                    // set median(rather than mean) depth of the list
                    Array.Sort(depthArr);
                    kCalibrator.SetDepth(depthArr[DEPTH_NEAR_COUNT / 2]);

                    ClearQueue();
                    Status = MainStatus.EdgeDetecting;
                    mainWindow.textBlock.Text = "Detecting edge now. Arm at four corners of the screen.";
                    // cooling time timer
                    edgeTimer.Start();
                    edgeCooling = true;
                }
            }
        }

        public void ProcessFrame(KinectSensor sensor, byte[] colorImage, ColorImageFormat colorImageFormat, DepthImageFrame depthFrame, short[] depthImage, DepthImageFormat depthImageFormat, Skeleton[] skeletonData, SkeletonFrame skeletonFrame)
        {
            //Console.WriteLine("N: ---------");
            coordinates.Clear();
            int detectedFace = 0;
            int trackedSkeletonsCount = 0;

            int playerIndex = -1;
            for (int i = 0; i < skeletonData.Length; i++)
            //foreach (Skeleton skeleton in skeletonData)
            {
                Skeleton skeleton = skeletonData[i];
                if (skeleton.TrackingState == SkeletonTrackingState.Tracked
                    || skeleton.TrackingState == SkeletonTrackingState.PositionOnly)
                {
                    // We want keep a record of any skeleton, tracked or untracked.
                    if (!trackedSkeletons.ContainsKey(skeleton.TrackingId))
                    {
                        trackedSkeletons.Add(skeleton.TrackingId, new SkeletonFaceTracker());
                    }

                    DepthImagePoint depthPoint = depthFrame.MapFromSkeletonPoint(skeleton.Joints[JointType.Head].Position);
                    ColorImagePoint colorPoint = depthFrame.MapToColorImagePoint(depthPoint.X, depthPoint.Y, colorImageFormat);

                    Coordinates2D c = new Coordinates2D();

                    playerIndex = i + 1;

                    c.X = colorPoint.X;
                    c.Y = colorPoint.Y;
                    c.Width = 0;
                    c.Height = 0;
                    c.LeftEyeX = 0;
                    c.LeftEyeY = 0;
                    c.RightEyeX = 0;
                    c.RightEyeY = 0;
                    c.PlayerIndex = playerIndex;

                    trackedSkeletonsCount++;

                    // Give each tracker the upated frame.
                    SkeletonFaceTracker skeletonFaceTracker;
                    if (!scannedIdentities.Contains(skeleton.TrackingId) &&
                        detectedFace < 1 &&
                        trackedSkeletons.TryGetValue(skeleton.TrackingId, out skeletonFaceTracker))
                    {
                        detectedFace++;
                        scannedIdentities.Add(skeleton.TrackingId);

                        skeletonFaceTracker.OnFrameReady(sensor, colorImageFormat, colorImage, depthImageFormat, depthImage, skeleton);
                        skeletonFaceTracker.LastTrackedFrame = skeletonFrame.FrameNumber;
                        Coordinates2D? realCoords = skeletonFaceTracker.GetFaceCoordinates();
                        if (realCoords.HasValue)
                        {
                            c = realCoords.Value;
                            c.PlayerIndex = playerIndex;
                        }
                    }

                    c.TrackingId = skeleton.TrackingId;
                    coordinates.Add(c);
                }
            }

            if (scannedIdentities.Count > 0 && scannedIdentities.Count >= trackedSkeletonsCount)
            {
                scannedIdentities.Clear();
                //Console.WriteLine("Clearing");
            }

            RemoveOldTrackers(skeletonFrame.FrameNumber);

            //if (coordinates.Count > 0)
            {
                int[] identities = new int[coordinates.Count];

              //  stopwatch.Reset();
              //  stopwatch.Start();
                double[] distances = new double[coordinates.Count * 8];
                this.
                 ProcessImage(colorImage, GetWidth(colorImageFormat), GetHeight(colorImageFormat), depthImage, 640, 480, coordinates.ToArray(), identities, distances);
              //  stopwatch.Stop();
             //       foreach (int i in identities)
             //       {
             //           Console.WriteLine("Recognized: {0} (in {1} millis - {2} ticks)", i, stopwatch.ElapsedMilliseconds, stopwatch.ElapsedTicks);
             //       }
            }
        }

        /// <summary>
        /// Convert real-world positions to 2D image coords.
        /// </summary>
        /// <param name="depthFrame">Datth frame used to conversion.</param>
        /// <param name="skeletonPosition">Point which position should be converted.</param>
        /// <returns>Point in 2D image.</returns>
        private Point GetPosition2DLocation(DepthImageFrame depthFrame, SkeletonPoint skeletonPosition)
        {
            DepthImagePoint depthPoint = depthFrame.MapFromSkeletonPoint(skeletonPosition);
            ColorImagePoint colorPoint = depthFrame.MapToColorImagePoint(depthPoint.X, depthPoint.Y, kinect.ColorStream.Format);

            // map back to skeleton.Width & skeleton.Height
            return new Point(colorPoint.X, colorPoint.Y);
        }

        /// <summary>
        /// get a joint display position
        /// </summary>
        /// <param name="joint"></param>
        /// <returns></returns>
        private Point getDisplayPosition(DepthImageFrame depthFrame, Joint joint)
        {
            DepthImagePoint depthPoint = depthFrame.MapFromSkeletonPoint(joint.Position);

            return new Point(
                        (int)(userCanvas.Width * depthPoint.X / depthFrame.Width),
                        (int)(userCanvas.Height * depthPoint.Y / depthFrame.Height));
        }

        private Point GetPosition2DLocation(DepthImageFrame depthFrame, SkeletonPoint skeletonPoint)
        {
            DepthImagePoint depthPoint = depthFrame.MapFromSkeletonPoint(skeletonPoint);
            return new Point(
                        (int)(this.RenderSize.Width * depthPoint.X / depthFrame.Width),
                        (int)(this.RenderSize.Height * depthPoint.Y / depthFrame.Height));
            /*
            switch (ImageType)
            {
                case ImageType.Color:
                    ColorImagePoint colorPoint = depthFrame.MapToColorImagePoint(depthPoint.X, depthPoint.Y, kinectSensor.ColorStream.Format);

                    // map back to skeleton.Width & skeleton.Height
                    return new Point(
                        (int)(this.RenderSize.Width * colorPoint.X / kinectSensor.ColorStream.FrameWidth),
                        (int)(this.RenderSize.Height * colorPoint.Y / kinectSensor.ColorStream.FrameHeight));
                case ImageType.Depth:
                    return new Point(
                        (int)(this.RenderSize.Width * depthPoint.X / depthFrame.Width),
                        (int)(this.RenderSize.Height * depthPoint.Y / depthFrame.Height));
                default:
                    throw new ArgumentOutOfRangeException("ImageType was a not expected value: " + ImageType.ToString());
            }
            */
        }

 public Point MapToPoint(SkeletonPoint s, DepthImageFrame depth)
 {
     DepthImagePoint dp = depth.MapFromSkeletonPoint(s);
     ColorImagePoint cp = depth.MapToColorImagePoint(dp.X, dp.Y, ColorImageFormat.RgbResolution640x480Fps30);
     return new Point(cp.X, cp.Y);
 }

        private int[] CalculatePlayerPositions(SkeletonFrame skeletonFrame, DepthImageFrame depthFrame)
        {
            Skeleton[] allskeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
            int[] pointdata = new int[skeletonFrame.SkeletonArrayLength * 3];
            skeletonFrame.CopySkeletonDataTo(allskeletons);
            int k = 0;

            foreach (Skeleton skeleton in allskeletons)
            {
                if (skeleton.TrackingState == SkeletonTrackingState.Tracked)
                {
                    Joint jointpoint = this.BestTrackedJoint(skeleton);
                    if (jointpoint.TrackingState == JointTrackingState.Tracked)
                    {
                        var depthImagePoint = depthFrame.MapFromSkeletonPoint(jointpoint.Position);
                        pointdata[k] = depthImagePoint.Depth;
                        pointdata[k + 1] = depthImagePoint.X;
                        pointdata[k + 2] = depthImagePoint.Y;
                    }
                    else
                    {
                        pointdata[k] = 0;
                        pointdata[k + 1] = 0;
                        pointdata[k + 2] = 0;
                    }
                }
                else
                {
                    pointdata[k] = 0;
                    pointdata[k + 1] = 0;
                    pointdata[k + 2] = 0;
                }

                k += 3;
            }

            return pointdata;
        }