非線形最適化

SLAMの推定は、最終的に以下のコスト関数を最小化する問題に帰着する

F(\mathbf{x}) = \frac{1}{2}\sum_i \|\mathbf{e}_i(\mathbf{x})\|^2_{\Sigma_i^{-1}}

これは状態 $\mathbf{x}$ (姿勢、ランドマーク、バイアス)についての最小化であり、各 $\mathbf{e}_i$ は残差——再投影誤差、オドメトリ誤差、IMU誤差など——であり、それぞれの逆共分散で重み付けされている。残差は状態に対して非線形である。カメラ投影は深度で除算を行い、回転は曲がったマニフォルド上に存在する。そのため閉形式の解は存在せず、反復計算が必要になる。

反復降下法のテンプレート

実用的なソルバーはすべて同じループを共有している。初期推定 $\mathbf{x}_0$ から出発し、コストを減少させる更新量 $\Delta\mathbf{x}$ を繰り返し求めて適用し、更新量または勾配がごく小さくなった時点で停止する。手法によって $\Delta\mathbf{x}$ の選び方が異なる。

勾配降下法: $\Delta\mathbf{x} = -\alpha\, \nabla F$ 。一次導関数のみを必要とするが、ジグザグに進み収束が遅い(線形収束)——SLAMで直接使われることは稀である。
ニュートン法: 真のヘッシアン $H$ を用いて $H\,\Delta\mathbf{x} = -\nabla F$ を解く。二次収束するが、残差の二次導関数の計算コストが高く、最適解から離れた場所では $H$ が正定値でないことがある。
Gauss-Newton法: 最小二乗の構造を利用する。各残差を線形化し、 $\mathbf{e}(\mathbf{x} + \Delta\mathbf{x}) \approx \mathbf{e}(\mathbf{x}) + J\Delta\mathbf{x}$ とし、正規方程式 $(J^T J)\,\Delta\mathbf{x} = -J^T\mathbf{e}$ を解く。行列 $J^T J$ は一次導関数のみを用いてヘッシアンを近似する——これが大規模SLAMを実用的にする鍵となるトリックである。
Levenberg-Marquardt法: 適応的な減衰項を加えたGauss-Newton法であり、 $(J^T J + \lambda I)\,\Delta\mathbf{x} = -J^T\mathbf{e}$ として、線形化が信頼できない場合には勾配降下法側にブレンドする。事実上の標準手法。
Dogleg/信頼領域法: 明示的に領域半径を保持し、その中でGauss-Newtonステップと勾配ステップを組み合わせる。精神的にはLMと同等であり、バンドル調整においてより速いことが多い。

マニフォルド上の最適化

カメラ姿勢は $\mathrm{SE}(3)$ の要素であり、ベクトルではない——回転行列に更新量を単純に加算すると直交性が破壊される。標準的な対処法は、リー代数における局所的な摂動 $\boldsymbol{\xi} \in \mathbb{R}^6$ を最適化し、指数写像を通じて適用することである。

T \leftarrow T \cdot \exp(\hat{\boldsymbol{\xi}})

ソルバーが目にするのは小さなベクトル $\boldsymbol{\xi}$ だけであり、リトラクション(retraction)によって状態はマニフォルド上に保たれる。すべてのSLAMライブラリがこれを実装している(Ceresのlocal parameterization、g2oの頂点oplus、GTSAMのretraction)。

SLAM問題を解けるようにしている要因

疎性(スパース性)。 各残差が触れる変数はごく少数である(1つの姿勢+1つのランドマーク、あるいは2つの姿勢)ため、 $J^T J$ は極めて疎になる。スパースコレスキー分解とシューア補行列によって、本来なら不可能な規模の問題(数万個の変数)がリアルタイムで解けるようになる。
良好な初期化。 これらの手法は非凸コストの局所最小値しか見つけられない。SLAMがうまくいくのは、常にまともな初期推定——前フレームの姿勢、動きモデル、RANSACによる最小解法の結果——を持っているからである。コールドスタート問題(リローカライゼーション、マップ統合)が難しいのは、まさにこの推定値が欠けているためである。
ロバスト性。 二乗コストはガウス雑音を仮定するが、実際のマッチャーは外れ値を生成する。ロバストカーネル(M推定量)はコストの形を変え、大きな誤差が支配的にならないようにする。その代わりに非凸性が増す。

ソルバー出力の読み方

反復は実用的な基準で停止する。反復ごとのコスト減少が許容値を下回ったとき、更新量ノルム $\|\Delta\mathbf{x}\|$ が無視できるほど小さくなったとき、勾配ノルムがゼロに近づいたとき、あるいは反復回数/時間予算が尽きたとき(リアルタイム系ではキーフレームごとのバンドル調整を数回の反復に制限することが多い)である。解が失敗する場合、症状は原因と対応付けられる。コストが爆発するのは通常、初期推定が悪いかヤコビアンにバグがあることを意味する。小さなステップを何度も繰り返しながらコストが高いまま停滞するのは、悪い局所最小値への収束、あるいはモデル化されていない外れ値を示唆する。正規方程式の行列がランク落ちしているのは、観測不可能な方向(ゲージの自由度、制約のないランドマーク)を指しており、事前分布や固定が必要である。

SLAMにおける意義

バンドル調整、ポーズグラフ最適化、VIOのスライディングウィンドウ、直接測光アライメントなど、現代のあらゆるSLAMシステムのバックエンドは、マニフォルド上でGauss-Newton/LM反復によって解かれる疎な非線形最小二乗問題の一例である。確率論的な視点(MLE & MAP)は何を最小化すべきかを教え、非線形最適化はどのように最小化するかを教える。ここでの習熟度はどこでも役に立つ——論文の「we minimize the following energy」の節を読むこと、発散の診断(初期化が悪いのか、パラメータ化が間違っているのか、外れ値なのか)、Ceres/g2o/GTSAMを効果的に使うこと、これらすべてがこの内容に基づいている。

ハンズオン

Ceres-solverハンズオン