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)