前回はexpmapを使った点に作用する姿勢の最適化を行ったが,今回は姿勢同士の作用に対する最適化について書いてみる.まずは簡単に現在の推定姿勢をとして,目標の姿勢に一致するように推定姿勢を更新してみる.
最小化する誤差関数は姿勢間誤差がで与えられるので,これにlogmapを適用したとする.とが一致していればとなってとなる.[1] http://ethaneade.com/lie.pdfの2.3に書かれているように,別の姿勢に作用するある姿勢の微分は,接ベクトルを他の空間へ移動するadjointと呼ばれる操作で得られる(をで空間にマッピングする).SE3におけるadjointは式(90)にある通り,以下の式で与えられる.
これを使って,単純に,を計算し,の更新量をガウスニュートンで求められる().あとは前回ICPを実装したときと同様にexpmapでを姿勢へ変換しに適用すれば良い().
上記の流れを単純に実装すると以下のようになる.そのままガウスニュートンで更新量を求めると一ステップで誤差がゼロになって面白くないので,適当なダンピング係数をヘッシアン行列に足して徐々に目標姿勢に合うようにしている.
#!/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つの姿勢間の誤差最小化を行ったが,これをそのまま拡張すれば複数姿勢間の誤差を最小化するポーズグラフ最適化を行うことができる.詳細は省くが,姿勢系列として,相対姿勢制約集合のもとでの誤差を最小化する(は与えられた相対姿勢).
適当に実装してみた例が以下のコード.グラフ定義はパーキングデータセットを使った(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を使った姿勢最適化(ICP編) | tetro