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()))