VDO-SLAM

Zhang 2020 · 論文

一行要約 — カメラ軌跡と剛体の移動物体のSE(3)モーションを1つのファクターグラフで同時推定する、動的物体を意識したSLAMシステム。事前の物体モデルを必要としない。

問題

動的環境におけるロボットの経路計画と障害物回避は、ロボット周囲の物体がどのように動いているかを知ることに依存する——しかし古典的なSLAMは静的な世界を仮定し、ほとんどの「動的SLAM」システムは単に動いているコンテンツを検出して破棄するだけである。VDO-SLAMは代わりに、動的な剛体を推定問題そのものに統合する。物体の形状や幾何モデルについて事前知識を必要とせず、あらゆる移動物体の完全なSE(3)モーションをカメラと同時に識別・追跡・推定する。

手法とアーキテクチャ

前処理。 インスタンスセグメンテーション（Mask R-CNN、COCO重み）が移動可能性のある物体を分離する。密なオプティカルフロー（PWC-Net）がどこでも対応点を提供し、少ない画素しか占めない物体上の追跡点数を最大化し、物体ごとの識別子を伝播する（セグメンテーションが失敗した場合にはマスクを復元する）。入力はRGB-D、ステレオから変換した深度、または「学習ベースモノキュラー」（MonoDepth2の深度を利用）である。

モーション表現。 カメラポーズ${}^{0}\mathbf{X}_k$と物体ポーズ${}^{0}\mathbf{L}_k \in \mathrm{SE}(3)$が与えられたとき、剛体上の点${}^{0}\mathbf{m}^i_k$は以下に従う:

$${}^{0}\mathbf{m}^{i}_{k} = {}^{0}\mathbf{L}_{k-1}\; {}^{L_{k-1}}_{k-1}\mathbf{H}_{k}\; {}^{0}\mathbf{L}^{-1}_{k-1}\; {}^{0}\mathbf{m}^{i}_{k-1} = {}^{0}_{k-1}\mathbf{H}_{k}\; {}^{0}\mathbf{m}^{i}_{k-1},$$

すなわち、物体のフレーム間モーション${}^{0}_{k-1}\mathbf{H}_k \in \mathrm{SE}(3)$が、物体ポーズを状態に一度も含めることなく、その物体自身の点を時間軸で関連付ける——これがモデルフリー動作を可能にする鍵である。

フロントエンド推定。 カメラポーズは静的な点にわたるHuberロバスト化された再投影誤差$\mathbf{e}_i = {}^{I_k}\tilde{\mathbf{p}}^i_k - \pi({}^{0}\mathbf{X}^{-1}_k\,{}^{0}\mathbf{m}^i_{k-1})$を最小化する。各物体のモーションは類似のコスト$\mathbf{e}_i = {}^{I_k}\tilde{\mathbf{p}}^i_k - \pi({}^{0}_{k-1}\mathbf{G}_k\,{}^{0}\mathbf{m}^i_{k-1})$を最小化する（${}^{0}_{k-1}\mathbf{H}_k = {}^{0}\mathbf{X}_k\,{}^{0}_{k-1}\mathbf{G}_k$）。両者は$\mathfrak{se}(3)$上でパラメータ化され、Levenberg-Marquardt法で解かれる。重要な点として、オプティカルフローは各モーションと同時にリファインされる（正則化項が初期フローにアンカーする）ため、点の追跡長が大幅に伸びる。物体はシーンフローの大きさ（閾値0.12、点の30%以上が動いていれば動的と判定）で動的と判定される。

バックエンドのファクターグラフ。 カメラポーズ、静的な点、動的な点、物体モーションが、4種類のファクターを持つ1つのグラフで洗練される:

$$\mathbf{e}_{i,k} = {}^{0}\mathbf{X}^{-1}_k\,{}^{0}\mathbf{m}^i_k - \mathbf{z}^i_k \quad \text{(3D点の観測)}, \qquad \mathbf{e}_{i,l,k} = {}^{0}\mathbf{m}^i_k - {}^{0}_{k-1}\mathbf{H}^l_k\,{}^{0}\mathbf{m}^i_{k-1} \quad \text{(3項の点-モーション)},$$

加えてオドメトリファクターと、急激な物体モーション変化に罰則を与える滑らかなモーション事前情報$\mathbf{e}_{l,k} = ({}^{0}_{k-2}\mathbf{H}^{l}_{k-1})^{-1}\,{}^{0}_{k-1}\mathbf{H}^l_k$。修正版g2oでLM法により解かれる。物体の線速度はモーションからそのまま導出される: $\mathbf{v} \approx {}^{0}_{k-1}\mathbf{t}_{k} - (\mathbf{I}_3 - {}^{0}_{k-1}\mathbf{R}_{k})\,\mathbf{c}_{k-1}$、ここで$\mathbf{c}_{k-1}$は物体の点重心である。

実験結果

• Oxford Multimotion（swinging_4_unconstrained、500フレーム）: カメラ誤差$E_t$ = 0.0112 m / $E_r$ = 0.77°、対するMVOは0.0314 m / 1.19°（カメラ推定は約35%改善、揺れる箱では15〜40%改善）。物体モーション推定はほとんどの箱でClusterVOより2倍以上正確だが、カメラ並行移動ではClusterVOがわずかに勝る（0.0066 m）。
• KITTI tracking（動的シーケンス9個）: カメラ精度はDynaSLAM IIと同等（回転誤差はわずかに低く、並行移動誤差はわずかに高い）。物体モーションの並行移動誤差は0.1〜0.3 m、回転誤差は0.2〜1.5°（RGB-D）——CubeSLAMの3 m超/3°超に対し、ほとんどの場合で一桁の改善である。物体は出現時間の80%超で追跡され、推定速度は正解値に一貫して近い（例: セグメンテーション失敗の33フレームにわたって追跡されたバンで、平均速度誤差2.64 km/h）。
• アブレーション: フローの結合リファインメントにより長い追跡が大幅に増加する（例: シーケンス01の背景で、5フレーム以上追跡された点が237から5075へ）。誤差はカメラで約10%、物体で約25%低下する。全体グラフのリファインメントは物体モーション誤差を最大39%（並行移動）/55%（回転）削減する。
• 実行速度: ラップトップCPU（i7 2.6 GHz）上でトラッキングは5〜8 fps。セグメンテーション/フローはオフラインで計算される。

SLAMにおける意義

VDO-SLAMは「推定する、破棄しない」学派の動的SLAMにおける画期的な研究であり、動的物体は外れ値ではなく一級の推定対象となる。物体ポーズ変数を用いずに剛体上の点を時間軸で関連付けるモデルフリーの点-モーションファクターは、現在では動的SLAMの標準となったエレガントな定式化であり、オープンソース公開により、運転およびモバイルロボットシナリオにおけるカメラ/物体の結合推定の基準ベースラインとなった。

関連ノート

• DynaSLAM — 動的コンテンツの検出と除去のアプローチ
• DynaSLAM II — 同じ精神に基づく密結合の複数物体追跡とSLAM
• MID-Fusion — RGB-Dによる動的物体の物体単位密トラッキング
• MaskFusion — 移動物体のリアルタイム認識と再構成
• PWC-Net — VDO-SLAMが基盤とする密なオプティカルフローフロントエンド
• Factor graph — カメラと物体の両状態をホストする仕組み