大域的目標
KellerらのRGB-D SLAM[1]が実装したい!と思い立ったので実装していく,というモチベーションの日誌記録.ちょっとずつ実装していく.モチベーションに関する詳細は初回の記事を参照のこと.今回が3回目.前回からだいぶ時間が経ってしまった...
今回の目標
ICPアルゴリズムの理解
今回実装したいICPアルゴリズムは,「センサから得られた2つの点群の位置合わせを行う」というよりは,「(連続して撮影された)奥行画像2枚から得られる点群同士の位置合わせ」を行う,という説明がしっくりくる.参考文献[1][2][3]のICPに該当する部分を読んで理解した内容が以下の通り.
- 奥行画像2フレームから頂点マップを生成
- 以降,各フレームをフレーム,フレームと呼称
- 各頂点マップを,とする
- 各頂点マップの3次元位置を,とする
- フレームから法線マップを生成
- 各法線をとする
- フレームの頂点マップをフレームに投影しを生成
- この時,フレーム上の2次元位置上にあるフレームとのデータが対応付いていると考える
- 投影する際の位置姿勢は初期状態では未知のため単位行列とする
- それ以降は,5で得られたパラメータから計算した行列を用いる
- フレーム上の頂点マップ,法線マップ,そしてより,以下の式8と10にある行列とベクトルを生成 参考文献[2]より
- 行列,ベクトル及び求めたいベクトル(式9)は以下の式13の関係にあるため,SVD等を用いてベクトルを計算
- の各要素は左から順に,ヨー,ピッチ,ロール,3次元位置,となっている
- フレームの座標系からフレームの座標系への変換に相当する
- EigenやOpenCVを使えばよい(所謂,solve関数)
- 収束条件を満たしていれば終了 or 3へ移動
ICPアルゴリズム(画像ピラミッドなし)のGPU実装
GPUが絡むので,どこまでをGPU処理するか考える.私のデザインは以下の通り.
実装結果
実装したICPアルゴリズムを使って,2フレーム間のトラッキングを行ってみた.以下のGifアニメーションは,「ICPアルゴリズムの理解」で説明した行列とベクトルの各要素をRGBA画像に置き換えて表示させた結果です.2フレームの情報が最初は離れているため,画像的にダブったように見えますが,時間が経つにつれていちしせいすいていがしんこう位置姿勢推定が進行していくため,合わさっていき,はっきりとした画像になったように見えると思います.
1フレーム辺り平均で約80msかかっています.尚,画像サイズ640×480画素で,ICPの各ループでの収束条件は,ICPでの現在のループと1つ前のループで得られたパラメータ及びの距離(L2ノルム; )を取った際,それがを下回っていた場合,収束したとみなしています.システム構成は以下の通り.
- OS: Windows 10 Home 64-bit
- CPU: Intel Core i7-6567U 3.30GHz
- GPU: Intel Iris Graphics 550
- RAM: 16.0GB
上手く全部実装できたらGitHubとかにプログラムを挙げたいと思っています.
参考文献
- M. Keller, et al. ``Real-Time 3D Reconstruction in Dynamic Scenes Using Point-Based Fusion,'' Proc. Int. Conf. on 3D Vision, pp. 1 - 8, 2013.
- Kok-Lim Low, ``Linear Least-squares Optimization for Point-to-plane ICP Surface Registration,'' Technical Report TR04-004, Dept. of Computer Science, University of North Carolina at Chapel Hill, 2004.
- R. A. Newcombe, et al. ``KinectFusion: Real-time Dense Surface Mapping and Tracking,'' Proc. Int. Conf. on Mixed and Augmented Reality, 2011.