Mugichoko's blog

Mugichoko’s blog

しがない研究者のプログラミングを中心としたメモ書き.

RGB-D SLAMを実装する #2

大域的目標

KellerらのRGB-D SLAM[1]が実装したい!と思い立ったので実装していく,というモチベーションの日誌.ちょっとずつ実装していく.今回が2回目.

前回(以下,参照)は論文読解とアルゴリズムの整理を行った.今回は,いよいよ実装に入る. mugichoko.hatenablog.com

方針転換

GLSLによる制約?

早速だが,実装の都合上,方針を変えようと思う.今回,比較的慣れているGLSLを使って実装しようとしているが,以下の通り,並列処理&&動的な配列を作れないことによる制約が大きいように思える.CUDAを使えばその辺り,扱いやすいのか??

  • グローバルなマップ(配列Gと呼ぶ)に新たな点Pを追加することが難しい(追加点Pの数(例えばkコ)が分かっていれば,大きな配列Gの最後の要素の番号lからl+k番目まで,同時/順に書き込めばよいことになるが...)
    1. まず,Pの数を数えるのが面倒.例えば,GPUのメモリ上にあるマスク画像(ステンシルバッファ)をメインメモリに転送して,有効画素数を数える?
    2. また,数が分かったとして,新たな点の内,どの点が何番目かが分からないので,配列Gのどの要素に書き込めばいいかIDを振って,そのIDのルックアップテーブルをGPUに転送し,一気に書き込む?
  • グローバルなマップ(配列G)にある点を削除するのが難しい
    1. 削除する点の入った要素以降の要素をシフトさせる?それってGLSLを使って処理できる??
    2. 信頼度を0にして,その点を一生使わないようにする?

解決案

そこで,キーフレームベースの手法を実装することにした.つまり,グローバルなマップの代わりに,キーフレームからの相対位置姿勢を求めるようにする.言い換えれば,キーフレームをグローバルなマップとしてトラッキングし,キーフレームから一定距離離れたら,そこに新たなキーフレームを生成し,同じことを繰り返す.これによって...

  1. メモリの管理はキーフレーム単位になり,管理しやすい
  2. 後に,ループクロージャを実装しやすくなる

と考えた.欠点として,複数のキーフレームに同一点が写っている場合,1つに統合されないことが考えられるが,利点の方が大きいので,とりあえずは良しとする.

今回の目標

  1. キーフレームクラスを作る(1画素に相当する要素は,グローバルなマップの1点と同じ)
    • カラー (uchar×3)
      • 使い道がないかもしれない
    • 奥行 (float)
      • 頂点で代替できるだろうけれど
    • 頂点 (float×3)
    • 法線 (float×3)
    • 信頼度 (float)
    • 半径 (float)
    • タイムスタンプ (int)
  2. キーフレームの頂点を別フレーム(別のカメラ位置姿勢)に投影して,処理が合っているか定性的に確認
    • 定性的に確認:投影した点が,それらしく別フレームに重なって見えるかどうか
    • これにはRGB-Dフレームの位置姿勢のグランドゥールスが提供されているTUMのRGB-D SLAM Dataset and Benchmarkを用いる

つまり,前回にまとめたアルゴリズムの内の1~3に相当する処理を実装する.尚,頂点マップ及び法線マップはこちらの記事を参照のこと.

実装

処理フローとメモリ管理

  1. 初期化
    • GLSLの初期化
    • キーフレーム作成(GPUメモリに必要な容量を確保)
  2. フレームkとk+nのRGB-D画像の読み込み
  3. フレームkの頂点マップの作成
    • 奥行画像,内部パラメータを入力として頂点マップを作成
  4. フレームkからフレームk+nへの変換行列を計算
    • TUM RGB-D SLAM Datasetのデータを利用
    • 適当に選んだkとnを基に,タイムスタンプが最も近い位置姿勢データを利用
  5. GLSLを使ってフレームkからフレームk+nに頂点を投影
    • 上手くいっていれば,RGB画像に重なるように頂点画像が描画されるはず

実装結果

以下のGIF動画にあるように,初期フレームの奥行マップを別フレームの奥行マップに投影できた.「①初期フレームのカラー画像,②初期フレームの奥行マップ,③別フレームのカラー画像,④別フレームの奥行マップ(投影結果)」のループ再生を行っている.目標達成!

尚,上記のGIF動画は,紹介しているプログラムの直接的な出力結果ではありません.紹介しているプログラムでは,単に,投影された頂点マップが画面に映し出されます.この内の1枚を取り出して,対応するカラー画像と合わせて見せているのが,上記のGif動画です.

どこかのタイミングでGitHubとかにプログラムを挙げたいと思っています.

参考文献

  1. M. Keller, D. Lefloch, M. Lambers, S. Izadi, T. Weyrich, and A. Kolb, ``Real-Time 3D Reconstruction in Dynamic Scenes Using Point-Based Fusion,'' Proc. Int. Conf. on 3D Vision, pp. 1 - 8, 2013.