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)