SLAM++

Salas-Moreno 2013 · 論文

一行要約 — 初のオブジェクト指向SLAMシステムであり、地図は「その場」で行われるリアルタイム3Dオブジェクト認識によって構築された6自由度オブジェクトポーズの明示的なグラフであり、生の幾何地図に比べてごく一部の記憶量で密な予測能力を提供する。

問題

SLAM++以前、リアルタイムSLAMシステムは点、線、パッチ、深度マップなどの非パラメトリックな表面といった低レベルのプリミティブ上で動作しており、これらを頑健にマッチングし、幾何的に変換し、最適化する必要があった。これは膨大な計算を浪費し、ドメイン知識を無視している。多くの屋内シーンは、椅子、テーブル、モニターなど、繰り返し出現するドメイン固有のオブジェクトや構造から構成されている。これらのオブジェクトに関する事前知識をSLAM運用そのもののループの中で活用すれば、深度データからのオブジェクト認識が生の幾何処理を置き換えることができ、コンパクトで意味的に意味のある地図、強力なループクロージング制約、オブジェクトグラフからの密な表面予測が得られる。

手法とアーキテクチャ

パイプライン(論文の図2): ライブフレームの表面計測(頂点マップ $\mathcal{D}_l$ + 法線マップ $\mathcal{N}_l$)→(1) 現在のSLAMグラフ $\mathcal{G}$ からレンダリングされた密なマルチオブジェクトシーン予測に対するICPによるライブカメラポーズ推定→(2) オブジェクト検出とグラフへの挿入→(3) ポーズグラフ最適化→(4) 次の予測とアクティブなオブジェクト探索のための表面レンダリング。

• オブジェクトデータベース: 繰り返し出現する各オブジェクトは、KinectFusionで一度スキャンされ、マーチングキューブによってメッシュが抽出され、クリーンアップされ、標準座標フレームと共に保存される。
• リアルタイムオブジェクト認識(GPU): Drostらの手法にならい、Point-Pair Features(PPF、指向性のある点対の相対位置・法線に関する4次元記述子)がGeneralised-Houghパラメータ空間に投票する。投票は64ビットコードにパックされ、GPU上での並列ソート+リダクションによって蓄積される。16万PPFに対する大域モデル記述の構築は5ms未満で完了する。検出されたポーズは回転で約±30°、並進で約±50cmの誤差を含むため、各候補は挿入前に2回目のICP推定によって洗練され、ゲーティングされる。
• オブジェクトグラフに対するカメラトラッキング: カメラ・モデル間変換は、ライブ深度マップの有効な全画素 $u \in \Omega$ にわたる点対平面誤差を反復的に最小化することで推定される。

$$E_c(\mathbf{x}) = \sum_{u\in\Omega} \psi\big(e(u,\mathbf{x})\big), \qquad e(u,\mathbf{x}) = \mathcal{N}_r(u')^\top\big(\exp(\hat{\mathbf{x}})\,\hat{v}_l(u) - v_r(u')\big),$$

ここで $\mathbf{x}\in\mathbb{R}^6$ は増分ツイスト、$v_r, \mathcal{N}_r$ は予測された頂点/法線マップ、$u'$ は射影的に関連付けられた画素、$\psi$ は頑健なHuberペナルティである。ガウス・ニュートン正規方程式($6\times6$、コレスキー分解)は最大10回反復される。KinectFusionの部分的なモデルとは異なり、完全で高品質なマルチオブジェクト予測に対して追跡することで、すでに説明済みの画素をマスクすることも可能になり、オブジェクト探索は未記述の領域のみに焦点を当てることができる(「アクティブサーチ」)。

• グラフ最適化: 世界は、$\mathbf{SE}(3)$上の過去のカメラポーズ $T_{wi}$ とオブジェクトポーズ $T_{wo_j}$ のグラフであり、6自由度のカメラ・オブジェクトICP計測 $Z_{i,o_j}$ とカメラ・カメラ制約 $Z_{i,i+1}$ を持つ。

$$E_m = \sum_{Z_{i,o_j}} \big\lVert \log\big(Z_{i,o_j}^{-1} \cdot T_{wi}^{-1} \cdot T_{wo_j}\big) \big\rVert_{\Sigma_{i,o_j}} + \sum_{Z_{i,i+1}} \big\lVert \log\big(Z_{i,i+1}^{-1} \cdot T_{wi}^{-1} \cdot T_{wi+1}\big) \big\rVert_{\Sigma_{i,i+1}},$$

ここで $\lVert\mathbf{x}\rVert_\Sigma := \mathbf{x}^\top\Sigma^{-1}\mathbf{x}$ はマハラノビス距離、$\log(\cdot)$ は $\mathbf{SE}(3)$ の対数写像であり、逆共分散はICPのヘシアン $\Sigma^{-1} = J^\top J$ によって近似される。レーベンバーグ・マーカート法と疎コレスキーソルバ(g2o)で解かれる。

• 構造的事前情報: オブジェクトごとの単項の地面平面制約がエネルギー($E_{m\&p}$)に追加され、最初の観測から検出された共通平面にオブジェクトを固定する。
• 再位置特定と大規模ループクロージング: 追跡失敗後、新しいローカルグラフが追跡され、それが少なくとも3つのオブジェクトを含むようになると、両方のグラフを指向性のある点のメッシュ(オブジェクト位置を頂点、オブジェクトのx軸を法線として)として扱い、同じPPF認識機構を通して長期グラフと照合される。ICP整合性ゲーティングに失敗した移動オブジェクトは無効とマークされ、グラフを損なわないようにされる。

実験結果

• 実際のループクロージングと追跡失敗後の再位置特定を含む、約10分間続くリアルタイムの実行で、15×10×3mの大きな共有空間をマッピングした。34個のオブジェクトインスタンス(2種類の椅子、2種類の丸テーブル)が、地面平面の事前情報のみで制約されてマッピングされた。
• ゲーミングノートPC上での10×6×3mの部屋に対するシステム統計: 20fps、132のカメラポーズ、5つのオブジェクトクラスから35個のオブジェクト、338のグラフエッジ。
• メモリ: 350KBのグラフ+20MBのオブジェクトデータベースに対し、同じ体積のKinectFusion TSDF(1ボクセルあたり4バイト、1mあたり128ボクセル)では1.4GB — 同等の予測能力に対しておよそ1/70の圧縮率。
• 移動オブジェクト検出(不整合なオブジェクトをフラグ付けし除外)と、コンテキストを意識したAR(仮想キャラクターがオブジェクト地図をナビゲートし、部屋全体をスキャンせずに座る場所を見つける)を実証した。

SLAMにおける意義

SLAM++は、オブジェクトレベルSLAMという研究方向を開拓し、意味的エンティティから構築された地図が、オブジェクトレベルでのループクロージング、再位置特定、シーン理解を可能にしつつ、生の幾何よりも桁違いにコンパクトになり得ることを示した。その主要な限界 — スキャン済みインスタンスの事前構築データベース — は、その系統がまさに取り除いたものである。Fusion++はMask R-CNNでオブジェクトをオンザフライで発見し、MoreFusionは複数オブジェクトの6Dポーズを推論し、NodeSLAMとDSP-SLAMはメッシュを学習済み形状事前情報で置き換える。意味的エンティティが生の幾何よりも強力な地図要素となるという中核的な洞察は、現代の3Dシーングラフシステムにも再び現れている。

関連ノート

• Fusion++
• MoreFusion
• DSP-SLAM
• KinectFusion
• Pose graph optimization