MID-Fusion

Xu 2019 · 論文

一行要約 — オクトリーに基づく物体レベル・マルチインスタンスの動的RGB-D SLAMシステムであり、シーンの動きに対してロバストなまま、各物体ごとに個別の体積(TSDF)モデルを構築する。

問題

静的な世界を想定したSLAMは動く物体をアウトライアとして扱うため、動的環境では追跡を失うか地図を破損させる — 生き残った場合でも、地図にはロボットが利用できる物体単位の情報が一切含まれない。MaskFusionとCo-Fusionはサーフェルベースの物体レベル動的SLAMを示していたが、サーフェルクラウドは自由空間や表面の連結性を直接表現できず、これはロボットのタスクに必要なものである。MID-Fusionは体積的な物体レベルの動的SLAMを目指す。すなわち、動的シーンにおけるロバストなカメラ追跡と、任意の物体の幾何的・セマンティック的・動き的な性質の継続的な推定である。

手法とアーキテクチャ

パイプラインは4つの部分から構成される — セグメンテーション、追跡、融合、レイキャスティング。検出された各物体 $n \in \{0..N\}$(0は背景)は独自の座標フレームに存在し、Supereight上に構築されたオクトリーベースのTSDFボリュームを持つ。そのボクセルはSDF、輝度、前景確率と重み、COCOクラス分布、姿勢 $T_{WO_n}$、そして動的/静的フラグを格納する。

カメラ追跡。 ライブ姿勢 $T_{WC_L}$ は、有効なピクセル(人間を除く)にわたる結合的な高密度残差を最小化する。

$$E_{\mathrm{track}}(T_{WC_L}) = \tfrac{1}{2}\Big(\sum_{\mathbf{u}_L \in M_L} w_{\mathrm{g}}\,\rho(e_{\mathrm{g}}) + \sum_{\mathbf{u}_R \in M_R} w_{\mathrm{p}}\,\rho(e_{\mathrm{p}})\Big),$$

ここで $\rho$ はCauchy損失、$e_{\mathrm{g}}$ はライブ頂点マップとモデルがレンダリングした頂点/法線マップとの間の点対平面ICP残差、$e_{\mathrm{p}}$ はレンダリングされた(ノイズ除去された)モデル深度に対して評価される光度学的残差である。残差は計測の不確実性で重み付けされる。RGBについては定数、ICPについては $w_{\mathrm{g}} = 1/(\mathbf{n}^{r\top}\mathbf{n}^{r}\,\sigma_D^{\top}\sigma_D)$ であり、センサーモデル $\sigma_D = \big(\tfrac{D}{f}\sigma_{xy},\ \tfrac{D}{f}\sigma_{xy},\ \tfrac{D^2}{fb}\sigma_z\big)$($f$は焦点距離、$b$は基線長)を用いる。Gauss–Newton法、3段階の粗密ピラミッドで解かれる。追跡は1フレームにつき2回実行される。まず人物を除くすべてのモデル頂点に対して実行され、その後 — レイキャスティングによって可視の物体が明らかになり、残差テストが動く物体にフラグを立てた後(物体は、そのレンダリングされたマスク内でのインライア比率が0.9を下回ると動的と見なされる)— 静的な部分のみに対して精緻化される。

物体姿勢推定。 動く物体は同じ重み付きICP + RGBコストで追跡されるが、物体座標系で再パラメータ化され、「仮想カメラ」ではなく $T_{C_LO_L}$ を直接推定する。回転のヤコビ行列はそのとき $C_{C_LO_L}^{-1}(\mathbf{v}_L - \mathbf{r}_{C_LO_L})$ に比例し、物体座標系が物体を中心としているため小さい — レバーアームが小さくなり、物体の大きな回転のもとでもより安定した収束をもたらす。

セグメンテーション。 Mask R-CNNのマスクは幾何学的なエッジによって精緻化され、既存のモデルにレンダリングされたマスクのIoUによって対応付けられる(0.5を超える場合に受理、クラスの一致は不要。ラベルは地図上で確率的に精緻化されるため)。その後、動きの残差によってフィルタされる — 結合ICP+RGB残差が高いピクセルはアウトライアとして扱われる。前景確率(マスク内では1、膨張された背景では0、Mask R-CNNが見逃した物体では0.5)が不良なマスクに対する融合を保護する。

融合。 深度、色、前景確率、セマンティック分布は各物体のオクトリーTSDFに統合される(セマンティクスはベイズ更新ではなく平均化によって行われる。ベイズ更新はMask R-CNNの予測に対して過度に確信を持ってしまうため)。新しい物体は、逆投影された点群サイズの3倍のボリュームで、約1 mmのボクセル解像度で初期化される — これは未使用のオクトリーボクセルが決して割り当てられないためにのみ実現可能である。

実験結果

• TUM RGB-D、ATE RMSE(cm): f3s_static 1.0、f3s_xyz 6.2、f3s_halfsphere 3.1、f3w_static 2.3、f3w_xyz 6.8、f3w_halfsphere 3.8 — ほぼすべてのシーケンスで、比較対象の高密度追跡手法の中で最良(f3w_halfsphereでは: VO-SF 73.9、StaticFusion 39.1、Co-Fusion 80.3、MaskFusion 10.6)。特徴ベースのDynaSLAMはさらに低い(例えばf3w_halfsphereで2.5) — これを著者は疎な追跡と高密度な物体マッピングを組み合わせる根拠として指摘している。
• 合成フォトリアリスティックシーン: Co-Fusionより低い物体再構成誤差。そのセグメンテーションを正解マスクに置き換えても結果はほとんど変わらないが、仮想カメラ追跡のアブレーションは物体の大きな回転のもとで明らかに悪化する — 物体中心のパラメータ化の妥当性を裏付ける。
• 実行時間: Mask R-CNNを除いてCPU(i7-7700、32 GB)上で2〜3 Hz。25個を超える物体がインスタンス化された状態で1フレームあたり約400 ms、新しい物体の初期化には約10 ms。単一のRGB-Dカメラによる最初の物体レベル動的体積地図。

SLAMにおける意義

MaskFusionとともに、MID-Fusionは物体レベルの動的SLAMを定義した。個別に追跡・再構成された物体インスタンスから構成される地図である。体積という選択は重要である — 物体ごとの水密なTSDFは、サーフェルクラウドにはできない形で把持計画、衝突判定、自由空間推論を支える — そしてその物体中心の追跡パラメータ化と不確実性重み付き残差は標準的な手法となった。これは動的SLAMが単に動きを生き延びることから、それをモデル化することへと移行した転換点を示している。

関連ノート

• MaskFusion
• VDO-SLAM
• DynaSLAM II
• Fusion++
• TSDF vs Surfel maps