def test_exp_log(self):
for v in [sophus.Vector6(0., 1, 0.5, 2., 1, 0.5),
sophus.Vector6(0.1, 0.1, 0.1, 0., 1, 0.5),
sophus.Vector6(0.01, 0.2, 0.03, 0.01, 0.2, 0.03)]:
w = Se3.exp(v).log()
for i in range(0, 3):
self.assertAlmostEqual(v[i], w[i])
def vee(Omega):
""" R^4x4 => R^6 """
""" returns 6-vector representation of Lie algebra """
""" This is the inverse of the hat-operator """
head = sophus.Vector3(Omega[0,3], Omega[1,3], Omega[2,3])
tail = sophus.So3.vee(Omega[0:3,0:3])
upsilon_omega = \
sophus.Vector6(head[0], head[1], head[2], tail[0], tail[1], tail[2])
return upsilon_omega
def setUp(self):
upsilon0, upsilon1, upsilon2, omega0, omega1, omega2 = sympy.symbols(
'upsilon[0], upsilon[1], upsilon[2], omega[0], omega[1], omega[2]',
real=True)
x, v0, v1, v2 = sympy.symbols('q.w() q.x() q.y() q.z()', real=True)
p0, p1, p2 = sympy.symbols('p0 p1 p2', real=True)
t0, t1, t2 = sympy.symbols('t[0] t[1] t[2]', real=True)
v = sophus.Vector3(v0, v1, v2)
self.upsilon_omega = sophus.Vector6(
upsilon0, upsilon1, upsilon2, omega0, omega1, omega2)
self.t = sophus.Vector3(t0, t1, t2)
self.a = Se3(sophus.So3(sophus.Quaternion(x, v)), self.t)
self.p = sophus.Vector3(p0, p1, p2)