Expmapを使った姿勢最適化（ICP編）

前回Expmapがなぜ最適化に使われるのかを書いたが，今回は実際にどのようにExpmapを使って姿勢の最適化ができるのか書いてみようと思う．Expmapの微分の導出などに関しては[1] http://ethaneade.com/lie.pdf を参照してほしい．

単純な例としてまずICPを考えよう．ICPでは点 $x_i$ とその最近傍点 $x_i'$ の距離を最小化する相対姿勢 $T$ を求める．つまり，ある点 $x_i$ に対する誤差関数は $e_i = T x_i - x_i'$ となり，点群全体における誤差その二乗和で与えられる．この誤差関数を最小化する相対姿勢 $T$ は実際にはクローズドフォームで求めることができるが，ここではあえてExpmapを使いガウスニュートンで求めてみる．

ガウスニュートンを使うには微分，ここでは $\partial T x / \partial T$ が必要になる．Expmapにおいてはある点 $x_i$ に作用する変換 $T$ のヤコビアンを求めるには，本当はジェネレータと呼ばれる変換の基底みたいな行列が出てくるが，ここでは脳死で[1]の(94)式だけ見ておけば良い．姿勢がゼロ近傍( $T=0$ )にあるとき $\partial T x / \partial T = [ I | x_{\times} ]$ で与えられる． $x_{\times}$ はskew symmetric matrixと呼ばれ以下のように与えられる．

このようにExpmapによる微分は簡単に得られるが，注意する必要があるのはこれは姿勢 $T$ がゼロ近傍にあるときだけということだ．任意の姿勢に対する微分を求めようとすると問題は一気に難しくなる．幸い，多くの場合には点を現在の推定姿勢 $\tilde{T}$ で変換した $\tilde{x_i} = \tilde{T} x_i$ を考え， $e_i = T \tilde{x_i} - x_i'$ (ただし $T = 0$ )とすることで，ゼロ点周りの微分のみで等価に問題を解くことができる．

これである点 $x_i$ に対する姿勢 $T$ のヤコビアン $J_i \in \mathcal{R}^{3\times6}$ が得られるため，あとは全ての点分これをスタックしたもの $J = [J_0^T J_1^T, \cdots, J_N^t]^T \in \mathcal{R}^{3N \times 6}$ と，誤差ベクトルをスタックしたもの $E = [e_0^T e_1^T, \cdots, e_N^T]^T \in \mathcal{R}^{3N}$ をもとにガウスニュートンなどで姿勢の変分量を求めれば良い( $H = J^T J, b = J^T e, H \delta T = b$ を解く)．そして得られた変分量ベクトル $\delta T \in \mathcal{R}^{6}$ にExpmapを適用して姿勢に変換した後，現在の姿勢に適用することで推定の更新を行う( $T_{new} = \exp( \delta T ) T$ )．

ここまでに書いた基本的な姿勢最適化の流れをほぼそのまま実装してみると以下のようなコードになる．

#!/usr/bin/python3
import numpy
import sophus
import open3d
import sklearn.neighbors
from matplotlib import pyplot


def main():
	target = numpy.load('scan1.npy')
	source = numpy.load('scan2.npy')

	kdtree = sklearn.neighbors.NearestNeighbors(n_neighbors=1, algorithm='kd_tree').fit(target)
	x = sophus.SE3()

	sum_errors = []
	for i in range(20):
		transed_source = x * source

		dists, indices = kdtree.kneighbors(transed_source)
		errors = transed_source - target[indices].reshape(-1, 3)

		J = numpy.zeros((errors.shape[0], 3, 6))
		J[:, :3, :3] = numpy.identity(3)

		skew = numpy.zeros((errors.shape[0], 3, 3))
		skew[:, 0, 1] = -transed_source[:, 2]
		skew[:, 0, 2] = transed_source[:, 1]
		skew[:, 1, 0] = transed_source[:, 2]
		skew[:, 1, 2] = -transed_source[:, 0]
		skew[:, 2, 0] = -transed_source[:, 1]
		skew[:, 2, 1] = transed_source[:, 0]
		J[:, :3, 3:] = -skew

		errors = errors.reshape(-1, 1)
		J = J.reshape(-1, 6)

		H = J.T.dot(J)
		b = J.T.dot(errors)
		delta = numpy.linalg.solve(H, -b)

		x = sophus.SE3.exp(delta) * x

		print('--- estimated ---')
		print(x.matrix())

		sum_errors.append(numpy.linalg.norm(errors, ord=2))
		print('sum_error:', sum_errors[-1])

	pyplot.plot(sum_errors)
	pyplot.show()


def validation():
	target = open3d.geometry.PointCloud()
	target.points = open3d.utility.Vector3dVector(numpy.load('scan1.npy'))
	source = open3d.geometry.PointCloud()
	source.points = open3d.utility.Vector3dVector(numpy.load('scan2.npy'))

	reg = open3d.pipelines.registration.registration_icp(source, target, 1e9)

	print('--- open3d ---')
	print(reg.transformation)


if __name__ == '__main__':
	main()
	validation()

scan1.npy & scan2.npy : https://tenteroring.org/tenteroring/data/scans.tar.gz

重要な点だけ抜き出して説明する．xは現在の推定姿勢を表す．

        transed_source = x * source
 
        dists, indices = kdtree.kneighbors(transed_source)
        errors = transed_source - target[indices].reshape(-1, 3)

まずは点群を現在の推定姿勢で変換し，k=1で最近傍対応点を求める．前述の説明でも出たとおり微分計算時は姿勢はゼロとして考える必要があるため，この変換した点群transed_sourceを使ってヤコビアンの算出を行っていく．

		J = numpy.zeros((errors.shape[0], 3, 6))
		J[:, :3, :3] = numpy.identity(3)

		skew = numpy.zeros((errors.shape[0], 3, 3))
		skew[:, 0, 1] = -transed_source[:, 2]
		skew[:, 0, 2] = transed_source[:, 1]
		skew[:, 1, 0] = transed_source[:, 2]
		skew[:, 1, 2] = -transed_source[:, 0]
		skew[:, 2, 0] = -transed_source[:, 1]
		skew[:, 2, 1] = transed_source[:, 0]
		J[:, :3, 3:] = -skew

ヤコビアンは基本的に前述したとおりの $\partial T x / \partial T = [I | x_{\times}]$ で与えられるので，変換済みのtransed_sourceを使って各点のヤコビアンを求める．

        H = J.T.dot(J)
        b = J.T.dot(errors)
        delta = numpy.linalg.solve(H, -b)
 
        x = sophus.SE3.exp(delta) * x

そして，ヤコビアンと誤差ベクトルをもとにガウスニュートンで変分量ベクトルdeltaを求め，それをexpmapで姿勢クラスへ変換した後現在の推定姿勢xを更新する．実行してみると以下のようにイテレーション毎に誤差の総和が減少していくことが確認できる．

動作確認用にOpen3DのICP結果と比べてみてもほぼ同じ結果が得られており，正しく動作していることがわかる．

--- estimated ---
[[ 9.99979859e-01 -6.33749072e-03 -3.42777728e-04  3.08255933e-01]
 [ 6.33673793e-03  9.99977602e-01 -2.15437730e-03  6.29273371e-02]
 [ 3.56423397e-04  2.15216182e-03  9.99997621e-01 -1.47334933e-02]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
--- open3d ---
[[ 9.99980555e-01 -6.22552305e-03 -3.63801969e-04  3.09716262e-01]
 [ 6.22472292e-03  9.99978292e-01 -2.16058864e-03  6.31702584e-02]
 [ 3.77244866e-04  2.15828206e-03  9.99997600e-01 -1.47732098e-02]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]

Red: target, Blue: source, Green: transed_source

得られた相対姿勢をもとに点群を表示してみると，初期姿勢よりターゲット点群に近づいていることが確認できるが，今回は最も単純なICPを考えたため完全には点群が重なっていない．遠すぎる対応付を捨てたり，Normal ICPなどの拡張を使えばより高精度なアラインメントが得られるが，いずれも基本的には同じ流れの処理で実装することができる．

今回は点に作用する姿勢の最適化例としてICP実装例を紹介した．次は姿勢同士の制約を考えたときの最適化として簡単なポーズグラフ最適化について書こうかと思う．

[2021/03/06]書いた

tetro

Expmapを使った姿勢最適化（ICP編）

関連

「Expmapを使った姿勢最適化（ICP編）」への2件のフィードバック

コメントを残すコメントをキャンセル

共有:

関連

「Expmapを使った姿勢最適化（ICP編）」への2件のフィードバック

コメントを残す コメントをキャンセル

コメントを残すコメントをキャンセル