ElasticFusion

Whelan 2015 · 論文

一行要約 — ポーズグラフを使わずに、ループ閉じ込み時にマップへ直接非剛体的な弾性変形を適用することで、大域的に一貫した再構成を実現するサーフェルベースの密なRGB-D SLAMシステム。

問題

密なSLAMシステムは、拡張的かつループの多い動き、たとえば手持ちの深度カメラで部屋を「塗り込む」ような動きに苦戦してきた。KinectFusionの固定ボリュームはシーンの大きさを制限する。Whelanの以前の研究であるKintinuousはポーズグラフの変形によって廊下のような環境にスケールするが、局所的にループの多い軌道ではうまく機能せず、再訪したマップ領域を再利用できない。DVO-SLAMはキーフレームのポーズを最適化するが、明示的な連続表面を構築しない。ElasticFusionは優先順位を逆転させる。すなわち、カメラの軌道(ポーズグラフ)を最適化してマップを再構築するのではなく、マップそのものを最適化する。すなわち、表面のループ閉じ込みを早期かつ頻繁に適用することで、システムは常にマップ分布のモードの近くに留まり続ける。

手法とアーキテクチャ

フレームごとのループ: RGB-D入力 → スプラッティングされたサーフェル予測 → フレーム対モデルのICP+RGBトラッキング → サーフェル融合 → 局所(モデル対モデル)ループチェック → 大域(フェルン)ループチェック → 非剛体変形。CUDAがトラッキングの縮約とマップ管理を行い、OpenGLがビュー予測を行う。

• 融合サーフェルマップ: 位置$\mathbf{p}\in\mathbb{R}^3$、法線$\mathbf{n}$、色$\mathbf{c}$、重み$w$、半径$r$、初期化タイムスタンプ$t_0$、最終更新タイムスタンプ$t$を持つサーフェルの順序のないリスト$\mathcal{M}$。時間ウィンドウ$\delta_t$は$\mathcal{M}$をアクティブサーフェル$\Theta$(最近観測され、トラッキングと融合に使われる)と非アクティブサーフェル$\Psi$(ループによって再活性化されるまで使われない)に分割する。
• 結合フレーム対モデルトラッキング: 各フレームは、アクティブモデルのスプラッティングされたレンダリング(深度と完全な色)に対して位置合わせされる。幾何項は、ライブ深度マップと予測深度の間の点対平面ICPである。

$$E_{\mathrm{icp}} = \sum_{k} \Big( \big(\mathbf{v}^k - \exp(\hat{\boldsymbol{\xi}})\,\mathbf{T}\,\mathbf{v}_t^k\big)\cdot\mathbf{n}^k \Big)^2 ,$$

そして測光項$E_{\mathrm{rgb}}$は、ライブカラー画像と予測されたアクティブモデルの色との輝度差にペナルティを課す。結合コスト$E_{\mathrm{track}} = E_{\mathrm{icp}} + w_{\mathrm{rgb}} E_{\mathrm{rgb}}$($w_{\mathrm{rgb}} = 0.1$)は、3レベルの粗から密へのピラミッドにわたってガウス・ニュートン法で最小化される(GPUのツリー縮約が6×6の系を構築し、CPUのコレスキー分解がそれを解く)。

• 時間方向にサンプリングされ接続された変形グラフ: 各フレームで、サーフェルからノード(位置$\mathcal{G}^n_{\mathbf{g}}$、変換$\mathcal{G}^n_{\mathbf{R}}, \mathcal{G}^n_{\mathbf{t}}$、タイムスタンプ)の新しいグラフ$\mathcal{G}$がサンプリングされる。接続関係は初期化時刻の順序(k=4近傍)に従い、これにより時間的に無相関な同じ表面へのパスが互いに影響を与えることを防ぐ。サーフェルは、それに影響を与えるノードによって変形される。

$$\hat{\mathcal{M}}^s_{\mathbf{p}} = \sum_{n\in I} w^n \big[ \mathcal{G}^n_{\mathbf{R}} (\mathcal{M}^s_{\mathbf{p}} - \mathcal{G}^n_{\mathbf{g}}) + \mathcal{G}^n_{\mathbf{g}} + \mathcal{G}^n_{\mathbf{t}} \big], \qquad w^n = \big(1 - \lVert \mathcal{M}^s_{\mathbf{p}} - \mathcal{G}^n_{\mathbf{g}} \rVert_2 / d_{\max} \big)^2 .$$

• 変形の最適化: 表面の対応関係$\mathcal{Q}$(元の点、目標の点、タイムスタンプ)が与えられると、グラフのパラメータは$E_{\mathrm{def}} = w_{\mathrm{rot}} E_{\mathrm{rot}} + w_{\mathrm{reg}} E_{\mathrm{reg}} + w_{\mathrm{con}} E_{\mathrm{con}} + w_{\mathrm{con}} E_{\mathrm{pin}}$($w_{\mathrm{rot}}{=}1, w_{\mathrm{reg}}{=}10, w_{\mathrm{con}}{=}100$)を最小化する。剛性項$E_{\mathrm{rot}} = \sum_l \lVert \mathcal{G}^{l\top}_{\mathbf{R}}\mathcal{G}^{l}_{\mathbf{R}} - \mathbf{I} \rVert_F^2$、グラフのエッジにわたる埋め込み変形の滑らかさ項$E_{\mathrm{reg}}$、制約項$E_{\mathrm{con}} = \sum_p \lVert \phi(\mathcal{Q}^p_{\mathbf{s}}) - \mathcal{Q}^p_{\mathbf{d}} \rVert_2^2$、そして非アクティブ領域を固定してアクティブモデルが非アクティブな座標系に変形するようにするピン項がある。CPU上でガウス・ニュートン法と疎コレスキー分解で解かれ、その後GPU上で全サーフェルに適用される。
• 局所ループ閉じ込み: 大域ループが発火していない場合、毎フレームで、現在の姿勢からのアクティブモデルと非アクティブモデルの予測レンダリングが同じICP+RGB法で位置合わせされる。この位置合わせは、残差が十分小さく、インライアが十分あり、ヘシアン由来の共分散の固有値が閾値以下である場合にのみ受け入れられる。受け入れられた制約はマップを変形させ、マッチした非アクティブサーフェルを再活性化する。多くの小さなループが連続的に閉じられる。
• 大域ループ閉じ込み: (生フレームではなく)80×60にダウンサンプリングされた予測ビューに対するランダム化フェルン符号化データベースが、任意のドリフト後の再訪を検出する。マッチしたビューは位置合わせされ、(最適化後の$E_{\mathrm{con}}$も含めて)チェックされ、マップ全体の変形として適用される。ポーズグラフも軌道の記録も不要である。

実験結果

• 軌道(TUM RGB-D, ATE RMSE): fr1/desk 0.020m、fr2/xyz 0.011m、fr3/office 0.017m、fr3/nst 0.016mであり、DVO SLAM(0.021/0.018/0.035/0.018)、RGB-D SLAM(0.023/0.008/0.032/0.017)、MRSMap(0.043/0.020/0.042/2.018)、Kintinuous(0.037/0.029/0.030/0.031)と同等かそれ以上である。
• 表面再構成(ICL-NUIM合成リビングルーム): kt0〜kt3における真値モデルへの平均距離は0.007/0.007/0.008/0.028mであり、比較した全システムより優れている(例: Kintinuousは0.011/0.008/0.009/0.150m)。軌道ATEは0.009/0.009/0.014/0.106m。kt3におけるアブレーション: 局所ループのみで表面誤差0.099m、大域ループのみで0.103m。どちらも必要である。
• 規模と速度: 400万を超えるサーフェルからなる包括的な部屋のスキャンがリアルタイムで取得された。Hotelシーケンスは7725フレームを4.1Mサーフェル、328グラフノード、11回の局所ループ閉じ込みと1回の大域ループ閉じ込みで処理する。平均フレーム時間は31ms、最悪45ms(最悪ケースで約22Hz)であり、Intel Core i7-4930KとNVIDIA GTX 780 Tiで動作させた。

SLAMにおける意義

ElasticFusionは、「マップそのものが状態である」という設計を実用可能にした。カメラの軌道を補正して計測を再統合するのではなく、密な表面自体を補正し、頻繁な小さな変形によってマップ分布のモードの近くに留まり続ける。これはサーフェルベースの密なSLAMの標準的なバックボーンとなり、SemanticFusionはそのサーフェル上に直接CNNの意味情報を付加する。そのアクティブ/非アクティブモデルの分割、モデル対モデルの局所ループ、フェルンによる再位置決めは、後の密なシステム全体に繰り返し現れる。オンラインのループ閉じ込みを伴う室内規模の高品質な密な再構成を学ぶために研究すべき論文である。

関連ノート

• KinectFusion
• Kintinuous
• SemanticFusion
• TSDF vs Surfel maps
• Frame-to-model tracking
• BundleFusion