def test_relative_se3_rot(self): a = lie.random_se3() b = lie.random_se3() self.assertTrue(lie.is_se3(a) and lie.is_se3(b)) a_to_b_rot = lie.relative_se3_rot(a, b) self.assertTrue(lie.is_so3(a_to_b_rot)) b_rot_from_a_rot = a[:3, :3].dot(a_to_b_rot) self.assertTrue(np.allclose(b_rot_from_a_rot, b[:3, :3]))
def test_relative_se3(self): a = lie.random_se3() b = lie.random_se3() self.assertTrue(lie.is_se3(a) and lie.is_se3(b)) a_to_b = lie.relative_se3(a, b) self.assertTrue(lie.is_se3(a_to_b)) b_from_a = a.dot(a_to_b) self.assertTrue(np.allclose(b_from_a, b))
def test_transform(self): traj_transformed = copy.deepcopy(ex_kitti_traj) t = lie.random_se3() traj_transformed.transform(t) # traj_transformed.transform(lie.se3_inverse(t)) self.assertAlmostEqual(traj_transformed.path_length(), ex_kitti_traj.path_length())
def test_so3_from_se3(self): p = lie.random_se3() r = lie.so3_from_se3(p) self.assertTrue(lie.is_so3(r))
def test_se3_inverse(self): p = lie.random_se3() p_inv = lie.se3_inverse(p) self.assertTrue(lie.is_se3(p_inv)) self.assertTrue(np.allclose(p_inv.dot(p), np.eye(4)))
def test_random_se3(self): self.assertTrue(lie.is_se3(lie.random_se3()))