def test_matrix(self):
R_foo_bar = So3.exp(self.omega)
Rmat_foo_bar = R_foo_bar.matrix()
point_bar = self.p
p1_foo = R_foo_bar * point_bar
p2_foo = Rmat_foo_bar * point_bar
self.assertEqual(sympy.simplify(p1_foo - p2_foo), sophus.ZeroVector3())
def test_matrix(self):
T_foo_bar = Se3.exp(self.upsilon_omega)
Tmat_foo_bar = T_foo_bar.matrix()
point_bar = self.p
p1_foo = T_foo_bar * point_bar
p2_foo = sophus.proj(Tmat_foo_bar * sophus.unproj(point_bar))
self.assertEqual(sympy.simplify(p1_foo - p2_foo), sophus.ZeroVector3())
def Dxi_exp_x_matrix_at_0(i):
v = sophus.ZeroVector3()
v[i] = 1
return So3.hat(v)
