Expmapを使った最適化(ポーズグラフ編)

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

最小化する誤差関数は姿勢間誤差がP = P_1^{-1} P_0で与えられるので,これにlogmapを適用したE = | \log(P_1^{-1} P_0) |^2とする.P_0P_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は姿勢の最適化において必須知識である割に実際にどうやって使えばよいのかという資料が少ないと思っていたので,今回は実際のコード例と合わせて説明を行ってみた.

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

  1. ピンバック: Expmapを使った姿勢最適化(ICP編) | tetro

コメントを残す

メールアドレスが公開されることはありません。 が付いている欄は必須項目です

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください