周辺化

リアルタイムのSLAM推定器は、過去のすべての姿勢とランドマークをアクティブな変数として保持することはできない — そうすると問題が無限に大きくなってしまう。周辺化(Marginalization) は古い変数を除去する確率論的に正しい方法である:単純に削除する(それらが持つ情報を捨ててしまう)のではなく、同時分布から積分消去し、除去された変数が寄与した情報を要約する事前分布を残りの変数に残す。

代数:シュア補元

線形化されたシステムを、保持する変数( $\mathbf{k}$ )と周辺化する変数( $\mathbf{m}$ )に分割する。

\begin{bmatrix} H_{kk} & H_{km} \\ H_{mk} & H_{mm} \end{bmatrix} \begin{bmatrix} \Delta\mathbf{x}_k \\ \Delta\mathbf{x}_m \end{bmatrix} = \begin{bmatrix} \mathbf{b}_k \\ \mathbf{b}_m \end{bmatrix}

$\mathbf{m}$ を消去すると縮約系が得られる。

H_{\text{prior}} = H_{kk} - H_{km} H_{mm}^{-1} H_{mk}, \qquad \mathbf{b}_{\text{prior}} = \mathbf{b}_k - H_{km} H_{mm}^{-1} \mathbf{b}_m

$(H_{\text{prior}}, \mathbf{b}_{\text{prior}})$ は、その後のすべての最適化において保持された変数に対する密な事前ファクターとして働く。

これはどこから来るのか。情報形式(情報行列 $H$ 、情報ベクトル $\mathbf{b}$ )で書かれた同時ガウス分布に対して、変数の部分集合を積分消去すると、その情報行列がまさに上記のシュア補元であるもう一つのガウス分布が得られる。つまり周辺化は、疎なシステムを解く際の変数消去の一ステップと同じ代数演算である — 違いは意図だけである:消去は一時的(その変数は後退代入で戻ってくる)であり、周辺化は永続的である。

コスト:充填(fill-in)

情報は保存されるが、疎性は保存されない — 周辺化は、除去された変数が接していた変数間の結合を密にする。小さな例を挙げる。窓 $x_0, x_1, x_2, x_3$ において、ランドマーク観測が $x_0$ を、 $x_1$ と $x_2$ からも見られている複数のランドマークに結びつけている場合、 $x_0$ とそれらのランドマークを周辺化すると、 $x_1$ と $x_2$ (および $x_0$ が接していたその他すべて)を結合する一つの密な事前分布が生成される — 以前は直接のエッジを持たなかった対である。これが、システムが任意の変数ではなく慎重に選ばれた変数(古いキーフレーム、それらが排他的に観測するランドマーク)を周辺化する理由であり、また一部のシステムが周辺化の前に意図的にいくつかの観測を捨てる理由でもある(OKVIS流の疎化)— 窓全体の事前分布が密になるのを防ぐために少量の情報を犠牲にするのである。

固定ラグ平滑化

固定ラグ平滑化(Fixed-lag smoothing) は、この操作を基盤に構築された推定器アーキテクチャである:最新の状態のスライディングウィンドウをアクティブな非線形変数として保持し、窓が満たされたら最も古い状態を事前分布へ周辺化する。これにより、(過去のすべての情報の線形化された要約を保持しながら)フレームあたりの計算量を制限できる — OKVIS以降の最適化ベースVIOの標準的な設計である。例えばVINS-Monoは、入力フレームがキーフレームかどうかによって、最も古いフレームか二番目に新しいフレームのいずれかを周辺化する。

落とし穴:整合性

事前分布は周辺化時点で成り立っていた推定値において線形化されており、後で再線形化することはできない(周辺化された変数はもう存在しない)。残りの変数の線形化点が変化していく一方で凍結された事前分布がそのままである場合、推定器は異なる線形化点からのヤコビアンを混在させ、見せかけの情報を得てしまう — いわゆる「FEJ問題」である。標準的な緩和策は次の通り。

First-Estimate Jacobians (FEJ) — 事前分布に接続された変数のヤコビアンをその最初の推定値で評価し、すべての情報が整合した方向に沿って入るようにする。
平方根 / コレスキー形式の事前分布(Basaltのように)による数値的安定性の確保。
遅延周辺化(Delayed marginalization、DM-VIO) — 凍結を延期し、グラフのより多くの部分を再線形化可能な状態に保つ。
非線形ファクター復元(Basalt) — 凍結された線形の事前分布を、それを近似する復元された非線形ファクター群に置き換える。

実務チェックリスト

事前分布に吸収された観測を再利用しないこと — それは情報を二重に数えてしまう。
離脱するキーフレームによって排他的に観測されるランドマークはそのキーフレームとともに周辺化する。まだ見えているランドマークについては、古い観測を捨てるか疎化によって処理するべきである。
事前分布の条件数に注意する:条件が悪い $H_{mm}$ は $H_{mm}^{-1}$ を爆発させる。平方根形式と慎重な変数スケーリングが役立つ。
NEES形式の整合性チェックが長時間の実行後にのみ失敗する場合、まず疑うべきは周辺化/FEJのパイプラインである。

対照的なバックエンド方式として、増分平滑化(iSAM2)はすべての変数を保持し巧妙に更新することで、凍結された事前分布の問題を回避する代わりに(効率的に維持されるとしても)問題は大きくなり続ける。固定ラグ+周辺化は厳密に制限されたコストを得る — 組み込みVIOにとって正しいトレードオフである。

SLAMにおける意義

周辺化は、リアルタイムかつメモリ制限のあるビジュアル・インナーシャルオドメトリを可能にする主力技術であり、スライディングウィンドウ + シュア補元の事前分布 + FEJのパターンは、OKVIS、VINS-Mono、Basalt、DM-VIOの文字通りのアーキテクチャである。また実践において理論が最も厳しく問われる部分でもある:誤って処理された周辺化は、過信的で整合性のない推定器の典型的な原因であり、そのためこの概念は整合性と可観測性に密接に結びついている。

周辺化