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

2021-05-06 (最終更新: 2022-01-23)

ガウス・ニュートン法

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

線形空間の場合

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

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

$Σ$ は不明なので単位行列としています。なお、実装する上では精度行列 $Σ^{- 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,
    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

$S O (3)$ の場合

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

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

ガウス・ニュートン法と直接関係ない実装上の話ですが、 $S O (3)$ はscipy.spatial.transform.Rotationを使うと簡単に実装できます。 $Exp (\cdot)$ と $Log (\cdot)$ はそれぞれ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$ が減少し、更新した $X$ が $X_{gt}$ にほとんど一致するのが確認できます。

実装上の注意

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

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

ここでは説明のためにガウス・ニュートン法を使っていますが、最小二乗法メモにあるようにこの問題の場合もっとよい解法があります。 ↩︎
例えば、ランク落ちしている場合はコレスキー分解を使用できません。実装例で用いているnumpy.linalg.lstsqはしきい値より小さい特異値を0に置き換えて条件数を改善するテクニックが使用されています。 ↩︎

	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)}")

	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()}")

ガウス・ニュートン法

線形空間の場合

SO(3)の場合

実装上の注意

$S O (3)$ の場合