Example #1
0
 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())
Example #2
0
 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())