大域的目標
KellerらのRGB-D SLAM[1]が実装したい!と思い立ったので実装していく,というモチベーションの日誌.ちょっとずつ実装していく.今回が2回目.
前回(以下,参照)は論文読解とアルゴリズムの整理を行った.今回は,いよいよ実装に入る. mugichoko.hatenablog.com
方針転換
GLSLによる制約?
早速だが,実装の都合上,方針を変えようと思う.今回,比較的慣れているGLSLを使って実装しようとしているが,以下の通り,並列処理&&動的な配列を作れないことによる制約が大きいように思える.CUDAを使えばその辺り,扱いやすいのか??
- グローバルなマップ(配列Gと呼ぶ)に新たな点Pを追加することが難しい(追加点Pの数(例えばkコ)が分かっていれば,大きな配列Gの最後の要素の番号lからl+k番目まで,同時/順に書き込めばよいことになるが...)
- グローバルなマップ(配列G)にある点を削除するのが難しい
- 削除する点の入った要素以降の要素をシフトさせる?それってGLSLを使って処理できる??
- 信頼度を0にして,その点を一生使わないようにする?
解決案
そこで,キーフレームベースの手法を実装することにした.つまり,グローバルなマップの代わりに,キーフレームからの相対位置姿勢を求めるようにする.言い換えれば,キーフレームをグローバルなマップとしてトラッキングし,キーフレームから一定距離離れたら,そこに新たなキーフレームを生成し,同じことを繰り返す.これによって...
- メモリの管理はキーフレーム単位になり,管理しやすい
- 後に,ループクロージャを実装しやすくなる
と考えた.欠点として,複数のキーフレームに同一点が写っている場合,1つに統合されないことが考えられるが,利点の方が大きいので,とりあえずは良しとする.
今回の目標
- キーフレームクラスを作る(1画素に相当する要素は,グローバルなマップの1点と同じ)
- カラー (uchar×3)
- 使い道がないかもしれない
- 奥行 (float)
- 頂点で代替できるだろうけれど
- 頂点 (float×3)
- 法線 (float×3)
- 信頼度 (float)
- 半径 (float)
- タイムスタンプ (int)
- カラー (uchar×3)
- キーフレームの頂点を別フレーム(別のカメラ位置姿勢)に投影して,処理が合っているか定性的に確認
- 定性的に確認:投影した点が,それらしく別フレームに重なって見えるかどうか
- これにはRGB-Dフレームの位置姿勢のグランドゥールスが提供されているTUMのRGB-D SLAM Dataset and Benchmarkを用いる
つまり,前回にまとめたアルゴリズムの内の1~3に相当する処理を実装する.尚,頂点マップ及び法線マップはこちらの記事を参照のこと.
実装
処理フローとメモリ管理
- 初期化
- GLSLの初期化
- キーフレーム作成(GPUメモリに必要な容量を確保)
- フレームkとk+nのRGB-D画像の読み込み
- メインメモリにまず読み込む
- TUM RGB-D SLAM Datasetのあるデータを読み込む
- フレームkの頂点マップの作成
- 奥行画像,内部パラメータを入力として頂点マップを作成
- フレームkからフレームk+nへの変換行列を計算
- TUM RGB-D SLAM Datasetのデータを利用
- 適当に選んだkとnを基に,タイムスタンプが最も近い位置姿勢データを利用
- GLSLを使ってフレームkからフレームk+nに頂点を投影
- 上手くいっていれば,RGB画像に重なるように頂点画像が描画されるはず
実装結果
以下のGIF動画にあるように,初期フレームの奥行マップを別フレームの奥行マップに投影できた.「①初期フレームのカラー画像,②初期フレームの奥行マップ,③別フレームのカラー画像,④別フレームの奥行マップ(投影結果)」のループ再生を行っている.目標達成!
尚,上記のGIF動画は,紹介しているプログラムの直接的な出力結果ではありません.紹介しているプログラムでは,単に,投影された頂点マップが画面に映し出されます.この内の1枚を取り出して,対応するカラー画像と合わせて見せているのが,上記のGif動画です.
どこかのタイミングでGitHubとかにプログラムを挙げたいと思っています.