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

数学
ガウス・ニュートン法の実装
Page content

ガウス・ニュートン法

前回、最小二乗法メモでガウス・ニュートン法の手続きを紹介しましたが、パラメーター更新を次の$\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)$で変換し、その回転を求めてみます1。簡単にするため、ノイズフリーのデータを入力し重みなし最小二乗法で解いています。

ガウス・ニュートン法と直接関係ない実装上の話ですが、$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$を再評価して増加した場合は打ち切る処理を入れる必要があります
  • 正規方程式を解くときに様々な問題があるので逆行列を陽に求めてはいけません（行列分解を使います）
  • 正規方程式を解くときに悪条件（ill-conditioned）になる場合があるので、数値的に不安定にならないように実装上のテクニックが必要になります2
  • 更新ステップの大きさや$E$の変化の大きさなどをチェックし、収束したと判断できる場合は打ち切って無駄なイテレーションを避ける必要があります

  1. ここでは説明のためにガウス・ニュートン法を使っていますが、最小二乗法メモにあるようにこの問題の場合もっとよい解法があります。 ↩︎

  2. 例えば、ランク落ちしている場合はコレスキー分解を使用できません。実装例で用いているnumpy.linalg.lstsqはしきい値より小さい特異値を0に置き換えて条件数を改善するテクニックが使用されています。 ↩︎