Mugichoko's blog

Mugichoko’s blog

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

RGB-D SLAMを実装する #4

大域的目標

KellerらのRGB-D SLAM[1]が実装したい!と思い立ったので実装していく,というモチベーションの記録.ちょっとずつ実装している.今回が4回目.モチベーションに関する詳細は初回の記事を参照のこと.

前回(以下,参照)は,ICPアルゴリズム(画像ピラミッドなし)を実装した.今回は,画像ピラミッドを使ったICPに拡張したいと思う.

mugichoko.hatenablog.com

画像ピラミッドの実装

画像ピラミッドとは,元の画像サイズを1/4,1/16… と小さくしていった画像のこと.小さい画像での処理結果を1つ大きい画像での処理の初期値として利用することで,処理の高速化やロバスト化が見込める画像処理の常套手段である.今回私が実装した画像ピラミッドの生成手順は以下の通り.

  1. RGB-D画像(カラー画像{C}と奥行画像{D})を取得
    • ただし,ICPに必要なのは,今のところ奥行画像だけなので,以降,奥行画像に対しての処理について述べる
    • 元の画像サイズをLevel 0として,Level nまでの処理を考える
    • それぞれのLevelでの画像を{D_0}{D_1},…,{D_n}とする
  2. バイラテラルフィルタを使って{D_0}を平滑化した{D'_0}を生成
  3. {D'_0}の頂点マップと法線マップを生成
  4. {D'_{i}}の画像サイズを1/4(縦横それぞれ1/2)とした画像{D'_{i+1}}を作成
    • ただし,{i}の初期値は0
    • 私の実装では,単純にある注目画像とその右,下,右下の画素4点を足してその数で割るという処理にした
  5. {D'_{i+1}}の頂点マップと法線マップを生成
  6. 目的のLevelまで,4と5を繰り返し

もしかしたら,OpenGLのMipmapとか使えばもっと賢いコードが書けるのかもしれませんが,私は上記の処理に,GLSLのCompute Shaderを使いました.

画像ピラミッドを使ったICPの実装

参考文献[2]を真似すると,Levelは1~3までの3段階,それぞれのICPの最大ループ回数は4, 5, 10となっている.収束条件は,前回記事と同じにしている.よって,簡単だが処理は以下の通りとなる.

  1. 位置姿勢を保存するための行列{\bf T}のメモリ確保
    • 初期フレームの場合は単位行列{\bf T}とする
    • それ以外の場合,前フレームの結果を{\bf T}とする
  2. Level {i}を最大値に設定
    • 参考文献[2]に合わせて{i = 3}とする
  3. ICPをLevel {i}に対して実行しTを更新
  4. {i \leftarrow i - 1}
  5. {i} > 0の場合は3へ,それ以外の場合は終了

実装結果

前回と同様の方法で可視化したGifアニメーションが以下のものです. f:id:Mugichoko:20170918061103g:plain

Gifアニメーションではスピードが速く解像度も微妙なので,あるフレームのLevel 3~1の画像を以下に示します.解像度が低いものから高いものへと移り変わっていっているのが分かると思います.尚,見やすさのため,全Levelの画像をLevel 0に合わせて最近傍補間で拡大しています.

f:id:Mugichoko:20170918061324j:plain Level 3の画像.本当は160×120画素の画像.

f:id:Mugichoko:20170918061338j:plain Level 2の画像.本当は320×240画素の画像.

f:id:Mugichoko:20170918061341j:plain Level 1の画像.もともと640×480画素の画像.

実行速度は,1フレームあたり約60msでした.前回の画像ピラミッドなしのICPと比較して,処理速度が向上していることが分かります.

SLAMとして成り立つようになったら,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.
  2. R. A. Newcombe, [tex:{\textit et al}] “KinectFusion: Real-time Dense Surface Mapping and Tracking,” Proc. Int. Conf. on Mixed and Augmented Reality, 2011.