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):
R_foo_bar = So2.exp(self.theta)
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.ZeroVector2())