Schur complement / Sparsity

バンドル調整は一見手に負えないように見える。$m$個のキーフレームと$n$個の点を持つ控えめなマップでも未知数は$6m + 3n$個あり、$n$は数十万に達することも容易にある。これが実用可能なのは問題が**疎(sparse)**であることによるものであり、**Schur補元(Schur complement)**はその疎性を利用するトリックである。

疎性はどこから来るか。 各再投影誤差項は正確に1つのポーズと1つの点のみに関わる。したがって、Gauss-Newton正規方程式$H \Delta\mathbf{x} = -\mathbf{b}$($H = J^T J$)において、ヘッシアンは矢印状のブロック構造を持つ。

$$H = \begin{bmatrix} B & E \\ E^T & C \end{bmatrix}$$

ここで$B$($6m \times 6m$)はポーズ同士のみを結合し、$C$($3n \times 3n$)は点同士のみを結合し、$E$はポーズと点の結合を保持する。重要なのは、2つの点が同じ残差に同時に現れることは決してないため、$C$はブロック対角であることだ。つまり点1つにつき独立した$3 \times 3$ブロックが1つある。

Schur補元のステップ(*点の周辺化(marginalizing out)*とも呼ばれる)は、線形系から点の変数を消去し、縮約されたカメラ系を残す。

$$\left(B - E C^{-1} E^T\right) \Delta\mathbf{x}_{\text{cam}} = -\mathbf{b}_{\text{cam}} + E C^{-1} \mathbf{b}_{\text{pts}}$$

$C$がブロック対角であるため、$C^{-1}$の計算コストはほぼゼロである(各$3 \times 3$ブロックを反転するだけ)。$6m \times 6m$の縮約系をポーズ更新について解き、その後、各点の更新を独立に逆代入して復元する。$(6m + 3n)$次元の求解が$6m$次元の求解になる。$n \gg m$の場合(常にそうである)、これは桁違いに高速である。

実用上さらに2層の疎性が重要となる。第一に、縮約カメラ行列$B - EC^{-1}E^T$自体も疎である。要素$(i, j)$は、キーフレーム$i$と$j$が共通の点を観測している場合(共可視性(covisibility)構造)にのみ非ゼロなので、良い変数順序付け(COLAMD)を伴う疎Cholesky分解が適用できる。第二に、同じ消去の視点は一般化できる。ファクタグラフの言葉で言えば、Schur補元は単なる変数消去であり、スライディングウィンドウVIOや、iSAM2のような増分スムーザにおける周辺化の基盤となる操作である。

この式がどこから来るか

Schur補元は、ブロックGauss消去以上の何ら特別なものではない。正規方程式の2つのブロック行を書き出す。

$$B\,\Delta\mathbf{x}_{\text{cam}} + E\,\Delta\mathbf{x}_{\text{pts}} = -\mathbf{b}_{\text{cam}}$$ $$E^T \Delta\mathbf{x}_{\text{cam}} + C\,\Delta\mathbf{x}_{\text{pts}} = -\mathbf{b}_{\text{pts}}$$

2番目の行を点の更新について解くと、$\Delta\mathbf{x}_{\text{pts}} = -C^{-1}\left(\mathbf{b}_{\text{pts}} + E^T \Delta\mathbf{x}_{\text{cam}}\right)$となり、これを最初の行に代入すると、上記の縮約カメラ系が直ちに現れる。同じ式は、$\Delta\mathbf{x}_{\text{cam}}$が求まった後の逆代入規則そのものでもある。統計的に言えば、縮約系はカメラに関する周辺分布の情報行列表現であり、点を消去することは何かを近似しているのではなく、同一のガウス分布を厳密に再表現しているだけである。

得られる効果を数える。 完全系の密な求解のコストは$O((6m + 3n)^3)$である。Schurのトリックを使うと、$C$の反転は$n$個の独立な$3\times 3$反転($O(n)$)であり、縮約系の構築は観測数によって制限され、残りの求解は最悪でも$O((6m)^3)$である。共可視性による疎性のおかげで、通常はそれよりはるかに小さい。$m$が数百、$n$が数十万という規模では、点の消去がミリ秒単位と分単位の違いを生む。$m$自体が大きくなると(都市規模のSfM)、縮約系自体の分解さえも安価ではなくなり、ソルバーは反復法(Schur補元に対する前処理付き共役勾配法 — CeresのITERATIVE_SCHUR)に切り替える。

Ceresでは、これらすべてが1つの設定選択で済む。

ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_SCHUR;  // or DENSE_SCHUR, ITERATIVE_SCHUR

ソルバーはポーズ/点の消去順序を自動検出する(あるいは明示的なParameterBlockOrderingを受け取る)。Ceres、g2o、GTSAMといった本格的なソルバーはすべて同じトリックを実装している。これを理解することで、なぜBAがスケールするのか、なぜ古い状態を周辺化すると残りのヘッシアンにフィルイン(密なブロック)が生じるのか、なぜソルバーの選択と順序付けが実行時間を桁違いに変えうるのかが説明できる。

よくある落とし穴

• 誤った順序で消去する — このトリックが機能するのは、点が多数あり、計算が安く、互いに独立しているからである。ポーズを先に消去すると、それらに関連するすべての点が結合されて系が密になる。消去順序は性能の正しさを左右する決定である。
• 周辺化と削除の混同 — 古い状態を削除することと周辺化することは異なる。周辺化はその情報を近傍に対する密な事前分布として保持する(フィルイン)。この密度こそが、スライディングウィンドウVIOシステムが何を周辺化し何を破棄するかを慎重に選ぶ理由である。
• ゲージの自由度 — 単眼の場合、完全なBAには観測不可能な7自由度(6 + スケール)がある。ポーズを固定する(または事前分布を追加する)ことなしでは、縮約カメラ行列は特異になり、ソルバーは苦しむか失敗する。
• 遠方点による悪条件化 — ほぼ無限遠の深度を持つ点は、その$3\times3$ブロックがほぼ特異になる。逆深度パラメータ化や深度の事前分布によって$C^{-1}$を良好な状態に保つ。

SLAMにおける意義

リアルタイムSLAMは、構造を利用した線形代数のおかげで存在する。Schur補元なしでは、わずか数百のキーフレームに対するバンドル調整さえ絶望的である。同じアイデア — 変数を消去し、問題を疎に保つ — は周辺化、スライディングウィンドウ推定器、増分スムージングにも再登場するため、これはロードマップ全体の中でも最も応用範囲の広い数学の一つである。

関連ノート

• MAP inference as sparse nonlinear least squares
• Pose graph optimization
• Marginalization
• Math libraries