インテル® RealSense™ 3Dカメラ でHand Tracking(実装編)

しばらく前に書きました
[インテル® RealSense™ 3Dカメラ でHand Tracking](http://qiita.com/n_kimura/items/fae0ab2edfc9968d7477)
↑こちらの続きとしてHandTrackingの実装をしましたので記事としてあげます。


#ソース
本記事の全ソースは以下に置きました。
https://github.com/aquaring/RealSenseTestCode
「RealSenseHandTracking」というプロジェクトです。

※ソースはあくまでテストコードですので、厳密にエラー処理を書いていませんし、
　カメラ制御の処理はクラスなどにせずxaml.csにべた書きにしています。
　あくまで基本的に使用方法を押さえる目的で書きました。


#実行結果
手の検出速度やジェスチャーの検出精度を見て頂きたくgifアニメーションにしました

![HandTracking動画](http://lab.aquaring.co.jp/contents/hand_tracking.gif)

gifアニなので、かなり画像は荒くなっていてすみませんが
・上の画像　　カラーカメラの映像に検出した手・指の位置に黄緑色の点を描画しています。
　　　　　　　以前の記事で書いた22点の情報です
・左下の画像　検出したジェスチャー名、ジェスチャーのイメージを表示しています。
　　　　　　　(イメージのグラデーションがgifアニなので完全に無くなってしまってますが)
・右下の画像　以前の記事で書いた「Blob」として認識した部分を白くしています

見て頂くとわかると思いますが
22点の検出速度としては思ったよりも早くて使えるかなぁという感じです。
(Leapと比べると少し反応が遅いですが画面操作とかに使う分には全然申し分ないかと思います。)
ジェスチャーについては誤検出はするものの、うまく条件を揃えれば実用に耐えるといった感じでしょうか。

#実装のポイント
##HandTrackingのオブジェクト生成、設定
```csharp:MainWindows.xaml.cs
// Hand Trackingを有効化と設定
m_Cm.EnableHand();
PXCMHandModule handModule = m_Cm.QueryHand();
PXCMHandConfiguration handConfig = handModule.CreateActiveConfiguration();
handConfig.SetTrackingMode(PXCMHandData.TrackingModeType.TRACKING_MODE_FULL_HAND);  // FULL_HANDモード
handConfig.EnableSegmentationImage(true);   // SegmentationImageの有効化
handConfig.EnableAllGestures();                 // すべてのジェスチャーを補足
handConfig.SubscribeGesture(OnFiredGesture);    // ジェスチャー発生時のコールバック関数をセット
handConfig.ApplyChanges();

// HandDataのインスタンスを作成
m_HandData = handModule.CreateOutput();
```
PXCMSenseManagerのインスタンスにEnableHand()してHandTrackingを有効化します。
PXCMHandConfigurationのインスタンスを生成してHandTrackingの設定をして最後にApplyChanges()で設定を反映します。

Handデータを取得するためにPXCMHandDataのインスタンスをここで取得しています。


##Handデータの取得
今までのカラーイメージの取得と同じように今回もTaskでループする形で取得します。
PXCMHandDataのUpdate()を呼んでHandデータを更新します。
(HandDataの更新がされた時にコールバックする書き方もできるようですが今回はしていません)

更新されたHandデータは、DisplayHandTrackingData()の中でデータを取得して画面に表示しています。
長いですが、ソースをのせておきます(詳細はコメントを見てもらえれば分かると思います・・・)

```csharp:MainWindows.xaml.cs
        /// <summary>
        /// HandTrackingで取得したHandDataを受け取り
        /// 画面にHandDataの情報を表示する
        /// </summary>
        private void DisplayHandTrackingData(PXCMHandData handData)
        {
            // SegmentationImageの情報格納用の変数
            int segWbStride = 0;
            Int32Rect segWbRect = new Int32Rect(0, 0, 0, 0);
            Byte[] segImageBuffer = null;

            // HandJointの情報格納用の変数
            List<List<PXCMHandData.JointData>> handJointsList = new List<List<PXCMHandData.JointData>>();

            for (int handIndex = 0; handIndex < handData.QueryNumberOfHands(); handIndex++)
            {
                // IHandDataを取得
                PXCMHandData.IHand iHandData;
                if (handData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, handIndex, out iHandData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
                {
                    // SegmentationImageを取得
                    PXCMImage image;
                    iHandData.QuerySegmentationImage(out image);    // 取得出来る画像は8bitGrayスケール画像 手の部分が0xff(白) 背景が0x00(黒)

                    // Imageから画像データを取得
                    PXCMImage.ImageData data = null ;
                    pxcmStatus sts = image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_Y8, out data);
                    if (sts == pxcmStatus.PXCM_STATUS_NO_ERROR)
                    {
                        // ImageDataが取得できたらWritableBitmapに書き込むためにバイト配列に変換する
                        int length = data.pitches[0] * image.info.height;
                        Byte[] tmpBuffer = data.ToByteArray(0, length);

                        // ストライドと描画領域を取得
                        segWbStride = data.pitches[0];
                        segWbRect = new Int32Rect(0, 0, image.info.width, image.info.height);

                        // Imageデータのアクセスを終了する
                        image.ReleaseAccess(data);

                        // HandSegmentationImageは複数ある可能性があるためすでにバイト配列を取得ずみの場合は重ね合わせる
                        if (segImageBuffer == null)
                        {
                            // まだない場合は、そのまま使用する
                            Array.Resize(ref segImageBuffer, tmpBuffer.Length);
                            tmpBuffer.CopyTo(segImageBuffer, 0);

                        }
                        else
                        {
                            // 既にひとつの手の情報がある場合は手の白部分(0xff)のみ重ね合わせる
                            for (int i=0; i<segImageBuffer.Length; i++)
                            {
                                segImageBuffer[i] = (byte)(segImageBuffer[i] | tmpBuffer[i]);
                            }
                        }
                    }

                    // TODO:後で取得してみる
                    //iHandData.QueryBoundingBoxImage   // 手の領域
                    //iHandData.QueryMassCenterImage    // 2D Image coordinatesでの手の中心座標
                    //iHandData.QueryMassCenterWorld    // 3D World Coordinatesでの手の中心座標
                    //iHandData.QueryExtremityPoint // TODO:Extremitiesモードで取得してみる

                    // 1つの手のJointを入れるListを生成
                    List<PXCMHandData.JointData> jointList = new List<PXCMHandData.JointData>();
                    
                    // 手のJoint座標を取得してListに格納
                    for (int jointIndex = 0; jointIndex < Enum.GetNames(typeof(PXCMHandData.JointType)).Length; jointIndex++)
                    {
                        // 手の1つのJoint座標を取得
                        PXCMHandData.JointData jointData;
                        iHandData.QueryTrackedJoint((PXCMHandData.JointType)jointIndex, out jointData);

                        jointList.Add(jointData);
                    }

                    // 作成した1つの手のJoint座標リストをListに格納
                    handJointsList.Add(jointList);
                }
            }

            // SegmentationImageデータをバイト配列にしたものをWriteableBitmapに書き込む
            if (segImageBuffer != null)
            {
                m_HandSegmentWBitmap.Dispatcher.BeginInvoke
                (
                    new Action(() =>
                    {
                        m_HandSegmentWBitmap.WritePixels(segWbRect, segImageBuffer, segWbStride, 0);
                    }
                ));
            }

            // HandJointの座標を画面に表示
            if (handJointsList.Count > 0)
            {
                m_ColorWBitmap.Dispatcher.BeginInvoke
                (
                    new Action(() =>
                    {
                        foreach (List<PXCMHandData.JointData> jointList in handJointsList)
                        {
                            foreach (PXCMHandData.JointData joint in jointList)
                            {

                                PXCMPoint3DF32[] depthPoint = new PXCMPoint3DF32[1];
                                
                                depthPoint[0].x = joint.positionImage.x;
                                depthPoint[0].y = joint.positionImage.y;
                                depthPoint[0].z = joint.positionWorld.z * 1000; // mmとpixcelを合わす

                                PXCMPointF32[] colorPoint = new PXCMPointF32[1];
                                pxcmStatus status = m_Projection.MapDepthToColor(depthPoint, colorPoint);

                                // 指の位置を描画                                
                                m_ColorWBitmap.FillEllipseCentered((int)colorPoint[0].x,
                                                                   (int)colorPoint[0].y,
                                                                   10, 10, Colors.YellowGreen);
                            }
                        }
                        
                    }
                ));
            }

            m_HandJointWBitmap.Dispatcher.BeginInvoke
            (
                new Action(() =>
                {
                    m_HandJointWBitmap.Clear();

                    foreach (List<PXCMHandData.JointData> jointList in handJointsList)
                    {
                        foreach (PXCMHandData.JointData joint in jointList)
                        {
                            m_HandJointWBitmap.FillEllipse(
                                               (int)joint.positionImage.x, (int)joint.positionImage.y,
                                               (int)joint.positionImage.x + 6, (int)joint.positionImage.y + 6, Colors.YellowGreen);
                        }
                    }
                }
            ));
            

        }
```

##カラー画像との座標変換
HandデータはDepthの3次元画像なので、これを画面に表示しているカラー画像の座標とは異なります。
これを変換するために「PXCMProjection」を使用します。

この部分でインスタンスを生成して

```csharp:MainWindows.xaml.cs
// 座標変換のためのProjectionインスタンスを取得
m_Projection = m_Cm.QueryCaptureManager().QueryDevice().CreateProjection();
```

この部分で座標の変換を行っています。

```csharp:MainWindows.xaml.cs
PXCMPoint3DF32[] depthPoint = new PXCMPoint3DF32[1];
                                
depthPoint[0].x = joint.positionImage.x;
depthPoint[0].y = joint.positionImage.y;
depthPoint[0].z = joint.positionWorld.z * 1000; // mmとpixcelを合わす

PXCMPointF32[] colorPoint = new PXCMPointF32[1];
pxcmStatus status = m_Projection.MapDepthToColor(depthPoint, colorPoint);
```



#所感
「Full Hand」の取得でも思ったよりもパフォーマンスも悪くないので十分実用に耐えるとおもいました。
ジェスチャーの検知については完全ではないので、インターフェースで促すなどが必要かなとおもいました。