ガウス・ニュートン法の実装

ガウス・ニュートン法

前回、最小二乗法メモでガウス・ニュートン法の手続きを紹介しましたが、パラメーター更新を次の$\oplus$（Sola2018 式25）で定義しました。 \[ \mathcal{X} \oplus {}^\mathcal{X}\tau := \mathcal{X} \circ \Exp({}^\mathcal{X}\tau) \] この記事では、具体例としてパラメーターが線形空間の場合と$SO(3)$の場合を考えてPythonで実装してみます。

線形空間の場合

Wikipediaの例を実装してみます。

線形空間を加法的なリー群としてみると、$\Exp(\cdot)$と$\Log(\cdot)$は恒等写像で、$\mathcal{X}^{-1}=-\mathcal{X}$です。つまり、$\oplus$と$\ominus$はそのまま$+$と$-$に読み替えることができます。

$\vect{\Sigma}$は不明なので単位行列としています。なお、実装する上では精度行列$\vect{\Sigma}^{-1}$の形で保持したほうが都合いいのでprecで保持しています。

init, x=[0.9 0.2]
iter=0, x=[0.33266293 0.26017391], tau=[-0.56733707  0.06017391], e=0.007536037691672326
iter=1, x=[0.34280925 0.42607918], tau=[0.01014633 0.16590527], e=0.004229161445442802
iter=2, x=[0.35777522 0.52950844], tau=[0.01496596 0.10342926], e=0.003932162019297914
iter=3, x=[0.36140546 0.5536581 ], tau=[0.00363024 0.02414967], e=0.0039220913053993915
iter=4, x=[0.36180308 0.55607253], tau=[0.00039762 0.00241443], e=0.003922003358180948

$SO(3)$の場合

前節を踏まえて$\mathcal{X} \in \mathcal{M} = SO(3)$の場合を考えてみます。線形空間との主な違いは、$\oplus$と$\ominus$を群に合わせて具体的に実装する必要があることです。また、$SO(3)$の接ベクトル空間の次元は3なので、$\vect{J}$の列数は3となることに注意します。

例としてランダムな点群を適当に決めた$\mathcal{X}_\textrm{gt} \in SO(3)$で変換し、その回転を求めてみます¹。簡単にするため、ノイズフリーのデータを入力し重みなし最小二乗法で解いています。

ガウス・ニュートン法と直接関係ない実装上の話ですが、$SO(3)$はscipy.spatial.transform.Rotationを使うと簡単に実装できます。$\operatorname{Exp}(\cdot)$と$\operatorname{Log}(\cdot)$はそれぞれRotation.from_rotvec()とRotation.as_rotvec()に対応しています。

init, Log(x)=[0. 0. 0.]
iter=0, Log(x)=[ 0.96595154 -0.33356813  0.07520014], tau=[ 0.96595154 -0.33356813  0.07520014], e=0.7546790260975119
iter=1, Log(x)=[ 1.53473948 -0.49624628  0.92601492], tau=[0.74362745 0.14536241 0.6672294 ], e=0.1527747875392677
iter=2, Log(x)=[ 1.21493633 -0.58191099  1.91868745], tau=[0.20468383 0.52713932 0.71491899], e=0.0201408861179663
iter=3, Log(x)=[ 1.05447562 -0.43427591  2.13450218], tau=[0.10381561 0.20495756 0.08216877], e=0.00021503147598789344
iter=4, Log(x)=[ 1.06345711 -0.42489307  2.13781984], tau=[ 0.0105031 -0.0027988  0.0001136], e=4.3477577342256835e-10
Log(x_gt)=[ 1.06346701 -0.42490731  2.13782281]

$\mathcal{X}$を直接表示できないので代わりに$\Log(\mathcal{X})$を表示しています。 イテレーションごとに$E$が減少し、更新した$\mathcal{X}$が$\mathcal{X_\textrm{gt}}$にほとんど一致するのが確認できます。

実装上の注意

ここでは説明のためかなり簡易的な実装にしましたが、実用上は注意するべき点があります。

• $\vect{J}^T \vect\Sigma^{-1} \vect{J}$が正定値になるとは限りません（実用上は半正定値になることがよくあります）
• $E$が毎回減るとは限らないので、レベンバーグ・マーカート法を使ったり、パラメーター更新後に$E$を再評価して増加した場合は打ち切る処理を入れる必要があります
• 正規方程式を解くときに様々な問題があるので逆行列を陽に求めてはいけません（直接的に解く場合は特異値分解やQR分解などの行列分解を使い、反復的に解く場合は共役勾配法などを使います）
• 正規方程式を解くときに悪条件（ill-conditioned）になる場合があるので、数値的に不安定にならないように実装上のテクニックが必要になります²
• 更新ステップの大きさや$E$の変化の大きさなどをチェックし、収束したと判断できる場合は打ち切って無駄なイテレーションを避ける必要があります