Simplestar Game

:::::::::::093話　手が画像にフィット:::::::::::

距離画像と手のモデルをレジストレーションすることができれば、手が距離画像にフィットするはずです。
レジストレーションには KinectFusion の機構を使います。
重要なのは手のモデルには関節があるということです。

ずっと開いた状態で手首だけ動かすならば、簡単なモデルフィッティングで済むのですが（それでも難しい問題で研究論文書けますが）
手の指の関節が自由に曲がるので、実際どういう姿勢なのか求めることが出来ません。

そうした難しい問題に現在多くの研究機関が取り組んでいます。
ここまで来たのかーと感心する動画が上がっていたのでリンクをはっておきます。
Kinect
LeapMotion

私も個人の力でこの問題に取り組んでいます。（これと同等か、それ以上の成果を求めています。）
自分で言うのも何ですが、できたらホント世界が変わっちゃいますよね…

発想はとても素人なのですが、そのモデルというものが姿勢不定である状態のまま距離画像にフィットしたら
良いのではないだろうか？と考えたわけです。
現在は姿勢不定である状態のモデルが求められています。
ここで今から1年10ヶ月前に 071話 で作成した、コンピュートシェーダで高速化した物体トラッキングライブラリ
というものを導入して、物体トラッキングを行ってみて、結果どうなるのか見てみたいと思うようになりました。
ダメなのか？いけるのか？よくわからないまま諦めることができずにここまで来ました。
Intel が手のモデルを初期姿勢まで配置してくれましたので、その後のトラッキングを書いてみます。

ということで1年以上前の自分が残してくれた 使い方 を頼りにライブラリを導入してみようと思います。
まずはトラッキングライブラリをソース管理に追加するところから…ですね。

もう、なかなかビルドに成功しないものだから冷や汗かきましたけど、なんとか msvc2013 へ移行でき
テストプログラムも成功を返すことを確認しました。

さて、このライブラリを使って正しい姿勢行列を手に入れることは出来るのでしょうか？
（出来ないと困るのですけどね…1年10ヶ月前の27歳の自分を信じたい！）

まぁ人間間違いはよくするものですから、まずは簡単な直方体を使って箱の追跡というものをテストしてみたいと思います。
テスト用のモデルを作ります。

その前にパッケージ化してプラグインから参照可能にしておきますね。

はい、パッケージ化しました。
これまでの HandPoseEstimator で参照してみます。

参照しました。
テストコードも書きました。
必要な DLL をプラグイン直下に配置することでクラッシュすることなく既存の動作を確認できました。

さて、一つ一つ思い出しながら書いていきます。

センサは複数可、だけど今回は一つ
オクルージョン問題を解決できるように072話にてキャリブレーションを試みましたが
今回は手元にセンサが一つしか無いのでセンサは一つです。
設定の仕方は次のとおり

ここで他のパラメータの説明を見ます。（あれ、コメントがないなどうなっているんだ？）

作った本人もよくわかっていないのだが…頂点インデックス数とは？
ソースコードを追ってやっと見えてきました。
レジストレーションに必要な情報は頂点情報のみになっていて、その頂点には
位置と法線の情報が埋まっている、でその頂点情報のオブジェクト単位での最大数を教えてくれと言っている。
その数字によってComputeShaderのShaderコードを最適化する様子（そんなことしてくれるのか？）
わかりづらかったが、maxIndexCount とはインデックスバッファの要素数のことだった。
インデックスバッファアクセス用のインデックス（まぎらわしいが）のオフセット計算に maxIndexCount を利用している。
そもそも頂点数を指定してほしいなら maxVertexCount にするはずなので
maxIndexCount はインデックスバッファの要素数で間違いない。

ほかのパラメータは説明のとおり。
大雑把に把握すると、入力情報はセンサ数、オブジェクト数（オブジェクトの形はオブジェクトごとに異なって良い）
オブジェクトごとに考えた時、そのオブジェクトが参照する頂点の数、これの最も多いオブジェクトに合わせた頂点数
これを渡して処理させるというもの

センサ数などの簡易な値の設定はこれでOK。
続いて、センサについての情報を渡します。

こちらがセンサの情報です。
解像度は少々粗めの 320x240 です。
これを解像度指定の設定に渡します。

特に疑問はないので次、センサの内部パラメータ…え？
センサの箱にはそんな情報書かれていません、キャリブレーションして求めないといけない？
そもそも内部パラメータはセンサが距離画像しか求められない場合に使うもので…
実際どのようにすればよいのか実装を見てみます。

ちょっと…シェーダリソースにパラメータが渡っているだけで何しているかわからない。
シェーダリソースを見なおしてみます。

あーなるほど、頂点バッファの頂点位置と現在のオブジェクトの姿勢からワールドでの頂点位置を出し
その頂点に隣接する最も近傍にあるセンサの頂点を求めたいがために走査するわけだが
センサの深度画像バッファのどのインデックスから調べてよいかわからないので
位置からセンサの内部パラメータを用いて、おおよそのセンサの深度画像バッファのインデックスを予想
その周辺を調べるという処理になっている。

ということはセンサの内部パラメータ必須じゃないですか…もー
研究ノートの該当箇所を読んだら、センサの情報は外部から渡す、
キャリブレーションも外部で行なうのがポリシーと書いてありました。
ポリシーに従ってなんとかします。

なーんだ忘れていましたがキャリブレータも作っていたのでした。
ということで、キャリブレートして内部パラメータを求めてみます。

まずはプラグインにキャリブレーションプログラムを書き込みます。
変数4つを求めるだけですし、既存のテストプラグインに成り下がった VerticesViewer に組み込みますか…

組み込みました。
いやー、いつの間にか私も成長したものです。（もうすぐ30歳ですが…）
大学の頃の私はこうもちゃかちゃかと思ったことを形にできませんでした。

さて、求められた値を設定します。

続いて設定するのが…外部パラメータですね。
複数のカメラの情報を使う場合は必要ですが、今回は一つのカメラしか使いませんので
外部パラメータは単位行列とします。

あとは…まだあります。
レジストレーションするオブジェクトの設定です。
テストコードなので、1つのインデックスしかない点オブジェクトを設定しているコードになっています。
さて、ここでテスト用のモデルを作成したいと思います。

Blender の出番です。
次のような頂点数 4x6 の直方体を定義しました。
こちらはテスト用のオブジェクトです。

Kimberlite でバイナリファイルに変換します。

これをリソースに埋め込み、オブジェクト情報を引き出します。

とりあえず埋め込んで描画するところまで確認しました。

オブジェクトの向きがわかりやすいようにテクスチャを用意します。

テクスチャを貼ったリソースを読み直すとこんな状態

さて、まだ初期化も終わらせていませんので、追従しませんが
うまくいけばこの距離画像にフィットするように Box が付いてくるはずです。
まだまだ設定することは多いですが、頑張りましょう。

やっとオブジェクトの頂点バッファとインデックスバッファが取れるようになりましたので
ここでオブジェクト情報の設定を行います。
つまりはこのような設定で良いのかな？

やっと初期化用の情報が揃ったので、初期化を実行します。

さて、一体何を返すのでしょうか？
CS_ERROR_CODE_SUCCESS よし、初期化データには問題ないようですね。

続いて、センサデータの設定を毎フレーム行います。
意味合いとしては GPU の確保済みのバッファに距離画像データの配列をコピーする処理です。
今度は初期化処理から外れて、UpdateVerticesBuffer 内部で行います。

入力をそのまま渡せば良いのでこんな感じでしょうか？

そうですね、そして現在のオブジェクトの姿勢をレジストレーションのライブラリに渡します。
レジストレーションをして、レジストレーション結果を受け取るようにもしてみます。 それが次のコード

おそらく正しく動かないのですが、クラッシュはせずに成功コードを返すはずです。
どうなるか？成功コードを返した時にヒットカウントするようにして、100回成功するか確かめてみます。

はい、常に成功を返し続けることを確認しました。
さて、オブジェクトが動かないのですが、それはなぜか？
MotionState でないとダメなんだっけ？

MotionState でもだめでした…そうか、もしかしたら近傍点が見つからないから
渡した姿勢と同じものを返しているのかもしれません。
とりあえず返ってきた情報に細工をして単位行列となるようにして、原点無回転の位置にBoxが移動するか見てみます。

あー原点に来ますね。ということはレジストレーションライブラリに渡した姿勢をそっくり返していると言えます。
KinematicObject にして、落下しないようにして MotionState を弄ってみるとどうなるでしょうか？

あれ？ActivationState を DisableDeactivation にしても追従しないな…
ライブラリは直前に渡した姿勢行列をそのまま返している模様…
探索条件が良くないのかな？
メチャメチャゆるい条件（低速になる）で試してみます。

まったく追従する気配がないので困りましたが…
センサに十分オブジェクトを近づけた時だけ反応がありました。
何かしらの結果が返ったので、原因を考察しましょう。

searchThreshold を十分大きくするとずっと遠い位置に配置されました。
与えている距離画像の単位でしょうか？
確認してみると…確かに渡しているデータの単位が mm でした。
これはまずかったかな…シェーダの方で mm を m に直すようにしてみます。

結構複雑な話ですが、センサからは mm 単位で入力が来ており、これを 10 倍した後
シェーダ側で 1000 分の 1 の値に直して描画しています。
レジストレーションライブラリ側は m 単位での入力を待っているので、渡す時は mm の入力を 100 分の 1 の値にして
センサに渡す必要があります。

まさにその通り！
追従…まではしれくてませんでしたが、反応はしました。
シェーダコードに問題がある様子だったので
シェーダコードを書き直します。（レジストレーションライブラリを更新します。）

うわー、今度はシェーダコードがビルド通らなくなりました。
詳細を確認します。

なるほど、switch にローカル変数を使ったのがまずかったみたいです。
追従するか確認した所、おかしな回転が加わりながら離れていくことを確認しました。
これ、つまり 1年10ヶ月前の私の仕事に不備があったことになります。
それはそれとして何が原因なのでしょうか？
テストプロジェクトをもう少し編集して詳細を追ってみたいと思います。
よくわからないですが、渡すオブジェクトの姿勢行列の転置とかかな？

当てずっぽうですが、転置して渡して、転置して受け取るとどうなるでしょうか？
そもそも追従しません。（入力データが不正扱いなので当たり前か…）

あー最悪です。面倒ですが…
デバッグ機能を入れて、対応する頂点がどの位置とペアを組んでいるのかわかるようにしてみます。

レジストレーションライブラリの内容を確認している。
センサの姿勢行列は初期化時に外部パラメータとして渡していて
センサの深度画像 floatx3 配列を毎回渡す際、その位置をセンサの姿勢行列で掛けて（シェーダで）
この結果をシェーダ側から受け取り、そのままコピーしている。
ここに問題はないと思った。

オブジェクトごとにスレッドを立てて、そのスレッド単位でレジストレーションを行っている。
スレッド周りも問題なさそう。
ではスレッド内処理はどうか？

うーん、アルゴリズムの内容を見ても正しいかどうかの判別が難しいです。
もともと正しいというコードをそのまま貼り付けているので、ここに手を入れるにはちょっと違う気がする。
気になる箇所を見つけたと思ったのは、MathLibrary から受け取った姿勢行列をわざわざ転置しているところ
シェーダに渡すならわかるが、それをしないのであれば転置は不要なのでは？

そんなことありませんでした。
転置を逆にしたら原点へ向かったので、やはり Math 側からの結果を転置するのが正しいようです。
もしかしたら初期条件がまずいのかもしれません。
トラッキングを開始して、センサのデータを近づけるところから始めたので
正しい位置に配置してからトラッキングを始める仕組みを入れてみます。

確認したところ、回転が逆になるという問題に直面しました。
位置合わせをしようとして逆の回転姿勢となるのです。
ということは…受け取った情報の回転だけを転置して結果どうなるか見てみます。

まぁ、訳がわからない。
ここまで来ると、頂点ペアの求め方に問題があると思われる。
しかし、頂点ペアを求めるシェーダコードにこれといって間違いが見当たらない。

視覚的にペアが正しいかどうか調べるのでレジストレーションライブラリには
ペアとなる頂点位置リストを要求時に返せるようにしようと思う。

それではライブラリ側にこの機能を追加します。
追加しました。
ここにもバグはありそうですが、結果を可視化してみます。
そのためには2つの頂点位置からラインを引く機能が必要です。

ラインを引く、これが結構面倒なんですよね。
ぱっと思いつくのはラインだけのオブジェクトをリソースに加えて
そのオブジェクトの頂点位置を移動させて描画するというもの。
DrawLine なる関数を用意する所から開始します。

まずはこの原点からZ軸方向へ伸びた単位長さのラインオブジェクトをリソースに埋め込みます。
リソースの読み込みシステムは既にあるので次のように1行追加するだけです。

あとは頂点位置情報を GPU に転送して、その位置情報を使ってラインを引いてもらうようにコードを書きます。
用意しました。
用意した関数を呼び出すコードが次のとおり。

結果はどうなるでしょうか？
これが正しいといよいよアルゴリズムの方を見ないといけなくなってきてしまうのですが…

あ、ラインを表示し続けるには Render で呼ばないといけなかった。
情報を一時領域に溜めて、RenderScene の内部でラインを引くように修正します。

あ？なんのギャグ？
これは一体どんなバグ？
値の設定を間違えたかな？
そもそもスケールが違いすぎる。

まずバッファ更新と描画が重ならないようにいつかロックする機構を入れましたが、これが
情報の更新を妨げていました。
これが全てです。そこだけ修正したら下図の通り、対応を取る処理は完璧といって良いペアを取っていました。
やはりシェーダコードに誤りが見つからなかった通り、ペアも完璧です。

ではなぜこのペアの偏差を最小とする姿勢にならないのでしょうか？
今度はペアのラインを引いて、そのペアで計算した姿勢となるように Box を動かし
その動かした後にオブジェクトには止まってもらいます。
どうなるでしょうか？何がつかめるでしょうか？

なんともよくわかりません。
ただ、これだけ正確なペアがとれているわけですから
ひとまず暫定で書いたペア情報取得コードを正式にデバッグ用コードとして採用しました。

一旦提出します。

さて、数学ライブラリを交えて転置不要となるように修正してみたいと思います。

D3DXMatrix と同じメンバ構成にして、キャストして渡せるようにしている点で
これが望ましいと思うのだけど、どうも ComuteShader に渡す際は転置しないとダメな様子。

なんでダメか、そりゃ memcpy で GPU 側に情報を渡したいからですね。
複数のオブジェクトがある場合でも一括で memcpy できるという嬉しい点を使うので
CPU 側も GPU 側のメモリ配置に合わせて転置した状態の行列を保持している。
気をつけるべきは、書き込む時に受け取った値を転置して書き込むこと
取り出す時は転置してから渡すことにしておけば問題は起きない。

問題がうまく切り分けられてきたが、MathLibrary が返す回転行列の部分が怪しい。
単に転置するだけでもダメなので、根本的に間違った結果を返している可能性がある。

さて、信頼できる MathLibrary にしたいので、これまで作ってきた Math の元となったC#ツールに
MathLibrary を参照させ、既存の結果と同じものを返すことを確認していこうと思う。

まずは4次方程式の解からやってみよう。

あれです、デバッグはエンターテイメントだ。（泣）

まずは MathLibrary を C# で呼べるようにします。
CLRプロジェクトを追加しました。
インタフェースを続々とラップしていきます。

結構大事な作業なので平日の空き時間を殆ど使ってラップしました。
テスト中にやはり不具合を数箇所見つけました。
これらを修正して再度トラッキングを行わせてみました。

やりました！
Creative カメラのケースが距離画像にフィットしながらかなりの精度でトラッキングできています。
この処理がリアルタイムですよ。
感動です。
すばらしいよ ComputeShader ありがとう。
さて、一旦この状態のライブラリセットを提出してラベルを貼っておきます。

実はモデルを既に直方体からベベルのモディファイアを通したポリゴンにしてテストしています。
これを手の形に修正して、エージェントを使ってうまい具合に考えて追従するシステムを作ってみます。

まずは固定した状態の手の形で、難易度を低めに設定して追従テストを行ってみたいと思います。

では、どうやって手を固定するのか？
Registration開始ボタンを押して、開始することをシステムに伝え
その後両点のトラッキングに成功してから、姿勢が安定したのを見計らって
手の姿勢モデルの頂点バッファ、インデックスバッファを構築します。

これを使って Registration を行い、結果の姿勢行列から各オブジェクトの姿勢行列に直し
トラッキングさせます。

もし大きく位置が異なるようであればそこまで移動せず。

ではここまで作ってみますか、まずはトラッキング後に一斉に姿勢行列を回収し
中指の中手骨を基準として相対姿勢行列を構築します。
それぞれの相対姿勢をそれぞれの頂点バッファに掛けて、手一つの頂点バッファを構築します。
ここまでやってみますね。

親指以外は引っ張りすぎ、親指はたるみすぎ。
引っ張りすぎを元に戻すにはどうすればよいのか？
手の中心に向かって線を引き、その延長線上で引き算すれば良い。

対応しました。
トラッキングポイント7x2点の安定度というものを定義します。
前回の位置からの差分が十分小さく、トラッキングしているというフラグです。 定義しました。

さて、まずはダメな例を確認します。
全ての指先位置の安定度が閾値以下である状態が1秒続いた時に、すべてのオブジェクトをRegistration機構に登録して
一斉に追従させます。
絶対にうまくいかないことがわかっていますが、レジストレーション開始機構の動作確認にもってこいです。
作ってみます。

一応全ての指先位置の安定度が閾値以下の状態に入り、これが1秒続いた状態というものを認識できるようになりましたが…
タイミングとしては、毎回フレームを更新する場面にて、良好状態を確認してからオブジェクト情報を使ってRegistrationを初期化
オブジェクト情報を使うっていっても…まぁ最初は失敗しても良いということなのですべてのトラッキングオブジェクトを渡します。

渡しました。
すべての指先位置がトラックを始めて、安定して1秒経ったら複数オブジェクトがResistrationを始めます。
うまくいかないどころかどこかにバグがあって、距離画像を無視して猛烈な振動を開始しました。
まぁ少しずつ見て行きましょう。

まず、全てのオブジェクトの中心位置を求めます。
すべて原点付近に来なければなりません。
結果は

いやいや、もっと簡単な確認方法がありました。
オブジェクトの姿勢行列を単位行列とした時、原点に来れば問題ありません。
オフセット行列も単位行列にして

いやいや、もっと確実な方法がありました。
オブジェクト一つ一つでレジストレーションを行いラインを引いてみれば分かります。

あー、なんてミスを…オブジェクトごとにバイナリデータを保存したつもりが…
シーン単位で保存していたため、常に同じ頂点バッファとインデックスバッファを参照していました。
オブジェクト単位で情報を持たせます。

解決したー

さて、一斉追従、させてみましょうか？
まずは Intel PerC SDK の結果追従と重ならないように、トラッキングを開始したら
優先度決定をするようにします。

具体的には、レジストレーションを開始してフラグを立て、フラグが立っている間は、Intel PerC SDK の言いなりにならない。
これがかなりのコンテキストを要する。
指先位置のconfidenceが90以上ある場合はそれに付いていこうとするわけだが
このときレジストレーション中であれば…位置変更しない…というのが正解なのか？
そもそもエージェントが行動するべき所なのに…

ということでエージェントに位置合わせ後の姿勢を渡して、エージェントに考えて行動してもらうことにしました。
まずは指先の追従を切った所…その場で緩やかな回転をはじめました。
まあ、仕方ないと言えば仕方ない。
たとえば手を下に移動させた時は指先位置を追従します。
あ、左の指の位置に移動するものも現れました。
上に動かすと指先はその場の位置にとどまろうとして…
これも考えてみれば自然な動きです。

ただ、個として考えたらという意味ですので、当初考えていたものはもう少し違うものでした。
今は指先位置だけのレジストレーションなので、指先以外のオブジェクトでも
同様のことをやってみたいと思います。

うん、まぁうまくはいかない。
手の原型を留めない。
せっかく手のモデルを作ったのだから、手じゃない形はやめてと、補正できないだろうか？
なるほど、解決策は作れるが、作るにはそれなりに時間を要するのでもう少し先にしようと思う。

先に手の形をつくるところから片付ける。
思考実験が最近明確になってきて、たとえ手の形をうまくつくっても
オクルージョン問題で手を裏返す途中の指が全て隠れる瞬間でうまくいかないことがあり得る。
頂点位置補正についてはレジストレーションの時にフラグを追加して、このフラグが立っている時に
オブジェクト同士を統合して一つのオブジェクトとして頂点ペアを組み合わせることとする。
親オブジェクトの姿勢行列を求めて、それに付随する子オブジェクトの姿勢行列も正しく求めて返そう。

さて、オクルージョン問題について解決できないか考えてみると
まず面をカメラに向けているのに、頂点のペアが見つからず、この頂点が手前のオブジェクトの影に隠れたと考えて良くなる。
その時の手前のオブジェクトとは、頂点ペアが十分見つかっているオブジェクトのことである。

人間が考えることは、距離画像に写らないオブジェクトは手前のオブジェクトに隠れているという想像であるが
この想像は計算に直すと、手前のオブジェクトを回転の中心とし、自身のオブジェクトまでの距離を半径
この球面とカメラと手前のオブジェクトを通る直線との後部交点を想像する位置とする。
さて、線を結ぶにはいささか不明な点が多く、手前のオブジェクトを中心に回転軸を決めることが出来ない。

なんか、ごちゃごちゃと文句ばっかり言っているようですが
当初の目標の通り手の形を固定して、手が距離画像にフィットするか確認してみました。

できました！
手を裏返したり、横に振ったり、縦に倒したり、後ろに倒したり
手首をひねりながら様々な姿勢をとっても、これに手のモデルがフィットしてついてきます。

やりました！
やりました！
8年前から構想していたことは、正しかった！
考えたとおりに動いた！

もし失敗したらどうしようとかなり精神的に不安定な状態がここ2ヶ月くらい続いていましたが
この成功映像を確認してからは感動のあまり、更に精神的におかしくなるという日々を送っています。

さて、まだまだ手は固定した状態、初期姿勢推定は Intel PerC に頼りきり
激しく手を振ると追従が切れる
前後、手のひらに垂直な面で回転すると追従が弱い
何より競合となる LeapMotion と比べて、その精度、即応性の足元にも及ばないという

こちらが LeapMotio Visualizer の結果
LeapMotion もどんどん精度上がっていますね！
（このまま私の思った通りのレベルまで行ってくれるとそれはそれで嬉しい！手の認識を任せて、自分はその先に進めるので）

ひとまず掲げた題の通り、手が画像にフィットするところまで作りましたので
ここで話を切ろうと思います。
次回をお楽しみに！

2014/10/18 初記。
2014/12/09 更新。