大域的目標

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

mugichoko.hatenablog.com

今回の目標

ICPアルゴリズムの理解
ICPアルゴリズム（画像ピラミッドなし）のGPU実装

ICPアルゴリズムの理解

今回実装したいICPアルゴリズムは，「センサから得られた2つの点群の位置合わせを行う」というよりは，「（連続して撮影された）奥行画像2枚から得られる点群同士の位置合わせ」を行う，という説明がしっくりくる．参考文献[1][2][3]のICPに該当する部分を読んで理解した内容が以下の通り．

奥行画像2フレームから頂点マップを生成
- 以降，各フレームをフレーム ${S}$ ，フレーム ${D}$ と呼称
- 各頂点マップを ${V_S}$ ， ${V_D}$ とする
- 各頂点マップの3次元位置を ${\bf s}$ ， ${\bf d}$ とする
フレームから法線マップを生成
- 各法線を ${\bf n}$ とする
フレームの頂点マップをフレームに投影しを生成
- この時，フレーム上の2次元位置 ${\bf u}$ 上にあるフレーム ${S}$ と ${D}$ のデータが対応付いていると考える
- 投影する際の位置姿勢は初期状態では未知のため単位行列とする
- それ以降は，5で得られたパラメータから計算した行列を用いる
フレーム ${D}$ 上の頂点マップ ${V_D}$ ，法線マップ ${N}$ ，そして ${V'_S}$ より，以下の式8と10にある行列 ${\bf A}$ とベクトル ${\bf b}$ を生成参考文献[2]より
行列，ベクトル及び求めたいベクトル（式9）は以下の式13の関係にあるため，SVD等を用いてベクトルを計算
- ${\bf x}$ の各要素は左から順に，ヨー，ピッチ，ロール，3次元位置，となっている
- フレームの座標系からフレームの座標系への変換に相当する
  - EigenやOpenCVを使えばよい（所謂，solve関数）
収束条件を満たしていれば終了 or 3へ移動

ICPアルゴリズム（画像ピラミッドなし）のGPU実装

GPUが絡むので，どこまでをGPU処理するか考える．私のデザインは以下の通り．

RGB-D画像を取得
RGB-D画像をGPUに転送
「ICPアルゴリズムの理解」の2～4までをGPUで処理
「ICPアルゴリズムの理解」の5及び6をCPUで処理

実装結果

実装したICPアルゴリズムを使って，2フレーム間のトラッキングを行ってみた．以下のGifアニメーションは，「ICPアルゴリズムの理解」で説明した行列 ${\bf A}$ とベクトル ${\bf b}$ の各要素をRGBA画像に置き換えて表示させた結果です．2フレームの情報が最初は離れているため，画像的にダブったように見えますが，時間が経つにつれていちしせいすいていがしんこう位置姿勢推定が進行していくため，合わさっていき，はっきりとした画像になったように見えると思います．

1フレーム辺り平均で約80msかかっています．尚，画像サイズ640×480画素で，ICPの各ループでの収束条件は，ICPでの現在のループと1つ前のループで得られたパラメータ ${\bf x}_i$ 及び ${\bf x}_{i-1}$ の距離（L2ノルム; ${\sqrt{{\bf x}_i \cdot {\bf x}_{i-1}}}$ ）を取った際，それが ${1.0^{-3}}$ を下回っていた場合，収束したとみなしています．システム構成は以下の通り．