Expmapを使った最適化（ポーズグラフ編）

前回はexpmapを使った点に作用する姿勢の最適化を行ったが，今回は姿勢同士の作用に対する最適化について書いてみる．まずは簡単に現在の推定姿勢を $P_0$ として，目標の姿勢 $P_1$ に一致するように推定姿勢を更新してみる．

最小化する誤差関数は姿勢間誤差が $P = P_1^{-1} P_0$ で与えられるので，これにlogmapを適用した $E = | \log(P_1^{-1} P_0) |^2$ とする． $P_0$ と $P_1$ が一致していれば $P = \mathbb{I}$ となって $E = \log(\mathbb{I}) = 0$ となる．[1] http://ethaneade.com/lie.pdfの2.3に書かれているように，別の姿勢 $C_1$ に作用するある姿勢 $C_0$ の微分 $\partial C_1 C_0 / \partial C_0$ は，接ベクトルを他の空間へ移動するadjointと呼ばれる操作で得られる( $\partial C_0 / \partial C_0 = \mathbb{I}$ を $adj(C_1)$ で $C_1$ 空間にマッピングする)．SE3におけるadjointは式(90)にある通り，以下の式で与えられる．

これを使って，単純に $J=adj(P_1^{-1})$ ， $e=\log(P_1^{-1} P_0)$ を計算し， $P_0$ の更新量 $\delta P_0$ をガウスニュートンで求められる( $H = J^T J, b = J^T e, H \delta P_0 = b$ )．あとは前回ICPを実装したときと同様にexpmapで $\delta P_0$ を姿勢へ変換し $P_0$ に適用すれば良い( $P_0^{new} = \exp(\delta P_0) P_0$ )．

上記の流れを単純に実装すると以下のようになる．そのままガウスニュートンで更新量を求めると一ステップで誤差がゼロになって面白くないので，適当なダンピング係数 $\mathbb{I}$ をヘッシアン行列に足して徐々に目標姿勢に合うようにしている．

#!/usr/bin/python3
import numpy
import sophus

# create random SE3 poses
C0 = sophus.SE3.exp(numpy.random.randn(6))
C1 = sophus.SE3.exp(numpy.random.randn(6))
inv_C1 = C1.inverse()

for i in range(12):
	error = (inv_C1 * C0).log()
	print('error:', numpy.linalg.norm(error, ord=2))

	# dC1C0 / dC0 = adj(C1)
	J = numpy.zeros((6, 6))
	J[:3, :3] = inv_C1.rotationMatrix()
	J[:3, 3:] = sophus.SO3.hat(inv_C1.translation()).dot(inv_C1.rotationMatrix())
	J[3:, 3:] = inv_C1.rotationMatrix()

	# find the delta with Gauss-Newton
	H = J.T.dot(J) + numpy.identity(6)
	b = J.T.dot(error)
	delta = numpy.linalg.solve(H, -b)
	
	# update the estimate
	C0 = sophus.SE3.exp(delta) * C0

上の例では非常に単純な2つの姿勢間の誤差最小化を行ったが，これをそのまま拡張すれば複数姿勢間の誤差を最小化するポーズグラフ最適化を行うことができる．詳細は省くが，姿勢系列 $\mathcal{P} = \left\{ P_0, \cdots, P_N \right\}$ として，相対姿勢制約集合 $\mathcal{F}$ のもとでの誤差 $E = \sum_{i, j, \tilde{P}_{ij} \in \mathcal{F}} \|\log(\tilde{P}_{ij}^{-1} P_i^{-1} P_j) \|^2$ を最小化する( $\tilde{P}_{ij}$ は与えられた相対姿勢)．

適当に実装してみた例が以下のコード．グラフ定義はパーキングデータセットを使った(https://github.com/AndreaCensi/efpno/blob/master/data/parking-garage.g2o)．

#!/usr/bin/python3
import csv
import numpy
import sophus
import scipy.sparse
import scipy.sparse.linalg
from scipy.spatial.transform import Rotation

def adj(C):
	adj_C = numpy.zeros((6, 6))
	adj_C[:3, :3] = C.rotationMatrix()
	adj_C[:3, 3:] = sophus.SO3.hat(C.translation()).dot(C.rotationMatrix())
	adj_C[3:, 3:] = C.rotationMatrix()
	return adj_C

class PoseGraph(object):
	def __init__(self):
		self.values = []
		self.factors = []

	def add_value(self, pose):
		self.values.append(pose)

	def add_factor(self, i, j, relative_pose):
		self.factors.append((i, j, relative_pose))

	def linearize_factor(self, factor):
		i, j, relative_pose = factor
		xi, xj = self.values[i], self.values[j]

		error = (relative_pose.inverse() * xi.inverse() * xj).log()
		Jj = adj(relative_pose.inverse() * xi.inverse())
		Ji = -adj(xj.inverse())
		return (i, j, error, Ji, Jj)

	def optimize(self):
		H = numpy.zeros((len(self.values) * 6, len(self.values) * 6))
		b = numpy.zeros(len(self.values) * 6)

		sum_errors = 0.0
		for factor in self.factors:
			i, j, error, Ji, Jj = self.linearize_factor(factor)

			H[i*6:(i+1)*6, i*6:(i+1)*6] += Ji.T.dot(Ji)
			H[j*6:(j+1)*6, j*6:(j+1)*6] += Jj.T.dot(Jj)
			H[i*6:(i+1)*6, j*6:(j+1)*6] += Ji.T.dot(Jj)
			H[j*6:(j+1)*6, i*6:(i+1)*6] += Jj.T.dot(Ji)
			b[i*6:(i+1)*6] += Ji.T.dot(error)
			b[j*6:(j+1)*6] += Jj.T.dot(error)
			sum_errors += numpy.linalg.norm(error, ord=2)
		print('sum_errors:', sum_errors)

		# fix the first node
		H[:6, :6] += numpy.identity(6) * 1e3

		x = scipy.sparse.linalg.spsolve(H, -b)

		for i in range(len(self.values)):
			delta_xi = sophus.SE3.exp(x[i*6: (i+1)*6])
			self.values[i] = delta_xi * self.values[i]

	def load_g2o(self, filename):
		with open(filename, 'r') as f:
			for line in csv.reader(f, delimiter=' '):
				if line[0] == 'VERTEX_SE3:QUAT':
					R = Rotation.from_quat(line[5:9]).as_matrix()
					t = line[2:5]
					self.add_value(sophus.SE3(R, t))

				if line[0] == 'EDGE_SE3:QUAT':
					R = Rotation.from_quat(line[6:10]).as_matrix()
					t = line[3:6]
					self.add_factor(int(line[1]), int(line[2]), sophus.SE3(R, t))


def main():
	pose_graph = PoseGraph()
	pose_graph.load_g2o('parking-garage.g2o')

	for i in range(5):
		pose_graph.optimize()


if __name__ == '__main__':
	main()

sum_errors: 6087.537418809932
sum_errors: 606.0143844042917
sum_errors: 104.74055102277116
sum_errors: 78.42263455526295
sum_errors: 78.21073540885442

相対姿勢誤差の総和が正しく減少しており，いわゆるループクローズができていることが確認できる．

前回記事と合わせて，点に作用する姿勢，姿勢間の作用における最適化がexpmapで簡単に実装できることがわかったと思う．単純な例ではあったが，複雑な系の最適化もこの2つの作用を基本として拡張していくことで実現ができる．expmapは姿勢の最適化において必須知識である割に実際にどうやって使えばよいのかという資料が少ないと思っていたので，今回は実際のコード例と合わせて説明を行ってみた．

tetro

Expmapを使った最適化（ポーズグラフ編）

関連

「Expmapを使った最適化（ポーズグラフ編）」への1件のフィードバック

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

共有:

関連

「Expmapを使った最適化（ポーズグラフ編）」への1件のフィードバック

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

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