Azure KinectのJump Analysisサンプルを読んでみた

3D Sensor Advent Calendar 2019の1日目の記事です。

この記事ではAzure Kinect Body Tracking SDKを利用したJump Analysis Sampleの要点をゆるーく解説します。
このサンプルプログラムは以下のリポジトリで公開されているなかの1つです。

* Azure Kinect DK Code Samples Repository | microsoft/Azure-Kinect-Samples   
  https://github.com/microsoft/Azure-Kinect-Samples

サンプルプログラムの概要
----------------------
Azure Kinect Body Tracking SDKで推定した姿勢を利用してジャンプの姿勢を解析するサンプルです。

1. 両手を頭より上に2秒ほど挙げてキャプチャを始める。
2. この状態でジャンプする。
3. 両手を頭より上に2秒ほど挙げてキャプチャを終わる。

以上の行程が終わるとジャンプの姿勢を解析して結果が表示されます。
姿勢は最も屈んだ姿勢、最も高く飛んだ姿勢が表示されます。
屈んだときの膝の角度、飛んだときの高さなどの数値も表示されます。

![2019-11-30_21h00_53.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/34834/47156583-14ac-27d9-5789-7317757026b9.png)


サンプルプログラムの要点
----------------------
このサンプルプログラムの実装の要点は以下の3つです。
これらの実装を読み解いて紹介していきます。

* 両手を挙げるジェスチャーの検出
* ジャンプの高さの検出
* 膝の角度の検出

両手を挙げるジェスチャーの検出
----------------------------
両手を挙げるジェスチャーは左手、右手、頭のジョイントの位置を利用します。
両手のジョイントのY軸の値と頭のジョイントのY軸の値と比較すればよさそうです。
Azure Kinect Body Tracking SDKではY軸の値は地面方向に向かって増加します。そのため、Y軸の値が小さい方が上になっています。
（ここでは両手を挙げるジェスチャーを検出しましたが、どちらかの手を挙げるジェスチャーを検出したい場合は<code>&&</code>を<code>||</code>に変更してみましょう。）

<div align="center">
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/34834/a127c222-e6ec-6885-1200-f74927c15313.jpeg" width="30%">
</div>

```cpp
// Get Joints Position
k4a_float3_t left_hand  = body.skeleton.joints[K4ABT_JOINT_WRIST_LEFT].position;
k4a_float3_t right_hand = body.skeleton.joints[K4ABT_JOINT_WRIST_RIGHT].position;
k4a_float3_t head       = body.skeleton.joints[K4ABT_JOINT_HEAD].position;

// Check One Hand Raised
bool left_hand_raised  = left_hand.xyz.y  < head.xyz.y;
bool right_hand_raised = right_hand.xyz.y < head.xyz.y;

// Check Both Hands Raised
bool is_raised = left_hand_raised && right_hand_raised;
```

これで両手が挙がっているかどうかを検出できました。あとは両手が2秒以上挙がっていることを検出しましょう。これは、両手が挙がっている時間を毎フレーム蓄積して閾値の時間（2秒）以上経過したら検出したとすればいいでしょう。

```cpp
// Initialize Valiables
bool is_timespan_raised = false;
const std::chrono::microseconds threshold( std::chrono::seconds( 2 ) );
std::chrono::microseconds raised_timespan( std::chrono::microseconds::zero() );
std::chrono::microseconds previous_timestamp( std::chrono::microseconds::zero() );
```
```cpp
// Set Current Frame TimeStamp
std::chrono::microseconds current_timestamp( body_frame.get_device_timestamp() );

// Initialize Previous TimeStamp and Raised TimeSpan
if( previous_timestamp == std::chrono::microseconds::zero() ){
    previous_timestamp = current_timestamp;
    raised_timespan    = std::chrono::microseconds::zero();
}

// Check Keep Raising Hand for Certain Period of Time
if( is_raised ){
    // Add TimeSpan
    raised_timespan += current_timestamp - previous_timestamp;

    // Check Threshold
    if( raised_timespan > threshold ){
        is_timespan_raised = true;
    }
}
else{
    // Reset
    is_timespan_raised = false;
    previous_timestamp = std::chrono::microseconds::zero();
    raised_timespan    = std::chrono::microseconds::zero();
}
```

ジャンプの高さの検出
------------------
ジャンプの高さはキャプチャしているあいだに蓄積したジョイントの中から解析します。
まず判定に使用する基準ジョイントを決めましょう。サンプルではK4ABT_JOINT_PELVISを利用しています。

Azure Kinect Body Tracking SDKではY軸の値は地面方向に向かって増加します。
基準ジョイントのY軸の値を時系列にみたとき一番小さくなった位置、そこをジャンプの頂点としてもよさそうです。<sup>*1</sup>

<sup>*1 大きなノイズがあると誤検出しやすいため、蓄積したデータは平滑化しておくとよいでしょう。 サンプルでは数フレームの移動平均で平滑化しています。</sup>

<div align="center">
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/34834/2e91e320-cb3e-28ba-3d04-4ab5adda6e3c.jpeg" width="30%">
</div>


膝の角度の検出
-------------
膝の角度もキャプチャしているあいだに蓄積したジョイントの中から解析します。
ジャンプするとき膝が曲がるタイミングは、飛ぼうとしたときと着地したときの2つのタイミングがあります。
ここではジャンプのために飛ぼうとしたときの膝の角度を検出してみましょう。

まず、ジャンプをするときに膝を曲げたと思われるタイミングを推定します。
飛ぼうとしたときの膝が曲がるタイミングは、勢いをつけるために屈んだときですね。
それはジャンプして頂点に達するまでのあいだに最も体が低い位置にいたときとみなせそうです。

<div align="center">
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/34834/a8727a06-e85c-cdc1-b40c-b80b1c2ec10b.jpeg" width="30%">
</div>

Azure Kinect Body Tracking SDKではY軸の値は地面方向に向かって増加します。
基準ジョイントのY軸の値を時系列にみたとき（頂点に達するまでのあいだで）一番大きくなったとき、そこを最も体が低い位置つまり膝が最も曲がっているタイミングとしてよさそうです。<sup>*2</sup>
そのタイミング（=フレーム）の胴体、膝、足首の位置から角度を出せば、最も曲がった膝の角度が検出できそうです。

<sup>*2 着地したときと区別するため、頂点に達するまでのあいだで探します。頂点に達したタイミングはジャンプの高さを検出したときに把握できていますね。</sup>

さて、3点のジョイントの位置から角度を算出しましょう。
点A、点B、点Cのなす角度ABCはベクトルBAとベクトルBCのなす角度です。

<div align="center">
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/34834/67ca3a7e-40ef-5659-9495-16056017afc6.jpeg" width="50%">
</div>

ベクトルBAとベクトルBCのなす角度は、次のように算出することができます。

```math
\begin{align}
\cos\theta &= \frac{\vec{BA}\cdot\vec{BC}}{|\vec{BA}||\vec{BC}|} \\[10pt]
\theta &= \cos^{-1}\theta
\end{align}
```

ソースコードにすると以下のようになります。
この引数A、B、Cに胴体、膝、足首のジョイントの位置を与えると膝の角度が得られます。
他の部位でも応用できそうですね。

```cpp
float get_angle( k4a_float3_t A, k4a_float3_t B, k4a_float3_t C )
{
    // Vector BA
    k4a_float3_t BA;
    BA.xyz.x = B.xyz.x - A.xyz.x;
    BA.xyz.y = B.xyz.y - A.xyz.y;
    BA.xyz.z = B.xyz.z - A.xyz.z;

    // Vector BC
    k4a_float3_t BC;
    BC.xyz.x = C.xyz.x - B.xyz.x;
    BC.xyz.y = C.xyz.y - B.xyz.y;
    BC.xyz.z = C.xyz.z - B.xyz.z;

    // Vector Length
    float norm_BA = std::sqrt( BA.xyz.x * BA.xyz.x + BA.xyz.y * BA.xyz.y + BA.xyz.z * BA.xyz.z );
    float norm_BC = std::sqrt( BC.xyz.x * BC.xyz.x + BC.xyz.y * BC.xyz.y + BC.xyz.z * BC.xyz.z );

    // Inner Product
    float inner_product = BA.xyz.x * BC.xyz.x + BA.xyz.y * BC.xyz.y + BA.xyz.z * BC.xyz.z;

    // Calculate Angle
    float radian = std::acos( inner_product / ( norm_BA * norm_BC ) );
    float degree = radian * 180.0f / M_PI;

    return degree;
}
```

まとめ
------
このサンプルプログラムでは、ジェスチャーの検出や解析のための基本的な考え方が学べる要素が詰め込まれています。ここでは両手を挙げる、ジャンプの高さ、膝の角度の検出を紹介しましたが、この他にも最大速度、着地点のズレなども検出しています。
大変参考になるので、一度読んでみることをお勧めします。（忙しくて全部は読めてない

明日12/2(月)はxbarusuiさんで「初心者がVisual Studioで始めるAzure Kinect」です。お楽しみに！  

---
<sup>* 掲載されているソースコードは説明のために要点を抜き出して書き換えています。サンプルプログラムそのままではありません。</sup>