def test_matrix(self):
T_foo_bar = Se2.exp(self.upsilon_theta)
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.ZeroVector2())
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())