大域的目標

KellerらのRGB-D SLAM[1]が実装したい！と思い立ったので実装していく，というモチベーションの記録．ちょっとずつ実装している．今回が6回目．モチベーションに関する詳細は初回の記事を参照のこと．

前回（以下，参照）は，完成したICPによる位置合わせ手法を使って，キーフレームをトラッキングしてみた．

mugichoko.hatenablog.com

できた？

何をやったかは後半に譲るとして，動いているっぽい．

※実際の実行速度ではありません．オフライン処理しています．

youtu.be

方針の再転換

過去にキーフレームベースじゃないとGPUで実装しにくいのでは？と思っていたのだけれど，結局，実装できそうだな！と元の実装方針に戻すことにした．詳細は次項に譲る． mugichoko.hatenablog.com

話ついでなので，キーフレームベースの手法とマップを1つしか持たない手法の利点と欠点を考えてみた．

キーフレームベース（ある点に関して重複した存在を許す）
- 必要なデータ容量が増えてしまう
- バンドル調整を設計しやすい？
  - キーフレーム内の点は画素単位で位置合わせしており高精度
  - キーフレーム間の位置合わせ精度は移動するにつれて落ちる
  - よって，キーフレーム間でバンドル調整
- 現視点から見える十分な点を再構成するためには，何枚の，どのキーフレームを使うかという判断が必要
  - 判断ができれば，グローバルマップの様に全点を投影する必要がない
- どのタイミングでキーフレームを新たに作成すればいいか判断する必要がある
  - トラッキングの破たんを防ぐためにも，ある程度の点が現フレームで見えていないといけないが，そのせいでかなりダブった点を大量に保持する必要が出てきそう
グローバルマップベース（世界の中のある点はその1点だけ）
- 必要なデータ容量が最小限で済む
- ある点は唯一無二の点なので...
  - 再構成した点群から物体を切り出したい，と思った場合，ダブルことなくその物体が得られる
- とりあえずマップ内の全点を現フレームに投影してみる必要がある
  - この辺りがGPU処理の恩恵を受けるところ
- 後からバンドル調整などでマップを変形することが難しい
  - 3DCGのリギングみたいにすれば，ぐにゃりとループクロージャはできそうだが...

以下，グローバルマップベースの手法に切り替えたことで苦労した細かな点を記しておこうと思う．

グローバルマップの管理

まず，Shader Storage Buffer Object (SSBO) というGPU上のバッファに，数百万コといった大きな配列を用意して，そこにグローバルマップを保存することにした．

例えば， ${M}$ コの点をSSBOとして用意したとする．その内，意味のある要素数を ${m}$ < ${M}$ として保持しておくことで，新たな要素は ${m + 1}$ 番目に保存されることとなる．この ${m}$ を実装するためにAtomic Counterなるものを使った．

Atomic CounterはGPU内のスレッド（Invocationと言うらしい）に共通のカウンタらしい．なので，これをインクリメントする際には，他のInvocationはこのAtomic Counterにアクセスできない，という仕様だ．これを ${m}$ として使うことで，SSBOとして用意したグローバルマップ内の同一要素に同時にアクセスすることがなくなる．

点のマージと削除

2つの点をマージすると一方が，点を削除するとその点がグローバルマップから削除されなくてはいけない．GPUは固定サイズの配列を一気に並列処理することが得意なので，可変の配列が用意されておらず，これを実装するのが意外と難しい．

現状の解法は，2つのグローバルマップを用意しておいて交互に使うという方法だ．グローバルマップ1と2を用意したとする．1を使っていて，要らない点がマージや削除によって出てきたとする．この時，いらなくなった点の要素をあり得ない数値，例えば，正の値しかとり得ないサーフェルの半径 (radius) を負にすることで，この要素が不要だということを記録しておく．そして，グローバルマップ1から2へコピーするのだが，この時に不要な要素はコピーしないようにする．

つまり，シーケンシャルに不要な要素以外を別のグローバルマップにコピーしていく．よって，どの要素まで書き込んだかを知るために，ここでもAtomic Counterを活用した．

残った問題

パラメータ調整

どのくらいの点を残して，どのくらいの点を消すか／統合するかはパラメータ依存で，論文には特段詳しい記述がない... ここはシーンによって微調整になってしまいそうだ...

例えば， ${c_{stable}}$ の値は，どの点を描画してICPに入力するか，他の点との統合の対象とするかといった基準となるが，どうも論文に書いてある10に設定しても上手くいかないことが多い．この値は，どのくらい観測したら信頼できる点とするか，という意味に等しい．なので，10に設定たとすると，始まりのフレームでじ～っとしていないとトラッキングがそもそも始まらない．1の書き間違いではないかと疑うくらいだ．

最初に示した結果の映像でも，点が統合されすぎていて，もう一度同じところに戻ってくるといくつかの点が消えているので，もう少し調整が必要そうだ．

実行速度

当初，Atomic Counterを使うことによって，各スレッドに待ち時間が発生して処理時間がかかってしまうのではないかと想像していたが，その心配を吹き飛ばすくらいにICPが遅い．VGA画像に関して，ICPだけで約100msかかっている（QVGAだと，20～30ms）．

ICPが遅い原因は明らかで，私の実装が悪いからだ．現状，ICPの各ループで， ${\bf A}$ 行列と ${\bf b}$ ベクトルを画面サイズ分GPUメモリからメインメモリにロードしているが，PCLのKinfuのソースコードを覗いてみると，GPUからロードされる行列の大きさは ${6\times6}$ に見える．つまり，

私の戦法
1. GPUメモリで ${N\times6}$ の ${\bf A}$ 行列と ${N\times1}$ の ${\bf b}$ ベクトルの各要素を画素数分計算
2. 上記をロードして，CPU上で有効要素のみを用いて ${6\times6}$ 行列に整形
3. ${\bf A}$ 行列と ${\bf b}$ ベクトルを入力してOpenCVのsolve関数で位置姿勢を計算
Kinfuの戦法
1. GPUメモリで ${\bf A}$ 行列と ${\bf b}$ ベクトルの各要素を画素数分計算（私と同じ）
2. GPU上で行列に ${6\times6}$ 行列と ${6\times1}$ ベクトルに整形
3. 上記をロードしてEigenのsolve関数で位置姿勢を計算