Basic Calculus

SLAMのバックエンドは、その処理時間の大半を非線形コスト関数の最小化に費やしている。これを可能にするのが、微積分学の2つの道具である微分(ヤコビ行列)とテイラー展開(線形化)である。

微分とヤコビ行列

スカラー関数 f:Rn→Rf: \mathbb{R}^n \to \mathbb{R} は勾配 ∇f=[∂f∂x1,…,∂f∂xn]T\nabla f = \left[\frac{\partial f}{\partial x_1}, \ldots, \frac{\partial f}{\partial x_n}\right]^T を持つ。ベクトル関数 f:Rn→Rm\mathbf{f}: \mathbb{R}^n \to \mathbb{R}^m はヤコビ行列を持つ:

J=∂f∂x=[∂f1∂x1⋯∂f1∂xn⋮⋱⋮∂fm∂x1⋯∂fm∂xn]∈Rm×nJ = \frac{\partial \mathbf{f}}{\partial \mathbf{x}} = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \cdots & \frac{\partial f_1}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial f_m}{\partial x_1} & \cdots & \frac{\partial f_m}{\partial x_n} \end{bmatrix} \in \mathbb{R}^{m \times n}

ヤコビ行列は、SLAMの最適化における中心的な道具である。残差 e(x)\mathbf{e}(\mathbf{x})(例えば再投影誤差)が与えられたとき、そのヤコビ行列 J=∂e∂xJ = \frac{\partial \mathbf{e}}{\partial \mathbf{x}} は、状態に対する微小な摂動によって残差がどのように変化するかを示す — これはガウス・ニュートン法やレーベンバーグ・マーカート法がまさに必要としている情報である。

SLAM残差における連鎖律

SLAMの残差はほとんど常に単純な関数の合成であるため、そのヤコビ行列は連鎖律から導かれる。ポーズ TT の下でピクセル z\mathbf{z} で観測されるマップ点 X\mathbf{X} の再投影誤差は

e=z−π(T X)\mathbf{e} = \mathbf{z} - \pi\big(T\,\mathbf{X}\big)

であり、これは(1)剛体変換、(2)透視除算、(3)内部パラメータによる写像の合成である。そのヤコビ行列は積に分解される:

∂e∂x=− ∂π∂Xc⋅∂Xc∂x\frac{\partial \mathbf{e}}{\partial \mathbf{x}} = -\,\frac{\partial \pi}{\partial \mathbf{X}_c}\cdot\frac{\partial \mathbf{X}_c}{\partial \mathbf{x}}

ここで Xc=TX\mathbf{X}_c = T\mathbf{X} はカメラ座標系における点である。各因子を個別に導出して乗じる方が、全体の式を一度に微分するよりもはるかに誤りが少ない — そしてこれは、SLAMライブラリが解析的ヤコビ行列を構成する際に実際に行っている方法である。

テイラー展開

テイラー級数は、滑らかな関数 ff を点 x0x_0 の周りで展開する:

f(x)=f(x0)+f′(x0)(x−x0)+12!f′′(x0)(x−x0)2+⋯f(x) = f(x_0) + f'(x_0)(x - x_0) + \frac{1}{2!}f''(x_0)(x - x_0)^2 + \cdots

多変数関数 f(x)f(\mathbf{x}) を x0\mathbf{x}_0 の周りで展開すると:

f(x)≈f(x0)+J(x0)(x−x0)+12(x−x0)TH(x0)(x−x0)f(\mathbf{x}) \approx f(\mathbf{x}_0) + J(\mathbf{x}_0)(\mathbf{x} - \mathbf{x}_0) + \frac{1}{2}(\mathbf{x} - \mathbf{x}_0)^T H(\mathbf{x}_0)(\mathbf{x} - \mathbf{x}_0)

ここで JJ はヤコビ行列(1次)、HH はヘッセ行列(2次)である。1次で打ち切るとガウス・ニュートン法で使われる線形近似になり、2次で打ち切るとニュートン法で使われる二次近似になる。

テイラー展開からガウス・ニュートン法へ

残差の二乗和 F(x)=12∥e(x)∥2F(\mathbf{x}) = \frac{1}{2}\|\mathbf{e}(\mathbf{x})\|^2 を最小化する問題を考える。現在の推定値の周りで残差を線形化し、e(x+Δx)≈e+JΔx\mathbf{e}(\mathbf{x} + \Delta\mathbf{x}) \approx \mathbf{e} + J\Delta\mathbf{x} を代入すると:

F(x+Δx)≈12∥e∥2+eTJ Δx+12ΔxTJTJ ΔxF(\mathbf{x} + \Delta\mathbf{x}) \approx \frac{1}{2}\|\mathbf{e}\|^2 + \mathbf{e}^T J\,\Delta\mathbf{x} + \frac{1}{2}\Delta\mathbf{x}^T J^T J\,\Delta\mathbf{x}

これは Δx\Delta\mathbf{x} に関する二次関数であり、その導関数をゼロと置くと正規方程式が得られる:

(JTJ) Δx=−JTe(J^T J)\,\Delta\mathbf{x} = -J^T \mathbf{e}

したがってガウス・ニュートン法は「H≈JTJH \approx J^T J としたニュートン法」である — 真のヘッセ行列の二次微分項が省略されるが、残差が小さい場合には良い近似となる。レーベンバーグ・マーカート法は減衰項を加え、(JTJ+λI)Δx=−JTe(J^TJ + \lambda I)\Delta\mathbf{x} = -J^T\mathbf{e} を解くことで、ガウス・ニュートン法(λ→0\lambda \to 0)と勾配降下法(λ\lambda が大きい)の間を補間する。

ヤコビ行列の数値的検証

解析的ヤコビ行列は、符号の誤りや転置ブロックの誤りなど、間違いを起こしやすいことで知られている。標準的な妥当性チェックは中心差分法である。状態の1次元ずつを摂動させ、次を比較する:

J:,k≈e(x+h 1k)−e(x−h 1k)2hJ_{:,k} \approx \frac{\mathbf{e}(\mathbf{x} + h\,\mathbf{1}_k) - \mathbf{e}(\mathbf{x} - h\,\mathbf{1}_k)}{2h}

微小なステップ(例えば h∼10−6h \sim 10^{-6})を用いる。まっとうなSLAMコードベースには、各残差タイプに対してこれを実行する単体テストが必ず存在する。

よくある落とし穴

SLAMにおける意義

バンドル調整では、再投影誤差 e(x+Δx)≈e(x)+JΔx\mathbf{e}(\mathbf{x} + \Delta\mathbf{x}) \approx \mathbf{e}(\mathbf{x}) + J\Delta\mathbf{x} が現在の推定値の周りで線形化される。これにより、非線形最小二乗問題が線形システム (JTJ)Δx=−JTe(J^T J)\Delta\mathbf{x} = -J^T \mathbf{e} の系列に変換され、反復的に解かれる。ポーズグラフ最適化からフルバンドル調整まで、最適化ベースのSLAMシステムはすべてこの「線形化・解・更新」というループの上に構築されている。したがって、ヤコビ行列を手で導出し(そして数値的に検証する)能力は、中核的なスキルである。

関連ノート