MRS-Map

Stückler 2014 · 論文

一行要約 — サーフェルごとに形状と色の統計量を同時に保持するオクツリーベースの多解像度サーフェルマップであり、RGB-D SLAMにおいてノイズを考慮した確率的位置合わせ、キービューによるポーズグラフSLAM、CPU上でのリアルタイム物体トラッキングを可能にする。

問題

単一の固定解像度による密なRGB-Dマッピングは、避けられないトレードオフに直面する: 細かい解像度は詳細を捉えるがメモリを浪費し、粗い解像度は忠実度を失う。RGB-Dセンサーのノイズは深度に対して二次的に増大するため、すべての測定値を一つの解像度で扱うことは統計的に誤っている。KinectFusionのようなGPU依存のシステムは、さらに軽量あるいは低コストなロボットプラットフォームを排除してしまう。必要とされているのは、測定品質に応じて詳細度を適応させ、画像・マップ・複数視点物体モデルの高速でロバストな位置合わせを支える、完全にCPU上で動作するコンパクトな表現である。

手法とアーキテクチャ

• オクツリー内の多解像度サーフェル: すべてのオクツリーノード(内部ノード・葉ノードの両方、すべてのボクセルサイズにわたって)がサーフェル——その体積内にある点$\mathcal{P}$の法線近似——を保持する。統計量は十分統計量$\mathcal{S}(\mathcal{P}) := \sum_{p\in\mathcal{P}} p$と$\mathcal{S}^2(\mathcal{P}) := \sum_{p\in\mathcal{P}} p p^T$として維持され、点集合$\mathcal{P}^A, \mathcal{P}^B$の間では数値的に安定な1パス更新によって統合される。

$$\mathcal{S}^2(\mathcal{P}^{A\cup B}) \leftarrow \mathcal{S}^2(\mathcal{P}^A) + \mathcal{S}^2(\mathcal{P}^B) + \frac{\delta\delta^T}{N_A N_B (N_A + N_B)}, \qquad \delta := N_B\,\mathcal{S}(\mathcal{P}^A) - N_A\,\mathcal{S}(\mathcal{P}^B),$$

そこから平均$\mu = \frac{1}{\lvert\mathcal{P}\rvert}\mathcal{S}$と共分散$\Sigma = \frac{1}{\lvert\mathcal{P}\rvert-1}\mathcal{S}^2 - \mu\mu^T$が求まる。この分布は6次元であり、位置と、輝度と色度を分離するL$\alpha\beta$色空間における色を同時に扱う。最大6つの直交する視線方向によって、1ボクセル内の異なる表面を区別できる。サーフェルには最低10点が必要である。

• ノイズ適応的な集約: 各ピクセルにおけるオクツリーの最大解像度は、センサーからの距離の二乗(センサーノイズの法則に一致する)に適応させられ、同一ノードに落ちる連続した画像領域はまず集約される——640×480の画像は、307,200点の挿入ではなく数千のノード挿入だけで取り込まれる。画像境界や遮蔽境界(仮想境界)にあるノードは、表面を部分的にしか観測していないため除外される。
• 形状-テクスチャ記述子: 各サーフェルと、最大26個の同解像度隣接サーフェルとの間のFPFH風の3ビン角度ヒストグラム、加えて輝度/色度のコントラストヒストグラムを用いて、記述子距離$d_f(s_i,s_j) \le \tau$($\tau=0.1$)を閾値として対応関係をゲーティングする。
• 確率的位置合わせ: 姿勢$x$のもとで元マップ$m_s$(例えば現在の画像)を目標マップ$m_m$に位置合わせするには、サーフェルはすべての解像度で同時に対応付けられる——各ノードについて立方体クエリを通じて最も細かい共通解像度が選ばれるため、暗黙的にcoarse-to-fineとなる——各対応関係の観測尤度は2つの正規分布の差である。

$$p(s_{s,i} \mid x, s_{m,j}) = \mathcal{N}\big(d_{i,j}(x);\, 0,\, \Sigma_{i,j}(x)\big), \quad d_{i,j}(x) := \mu_{m,j} - T(x)\,\mu_{s,i}, \quad \Sigma_{i,j}(x) := \Sigma_{m,j} + R(x)\,\Sigma_{s,i}\,R(x)^T,$$

これにより、データから学習された共分散が信頼できる幾何情報を自動的に重み付けする——特徴点抽出は不要である。対数尤度$L(x) = \sum_{a\in\mathcal{A}} \log\lvert\Sigma_a(x)\rvert + d_a^T(x)\,\Sigma_a^{-1}(x)\,d_a(x)$は、高速な近似Levenberg-Marquardt法(通常10〜20回の反復)に続く約5回のNewton反復(モデルサーフェルの三線形補間を伴う)によって最適化される。評価はCPUコアにわたって並列化される。姿勢共分散の閉形式推定は、観測不能な次元(例えば平面を見ている場合)に沿った不確実性を捉える。

• ランダム化ループクロージングを備えたキービューSLAM: 画像は参照キービューに対して追跡される。十分な動きの後に新しいキービューが生成され、空間制約エッジが追加される。各フレームで、ちょうど1つの追加制約が、姿勢距離に関する$p_{\mathrm{chk}}(v_{\mathrm{cmp}}) = \mathcal{N}(d;0,\sigma_d^2)\cdot\mathcal{N}(\lvert\alpha\rvert;0,\sigma_\alpha^2)$に従ってサンプリングされた比較キービューによって検証される。マッチは双方向のサーフェルマッチング尤度によって検証される。キービューグラフ$p(\mathcal{V}\mid\mathcal{E}) \propto \prod_{e_{ij}} p(x_i^j \mid x_i, x_j)$は、フレームごとに1回、g2o(疎コレスキー分解)によって最適化される。最適化されたキービューは最終的に1つの複数視点サーフェルマップに融合され、これはリアルタイムの6自由度姿勢トラッキングのための物体モデルとしても利用できる。

実験結果

すべての計測は、ノートブック用Intel Core i7-3610QM(2.3 GHz、クアッドコア)上で、フル640×480解像度、最大マップ解像度0.0125 mで行われた。逐次位置合わせ(TUM RGB-Dベンチマーク、並進RPEの中央値): fr1/deskで4.4 mm(warpは5.8、GICPは10.2、3D-NDTは7.9、fovisは6.3)であり、ほとんどのfr1シーケンスで最良(例: fr1/plant 3.5 mm、fr1/xyz 2.6 mm、fr2/xyz 1.4 mm)。平均実行時間はfr1/deskで75.15 ms(warpは108.64、GICPは4015.4、3D-NDTは414.87)——約15 Hzであり、warpが破綻するようなフレームスキップに対してもICP同様のロバスト性を保持する。SLAM: 全フレームを処理した場合、11シーケンス中8つでRGB-D SLAMよりも良いRMSE RPEを達成した。例: freiburg1_room 0.111 m対0.219 m、freiburg2_desk 0.100 m対0.143 m、freiburg1_teddy 0.066 m対0.138 m。freiburg1_floor(テクスチャの乏しい床)とfreiburg2_large_loop(遠く不確実な深度)では失敗した。1回のグラフ最適化反復は数msまでで済む(freiburg2_desk: 最大64キービュー、138エッジで中央値0.79 ms)。物体モデリング/トラッキング: 360°モデル構築でATE中央値は約1〜2 cm。学習済みモデルのトラッキングはフレームあたり32〜50 msでATE中央値16〜30 mm。RoboCup@Home 2011/2012(両大会で優勝)においてロボットCoseroでライブ実演された。

SLAMにおける意義

MRS-Mapは、密なボクセルグリッドではなく統計的なサーフェル表現によって、控えめなメモリとGPUなしで高精度なRGB-Dトラッキングを支えられることを示し、実際のロボットハードウェア上での準密なSLAMと物体トラッキングを実現可能にした。その不確実性を考慮したサーフェル位置合わせ(NDTのRGB-D的な後継)と多解像度オクツリー設計は、ElasticFusionのような後発のサーフェルベースシステムやサーフェルベースのLiDARマッピングに影響を与え、2010年代中盤まで密なRGB-D SLAMシステム(DVO-SLAM、ElasticFusion)の標準的な比較対象となった。

関連ノート

• ElasticFusion
• TSDF vs Surfel maps
• KinectFusion
• DVO
• Pose graph optimization
• SuMa