Mugichoko's blog

Mugichoko’s blog

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

RGB-D SLAMを実装する #3

大域的目標

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

mugichoko.hatenablog.com

今回の目標

  1. ICPアルゴリズムの理解
  2. ICPアルゴリズム(画像ピラミッドなし)のGPU実装

ICPアルゴリズムの理解

今回実装したいICPアルゴリズムは,「センサから得られた2つの点群の位置合わせを行う」というよりは,「(連続して撮影された)奥行画像2枚から得られる点群同士の位置合わせ」を行う,という説明がしっくりくる.参考文献[1][2][3]のICPに該当する部分を読んで理解した内容が以下の通り.

  1. 奥行画像2フレームから頂点マップを生成
    • 以降,各フレームをフレーム{S},フレーム{D}と呼称
    • 各頂点マップを{V_S}{V_D}とする
    • 各頂点マップの3次元位置を{\bf s}{\bf d}とする
  2. フレーム{D}から法線マップ{N}を生成
    • 各法線を{\bf n}とする
  3. フレーム{S}の頂点マップ{V_S}をフレーム{D}に投影し{V'_S}を生成
    • この時,フレーム上の2次元位置{\bf u}上にあるフレーム{S}{D}のデータが対応付いていると考える
    • 投影する際の位置姿勢は初期状態では未知のため単位行列とする
    • それ以降は,5で得られたパラメータから計算した行列を用いる
  4. フレーム{D}上の頂点マップ{V_D},法線マップ{N},そして{V'_S}より,以下の式8と10にある行列{\bf A}とベクトル{\bf b}を生成 f:id:Mugichoko:20170821222943j:plain 参考文献[2]より
  5. 行列{\bf A},ベクトル{\bf b}及び求めたいベクトル{\bf x}(式9)は以下の式13の関係にあるため,SVD等を用いてベクトル{\bf x}を計算
    • {\bf x}の各要素は左から順に,ヨー,ピッチ,ロール,3次元位置,となっている
    • フレーム{S}の座標系からフレーム{D}の座標系への変換に相当する
      • EigenやOpenCVを使えばよい(所謂,solve関数) f:id:Mugichoko:20170821222930j:plain
  6. 収束条件を満たしていれば終了 or 3へ移動

ICPアルゴリズム(画像ピラミッドなし)のGPU実装

GPUが絡むので,どこまでをGPU処理するか考える.私のデザインは以下の通り.

  1. RGB-D画像を取得
  2. RGB-D画像をGPUに転送
  3. 「ICPアルゴリズムの理解」の2~4までをGPUで処理
  4. 「ICPアルゴリズムの理解」の5及び6をCPUで処理

実装結果

実装したICPアルゴリズムを使って,2フレーム間のトラッキングを行ってみた.以下のGifアニメーションは,「ICPアルゴリズムの理解」で説明した行列{\bf A}とベクトル{\bf b}の各要素をRGBA画像に置き換えて表示させた結果です.2フレームの情報が最初は離れているため,画像的にダブったように見えますが,時間が経つにつれていちしせいすいていがしんこう位置姿勢推定が進行していくため,合わさっていき,はっきりとした画像になったように見えると思います.

f:id:Mugichoko:20170918050540g:plain

1フレーム辺り平均で約80msかかっています.尚,画像サイズ640×480画素で,ICPの各ループでの収束条件は,ICPでの現在のループと1つ前のループで得られたパラメータ{\bf x}_i及び{\bf x}_{i-1}の距離(L2ノルム; {\sqrt{{\bf x}_i \cdot {\bf x}_{i-1}}})を取った際,それが{1.0^{-3}}を下回っていた場合,収束したとみなしています.システム構成は以下の通り.

上手く全部実装できたらGitHubとかにプログラムを挙げたいと思っています.

参考文献

  1. M. Keller, [tex:{\textit et al}] “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, Dept. of Computer Science, University of North Carolina at Chapel Hill, 2004.
  3. R. A. Newcombe, [tex:{\textit et al}] “KinectFusion: Real-time Dense Surface Mapping and Tracking,” Proc. Int. Conf. on Mixed and Augmented Reality, 2011.