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

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

ガウス・ニュートン法

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

線形空間の場合

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

線形空間を加法的なリー群としてみると、Exp()Log()は恒等写像で、X1=Xです。つまり、はそのまま+に読み替えることができます。

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

import numpy as np
N = np.ndarray
TangentN = np.ndarray
M = np.ndarray
TangentM = np.ndarray
def ominus_n(lhs: N, rhs: N) -> TangentN:
return np.asarray(lhs - rhs)
def oplus_m(lhs: M, rhs: TangentM) -> M:
return np.asarray(lhs + rhs)
# from https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm#Example
s = np.array([0.038, 0.194, 0.425, 0.626, 1.253, 2.500, 3.740])
z_bar = np.array([0.050, 0.127, 0.094, 0.2122, 0.2729, 0.2665, 0.3317])
prec = np.eye(len(z_bar))
def f(x: M) -> N:
return np.asarray(x[0] * s / (x[1] + s))
def e(x: M) -> float:
eps = ominus_n(z_bar, f(x))
return float(0.5 * eps.T @ prec @ eps)
def jacobian(x: M) -> np.ndarray:
ret = np.array(
[
-s / (x[1] + s),
x[0] * s / (x[1] + s) ** 2,
]
).T
n = len(s)
m = len(x)
assert ret.shape == (n, m)
return ret
x = np.array([0.9, 0.2])
print(f"init, x={x}")
for k in range(5):
eps = ominus_n(z_bar, f(x))
j = jacobian(x)
tau = np.linalg.lstsq(j.T @ prec @ j, -j.T @ prec @ eps, rcond=None)[0]
x = oplus_m(x, tau)
print(f"iter={k}, x={x}, tau={tau}, e={e(x)}")
view raw linear.py delivered with ❤ by emgithub
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)の場合

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

例としてランダムな点群を適当に決めたXgtSO(3)で変換し、その回転を求めてみます1。簡単にするため、ノイズフリーのデータを入力し重みなし最小二乗法で解いています。

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

from typing import Type
import numpy as np
from scipy.spatial.transform import Rotation
N = np.ndarray
TangentN = np.ndarray
M = Type[Rotation]
TangentM = np.ndarray
def ominus_n(lhs: N, rhs: N) -> TangentN:
return np.asarray(lhs - rhs).reshape(-1)
def oplus_m(lhs: Rotation, rhs: TangentM) -> Rotation:
return lhs * Rotation.from_rotvec(rhs)
np.random.seed(0)
s = np.random.random((3, 3))
x_gt: M = Rotation.random()
z_bar: N = x_gt.apply(s)
prec = np.eye(s.size)
def f(x: M) -> N:
return np.asarray(x.apply(s))
def e(x: M) -> float:
eps = ominus_n(z_bar, f(x))
return float(0.5 * eps.T @ prec @ eps)
def jacobian(x: M) -> np.ndarray:
ret = []
for i in s:
ret.append(
x.as_matrix()
@ np.array([[0, -i[2], i[1]], [i[2], 0, -i[0]], [-i[1], i[0], 0]]),
)
return np.vstack(ret)
x = Rotation.identity()
print(f"init, Log(x)={x.as_rotvec()}")
for k in range(5):
eps = ominus_n(z_bar, f(x))
j = jacobian(x)
tau = np.linalg.lstsq(j.T @ prec @ j, -j.T @ prec @ eps, rcond=None)[0]
x = oplus_m(x, tau)
print(f"iter={k}, Log(x)={x.as_rotvec()}, tau={tau}, e={e(x)}")
print(f"Log(x_gt)={x_gt.as_rotvec()}")
view raw so3.py delivered with ❤ by emgithub
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]

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

実装上の注意

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

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

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

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