MAP推定と疎な非線形最小二乗問題

これは現代のSLAMにおける中心的な理論的な同一性である。ガウス雑音の下でのファクターグラフに対する最大事後確率(MAP)推定は、まさに疎な非線形最小二乗問題そのものである。 バックエンドが行うすべての処理は、この一つの導出から導かれる。

ベイズ則から最小二乗へ

ベイズ則から始める。観測値が与えられたときに最も確率の高い状態を求めたい。

x∗=arg⁡max⁡x p(x∣z)∝p(z∣x) p(x)\mathbf{x}^* = \arg\max_{\mathbf{x}}\, p(\mathbf{x} \mid \mathbf{z}) \propto p(\mathbf{z} \mid \mathbf{x})\, p(\mathbf{x})

運動モデル xt=f(xt−1,ut)+wt\mathbf{x}_{t} = f(\mathbf{x}_{t-1}, \mathbf{u}_t) + \mathbf{w}_t、wt∼N(0,Qt)\mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, Q_t) と観測モデル zt=h(xt,m)+vt\mathbf{z}_t = h(\mathbf{x}_t, \mathbf{m}) + \mathbf{v}_t、vt∼N(0,Rt)\mathbf{v}_t \sim \mathcal{N}(\mathbf{0}, R_t) を用いて、ガウス分布の積の負の対数を取ると、確率の最大化が共分散で重み付けされた二乗残差の和の最小化に変わる。

x∗=arg⁡min⁡x[∑t∥h(xt)−zt∥Rt−12+∑t∥f(xt−1,ut)−xt∥Qt−12]\mathbf{x}^* = \arg\min_{\mathbf{x}} \left[ \sum_t \|h(\mathbf{x}_t) - \mathbf{z}_t\|^2_{R_t^{-1}} + \sum_t \|f(\mathbf{x}_{t-1}, \mathbf{u}_t) - \mathbf{x}_t\|^2_{Q_t^{-1}} \right]

ファクターグラフの各ファクターが一つの項を提供する。ビジュアルSLAMでは観測項は再投影誤差であり、この問題はバンドル調整に特化する。有用な整理として、共分散の重み付けは残差に吸収できる。∥r∥Σ−12=∥Σ−1/2r∥2\|\mathbf{r}\|^2_{\Sigma^{-1}} = \|\Sigma^{-1/2}\mathbf{r}\|^2(「白色化」)であるため、すべての重み付き問題は白色化された残差に対する通常の最小二乗問題になる。

解法:ガウス・ニュートン法とレーベンバーグ・マーカート法

この問題は(投影や回転のため)非線形であり、反復的に解かれる。ガウス・ニュートン法は、スタックした残差 e\mathbf{e} を現在の推定値 xk\mathbf{x}_k の周りで線形化する。

e(xk+Δx)≈e(xk)+JkΔx\mathbf{e}(\mathbf{x}_k + \Delta\mathbf{x}) \approx \mathbf{e}(\mathbf{x}_k) + J_k \Delta\mathbf{x}

これをコストに代入し Δx\Delta\mathbf{x} について最小化すると、正規方程式が得られる。

(JkTJk) Δx=−JkTe(xk)(J_k^T J_k)\, \Delta\mathbf{x} = -J_k^T \mathbf{e}(\mathbf{x}_k)

行列 H=JTJH = J^T J はヘッシアンを近似する(二次項を無視する)。ガウス・ニュートン法は解の近傍では速く収束するが、初期推定が悪いと発散する可能性がある。レーベンバーグ・マーカート法は減衰項によってこれを解決する。

(JkTJk+λI) Δx=−JkTe(xk)(J_k^T J_k + \lambda I)\, \Delta\mathbf{x} = -J_k^T \mathbf{e}(\mathbf{x}_k)

λ→0\lambda \to 0 ではガウス・ニュートン法のように振る舞う(最小値付近で高速)。λ→∞\lambda \to \infty では小さな最急降下ステップになる(遠方でロバスト)。λ\lambda は反復ごとに調整される — ステップがコストを減少させれば減少し、そうでなければ増加する。LMはCeresや大半のSLAMバックエンドにおける標準的なアルゴリズムである。

Lie群のノートに示されている多様体の仕組みを取り入れた完全な反復ループは以下の通り。

  1. 現在の推定値ですべての残差を線形化する(接空間の摂動 ξ\boldsymbol{\xi} に関するヤコビアン)。
  2. 疎な(減衰付き)正規方程式を解いて Δx\Delta\mathbf{x} を求める。
  3. 多様体上で更新する:各姿勢について T←T⋅exp⁡(ξ^)T \leftarrow T \cdot \exp(\hat{\boldsymbol{\xi}})、ユークリッド変数については単純な加算。
  4. コストまたは更新ノルムが収束するまで繰り返す。

ロバストカーネル。 実際の対応点集合には外れ値が含まれ、一つの悪い二乗残差が和全体を支配してしまうことがある。実用的なシステムは各項をロバスト損失 ρ\rho で包む(Huber:小さな残差では二次関数、閾値を超えると線形;Cauchy:対数的な増大により大きな外れ値を強く重み下げする)。この最適化は反復重み付き最小二乗として解かれる — 各残差は重み wi=ρ′(ri)/riw_i = \rho'(r_i)/r_i を得る — これはまさに同じ正規方程式の仕組みに当てはまる。

疎性:もう半分の話

各ファクターはごく少数の変数にしか関わらない(一つの観測は一つの姿勢と一つのランドマークだけに関わる)ため、JJ と HH は圧倒的に疎でブロック構造を持つ。これを利用することがSLAMを大規模にも扱いやすくしている要因であり、疎コレスキー分解/QR分解、およびバンドル調整においてすべてのランドマークを先に消去するシュア補元を通じて実現される((6m+3n)(6m+3n) 次元のシステムを 6m6m 個のカメラ変数に縮約し、ランドマークが姿勢数を大きく上回る場合には非常に有効である)。

変数消去とベイズ木。 疎なシステムを解くことはグラフ理論的に見ることができる:ファクターグラフから変数を一つずつ消去し、各消去は残る変数に対する条件付き密度と新たな誘導ファクターを生成する。消去の順序がどれだけの充填(密度の増加)を生じるかを決める — 良い順序付け(例:COLAMD)は分解を疎に保つ。消去を完了まで行うとベイズネットが得られ、そのクリークはベイズ木にまとめられる:各変数の解がその祖先にのみ依存する、クリークの有向木である。ベイズ木は単なる実装上の詳細ではなく、新しい観測が解のどの部分に影響を与えるかを明らかにする。これはまさにiSAM2が増分更新のために利用する構造であり、変数の消去(永続的に行う)としての周辺化を説明するものでもある。

この基盤の上に二つの解法スケジュールの系統が構築される。

SLAMにおける意義

この定式化は、この分野がフィルタリングから最適化(「平滑化」)へ移行した理由である。必要な箇所で再線形化を行い、任意の種類のファクターを均一に扱い、疎性を通じてスケールする。あらゆるバックエンドライブラリ(Ceres、g2o、GTSAM)はまさにこのパイプラインの実装であり、あなたが読むすべてのバックエンドの論文 — ORB-SLAMのBAからVINS-Monoのスライディングウィンドウ、iSAM2まで — は「どの変数、どのファクター、どの消去/解法スケジュールか」という問いに対する特定の答えである。

関連ノート