Filter-based vs Optimization-based

密結合VIOの中には2つの推定パラダイムが存在する。両者は同じ確率的推論問題を解いているが、どのくらいの頻度で再線形化を行うかが異なる。

フィルタベース(EKF)

フィルタベースの手法(MSCKF, ROVIO, OpenVINS)は、状態ベクトル $\mathbf{x}$ と共分散 $\mathbf{P}$ を再帰的に更新しながら保持する:

• 予測(propagate): IMU計測値を用いて $\mathbf{x} \leftarrow f(\mathbf{x}, \tilde{\boldsymbol{\omega}}, \tilde{\mathbf{a}})$、$\;\mathbf{P} \leftarrow \boldsymbol{\Phi}\mathbf{P}\boldsymbol{\Phi}^\top + \mathbf{Q}$。
• 更新(update): 視覚計測値を用いて、残差 $\mathbf{r} = \mathbf{z} - h(\mathbf{x})$ とヤコビアン $\mathbf{H}$ から、カルマンゲイン $\mathbf{K} = \mathbf{P}\mathbf{H}^\top(\mathbf{H}\mathbf{P}\mathbf{H}^\top + \mathbf{R})^{-1}$ を適用し、計測値をガウス分布の信念に取り込む。

各計測値は処理される瞬間に一度だけ線形化される。これにより一定時間かつ低遅延の更新が得られる — しかし線形化点は永続的である: 後で推定値が移動しても、すでに取り込まれた情報を再線形化することはできず、累積した線形化誤差はドリフトと(注意しないと)不整合を引き起こす。MSCKFの着眼点は、EKFをウィンドウ状にすること(過去のカメラ姿勢のスライディング集合を状態に保持し、特徴制約を構造レスに適用すること)であった — つまり「フィルタリング」であるVIOも実は密かにウィンドウ化されている。

最適化ベース(スライディングウィンドウBA)

最適化ベースの手法(OKVIS, VINS-Mono, Basalt, ORB-SLAM3)は、直近のキーフレーム状態のスライディングウィンドウを保持し、再投影残差・プレインテグレーションされたIMU残差・マージナリゼーション事前分布からなる結合非線形コストを、Gauss-NewtonまたはLevenberg-Marquardt法で最小化する:

$$\min_{\mathcal{X}} \; \|\mathbf{r}_p\|^2 + \sum \|\mathbf{r}_{\text{IMU}}\|^2_{\Sigma^{-1}} + \sum \rho\left(\|\mathbf{r}_{\text{reproj}}\|^2_{R^{-1}}\right)$$

ソルバーの各イテレーションがウィンドウ内のすべての残差を現在の推定値で再線形化するため、線形化誤差はかなり小さく、それに応じて精度も高くなる — その代償として計算コストは高くなる。古い状態はシューア補元によるマージナリゼーションによって事前分布 $\mathbf{r}_p$ に圧縮される。これが両パラダイムが接する点である(下記参照)。

比較

この2系統は見た目より近い

• シューア補元によって古い状態をマージナリゼーションするスライディングウィンドウ最適化器は、数学的にはマージナリゼーションされた部分に対してフィルタリングステップを実行しているのと同じである — それが作る事前分布は一度線形化されて凍結される点で、EKFの更新と完全に同じである。
• 逆に、EKFはGauss-Newtonの1イテレーションで止まる最適化器である; (ROVIOの更新ステップで用いられる)反復EKFは更新の中で再線形化を行い、最適化側へさらに近づく。
• 両系統は、凍結された線形化点において同じ病 — 観測不能な方向に沿った見せかけの情報増加 — を患い、両者とも同じ薬(FEJおよびその類似手法)を用いる。MSCKFから発展した整合性の文献は、あらゆるスライディングウィンドウシステムのマージナリゼーション事前分布に適用できる。

したがって実務上の違いは「再帰的かバッチ的か」ではなく、各設計が計算予算をどこに費やすかである: フィルタは再線形化にほとんど計算を割かず、そのすべてをスループットに費やす; 最適化器は、小さく適切に選ばれたウィンドウを再線形化することで精度を買う。

実務における選択

• ハードリアルタイム、組み込み計算資源、高レート制御ループ → フィルタ(MSCKF/OpenVINSクラス)。一定で予測可能なフレームあたりコスト; この効率特性こそが、フィルタ型推定器がデプロイされたAR/VRトラッキングと結び付けられる理由である。
• 精度が重要なマッピング、オフラインまたはデスクトップ級の計算資源 → スライディングウィンドウ最適化、あるいは最適化とマッピングバックエンドの組み合わせ(Basalt, OKVIS2)。
• 両方の失敗モードを学ぶ: フィルタの不整合は過信した共分散とヨードリフトとして現れる; 最適化器のレイテンシは負荷時のフレーム落ちとして現れる。パッチ的な文献 — FEJ、観測可能性制約付きEKF、非線形因子回復、遅延マージナリゼーション — はまさにこれらに対処するために存在する。

よくある落とし穴

• 異なる計算予算でフィルタと最適化器を比較し、一方のパラダイムが「優れている」と結論づけること — 古典的な「なぜフィルタなのか?」分析(Strasdatら)は固定予算の議論である: 小さなキーフレームウィンドウを再線形化する方が、大きな状態をフィルタリングするより優れている。
• スライディングウィンドウシステムのマージナリゼーション事前分布を厳密なものとして扱うこと — それは一度限りの線形化であり、これを無視すること(FEJ処理を行わないこと)は、「最適化ベース」のシステムにフィルタ型の不整合を再び持ち込んでしまう。
• フィルタは精度が出せないと仮定すること: FEJ、オンラインキャリブレーション、良好なフロントエンドを備えた現代のMSCKF実装は、標準ベンチマークにおいてコストのごく一部で競争力を持つ。

SLAMにおける意義

これは、視覚SLAMについて研究された古典的な「なぜフィルタなのか?」という問い(Strasdatら)のVIO版である: 固定された計算予算の下では、キーフレームの小さなウィンドウを再線形化する方が、大きな状態をフィルタリングするより優れている。それぞれの側の失敗モード — フィルタの不整合 vs 最適化器のレイテンシ — を知ることで、あるプラットフォームにどのシステムを選ぶべきか、そしてそれらの失敗モードを修正するためにどの論文(FEJ、非線形因子回復、遅延マージナリゼーション)が存在するかが分かる。

関連ノート

• Visual-SLAM why filter? — このトレードオフに関する基礎的な分析。
• MSCKF — 代表的なフィルタベースVIO。
• OKVIS — 代表的な最適化ベースVIO。
• Basalt — マージナリゼーション事前分布の線形化誤差を修正する非線形因子回復。
• Marginalization — 両世界を結びつける仕組み。
• Consistency — この選択で問われる推定器の性質。