DM-VIO

von Stumberg 2022 · 論文

一行要約 — 2つの新しい手法 — 遅延マージナリゼーションとIMU初期化のためのポーズグラフバンドル調整(PGBA) — に基づく直接法(DSOベース)の単眼VIOであり、単一のカメラとIMUだけでステレオ慣性システムをも上回る性能を示す。

問題

マージナリゼーションはスライディングウィンドウ型VIOをリアルタイムに保つが、「それは容易に元に戻すことができず、接続された変数の線形化点は固定されなければならない」(初期推定ヤコビアン、First-Estimates Jacobians)。これは特に単眼VIOで問題となる。なぜなら、最初のキーフレームがマージナリゼーションされた瞬間にスケールがマージナリゼーション事前分布に結びつけられるにもかかわらず、スケールは(車でよく見られる)等速運動下では観測不能であるため、その後も大きく変化しうるからである。従来のシステムは、視覚のみの推定と別個のIMU初期化を行う(測光の不確実性を失う)か、VI-DSOのように任意のスケールで即座に初期化し、情報を失う動的マージナリゼーションを用いるかのいずれかであった。DM-VIOは、初期化器で視覚の不確実性を完全に捉える方法、それを主システムへ引き継ぐ方法、スケールが変化した際に事前分布の整合性を保つ方法という3つの問いに、単一の仕組みで答える。

手法とアーキテクチャ

中核は、すべてのアクティブなキーフレームに対する視覚慣性バンドル調整であり、Levenberg-Marquardt法で最小化される(測光部分はDSOからSIMD高速化され、その他の因子はGTSAMで処理される):

$$E(\mathbf{s}) = W(e_{\text{photo}}) \cdot E_{\text{photo}} + E_{\text{imu}} + E_{\text{prior}}$$

• 測光エネルギー(DSO方式)。 各アクティブキーフレーム $i \in \mathcal{F}$ は、観測フレーム $j$ に投影される点 $\mathbf{p}$ をホストする: $$E_{\mathbf{p}j} = \sum_{\mathbf{p} \in \mathcal{N}_{\mathbf{p}}} \omega_{\mathbf{p}} \left\lVert (I_j[\mathbf{p}'] - b_j) - \frac{t_j e^{a_j}}{t_i e^{a_i}} (I_i[\mathbf{p}] - b_i) \right\rVert_{\gamma},$$ アフィン輝度パラメータ $a, b$、露出時間 $t$、Huberノルム $\lVert\cdot\rVert_\gamma$ を伴う。
• 動的測光重み。 $e_{\text{photo}} = \sqrt{E_{\text{photo}}/n_{\text{residuals}}}$ とすると、$e_{\text{photo}} \geq \theta$ のとき重み $W(e_{\text{photo}}) = \lambda \cdot (\theta/e_{\text{photo}})^2$(それ以外は $\lambda$)であり、$\theta = 8$ である。点ごとのHuberノルムとは異なり、これは品質が悪いときに画像全体の重みを下げ、信頼をIMUへ移す。
• IMU因子。 オンマニフォルドのプレインテグレーションにより、共分散 $\widehat{\Sigma}_j$ を伴う状態 $\widehat{\mathbf{s}}_j^I$ が予測される: $$E_{\text{imu}}(\mathbf{s}_i^I, \mathbf{s}_j^I) = \left(\widehat{\mathbf{s}}_j^I \boxminus \mathbf{s}_j^I\right)^T \widehat{\Sigma}_j^{-1} \left(\widehat{\mathbf{s}}_j^I \boxminus \mathbf{s}_j^I\right)$$
• 明示的なスケールと重力。 視覚因子はスケール任意のフレーム $V$ に存在し、IMU因子はメトリックフレーム $I$ に存在する; 状態にはスケール $s$ と回転 $\mathbf{R}_{V\_I}$ が明示的に含まれ、初期化後も主システム内でスケールは継続的に最適化される。
• マージナリゼーション。 古い変数 $\beta$ はシューア補元 $\widehat{\mathbf{H}}_{\alpha\alpha} = \mathbf{H}_{\alpha\alpha} - \mathbf{H}_{\alpha\beta}\mathbf{H}_{\beta\beta}^{-1}\mathbf{H}_{\beta\alpha}$ によって除去され、DSOの非固定ラグ戦略により最大 $N_f = 8$ 個のキーフレームが保持される。
• 遅延マージナリゼーション。 第2の因子グラフが、同じマージナリゼーション順序を遅延 $d = 100$ フレームで再生する(点は依然として即座にマージナリゼーションされるため、各線形化された測光因子はちょうど $N_f$ 個のキーフレームを結ぶ; マルコフブランケット — したがって実行時間、0.44 msと計測される — はメイングラフと同じである)。このグラフは(1)IMU因子を投入する、(2)事前分布を再構築するために再進行させる、(3)FEJ値を再線形化するために用いる、といった用途に使える。
• IMU初期化のためのPGBA。 IMU因子とバイアス因子が遅延グラフに挿入され(少なくとも $d - N_f + 2$ 個の姿勢がこれらを受け取れる、すなわち93個以上のIMU因子)、すべての変数が最適化される。ポーズグラフ最適化とは異なり、完全なBAの確率分布を捉える「オクタナリー(8項)」因子を用いる; 完全なBAとは異なり、測光項を決して再線形化しない — 精度と速度を両立する。最適化されたグラフを再進行させることで、主システムにすべての視覚・慣性情報を含むマージナリゼーション事前分布が渡される。
• 多段階初期化器。 粗いIMU初期化(姿勢固定、単一バイアス; 重力は加速度計の平均から、スケール=1)→ スケールの共分散マージナルに対する閾値判定 → PGBA初期化(より厳しい閾値 $\theta_{\text{reinit}}$ の下でのオプションの再初期化を含む)→ $\max(s, s_{\text{fej}})/\min(s, s_{\text{fej}}) > \theta_s$ となるたびに現在の線形化点で事前分布を再構築するマージナリゼーション置換。

実験結果

EuRoC、TUM-VI、4Seasons(ドローン、ハンドヘルド、車載)で評価; EuRoCの各系列で10回、それ以外は5回、2013年製MacBook Pro(i7 2.3 GHz、GPUなし)上でリアルタイムモードで実行。

• EuRoC: 平均RMSE ATE 0.069 m(単眼、ループ閉じ込みなし) — 当時報告された最良のVIOであり、ステレオ慣性のBasalt(0.072)、VI-DSO(0.089、単眼)、VINS-Mono(0.184)、OKVIS(0.231)を上回る。このデータセットで報告された中で最も低い平均スケール誤差(0.6%)も達成。
• TUM-VI: 平均ドリフト 0.472% に対し、Basalt 0.939%(ステレオ)、VINS-Mono 1.700%; 28系列中16系列で最良(例: outdoors8では2.11 mに対しBasaltは13.53 m)。累積プロットにおいてはORB-SLAM3よりも全体として頑健であるが、一部の系列ではORB-SLAM3のループ閉じ込みが優れている。
• 4Seasons: 単眼スケールが観測不能な長い等速区間があっても、DM-VIOはステレオ慣性のORB-SLAM3やBasaltを依然として上回る。
• 実行時間: 追跡10.34 ms、キーフレーム処理53.67 ms; 遅延マージナリゼーションはキーフレームスレッドにわずか0.44 ms(0.8%)しか追加しない。

SLAMにおける意義

DM-VIOは、直接法と特徴ベースのVIOとのギャップを埋めた: よく設計されたマージナリゼーションと初期化戦略を持つ単眼測光システムは、特徴ベースのステレオ慣性パイプラインを上回ることができる。これはTUM直接法系譜(DSO → VI-DSO → DM-VIO)の集大成であり、マージナリゼーションの整合性問題(FEJ、観測不能なスケール)が実際にどのように現れ、どのように緩和されるかを学ぶためのシステムである。遅延マージナリゼーションは汎用的な道具である: 論文自体が、地図再利用のためのキーフレーム再活性化や、長期的なループ閉じ込みのためのPGBAを提案している。

関連ノート

• DSO — 直接スパースオドメトリの核。
• VI-DSO — 同グループによる先行の直接視覚慣性システム。
• Basalt — マージナリゼーションの線形化誤差に対する別の対処法(非線形因子回復)。
• IMU Preintegration on Manifold — 用いられているIMU因子の定式化。
• Marginalization — 「遅延」させられる仕組みの背景。
• Schur complement / Sparsity — 式(10)の背後にある線形代数の道具。
• Observability — 単眼スケールがなぜ初期に観測不能であり、明示的に処理される必要があるのか。