Mugichoko's blog

Mugichoko’s blog

プログラミングを中心としたメモ書き.

RGB-D SLAMを実装する #1

目標

KellerらのRGB-D SLAM[1]が実装したい!と思い立ったので実装していく,というモチベーションの日誌.今回が初稿.ちょっとずつ実装していく.今回は,論文読解とアルゴリズムの整理を行う.

アルゴリズム

論文を読んで,今考えているアルゴリズムは以下のイメージ.ただし,動物体の検出に関しては,今回,ひとまず無視する.

  1. メモリの確保
    • グローバルなIndexマップを持ったある点群(以下は,1点当たりの要素,SSBOとして確保?)
      • 点群の配列 (float×3)
        • GPUに確保するので,一気に大きな配列を用意
      • 信頼度 (float)
        • 現在の点と対応する点がマージされると+1
        • 10以上(これは経験的な値)になると不安定から安定へ
      • 半径 (float)
        • サーフェル描画用
      • タイムスタンプ (int)
    • 4W×4HのIndexマップ(int,WとHは画面サイズの幅と高さ)
      • 全点を投影して,ルックアップテーブル (LUT) として使う
    • W{\times}Hの
      • 頂点群マップ (float×3)
      • 奥行マップ (float×3)
      • 法線マップ (float×3)
  2. 奥行画像の処理
    1. 奥行マップをバイラテラルフィルタでスムージング
    2. 奥行マップを頂点マップに変換
    3. 頂点マップ画像から法線マップの作成
    4. 頂点マップと奥行マップは3段階の解像度のものを用意
      • Lv0:元の解像度,Lv1:0の半分,Lv2:1の半分
      • Mipmapで実装できる?
  3. 位置合わせ(Iterative Closest Point (ICP) アルゴリズム等)
    • 参考文献[2]
  4. Indexマップの生成
    • 点群の配列をIndexマップに単なる点群として投影して,各点のindexを書き込む f:id:Mugichoko:20170725103831j:plain
  5. Indexマップから1点抽出(以下は,そのルール)
    1. {\pm \sigma_{depth}}以上離れている点は候補から外す
    2. 保存された法線と現在の法線との角度が20度以上の点は候補から外す
    3. 残りの点の内,信頼度の最も高い点を選択
    4. もし,3まで満たす点が複数あれば,2の内最近傍の点を使う
  6. 選択した点の統合/追加
    • 現フレームの3次元点と法線を以下の式に基づいて統合し,信頼度マップとタイムスタンプマップも更新する f:id:Mugichoko:20170725103915j:plain
    • ただし,{ \alpha = e^{- \gamma^2  / 2 \sigma^2} }
    • {\gamma}は正規化画像座標系における,光軸中心からの距離,{\alpha = 0.6}
    • 式1は,半径が現在の半径と比べて {(1 + \delta_r) \bar{r}} 以下だった場合のみ行う
      • ただし,{\gamma_r = 1 / 2}
    • 仮に,どの候補も見つからなければ,新しい点として追加される
  7. 点の削除
    1. 長い間,unstableであった点は削除する
    2. 点が統合された場合,それよりも前にある点は全て削除する
      • Indexマップ内で探索
    3. Indexマップ内の周囲に類似した点があれば1つにする

参考文献

  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.
  2. Kok-Lim Low, “Linear Least-Squares Optimization for Point-to-plane ICP Surface Registration,” Technical Report TR04-004, pp. 1 - 3, 2004.