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): path = helpers.fake_path(10) path_transformed = copy.deepcopy(path) t = lie.random_se3() path_transformed.transform(t) # traj_transformed.transform(lie.se3_inverse(t)) self.assertAlmostEqual(path_transformed.path_length, path.path_length)
def test_alignment_degenerate_case(self): length = 100 poses = [lie.random_se3()] * length traj_1 = PoseTrajectory3D(poses_se3=poses, timestamps=helpers.fake_timestamps( length, 1, 0.0)) traj_2 = copy.deepcopy(traj_1) traj_2.transform(lie.random_se3()) traj_2.scale(1.234) self.assertNotEqual(traj_1, traj_2) with self.assertRaises(GeometryException): traj_1.align(traj_2) with self.assertRaises(GeometryException): traj_1.align(traj_2, correct_scale=True)
def test_se3_alignment(self): traj = helpers.fake_trajectory(1000, 1) traj_transformed = copy.deepcopy(traj) traj_transformed.transform(lie.random_se3()) self.assertNotEqual(traj, traj_transformed) traj_transformed.align(traj) self.assertEqual(traj_transformed, traj)
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_sim3_alignment(self): traj = helpers.fake_trajectory(1000, 1) traj_transformed = copy.deepcopy(traj) traj_transformed.transform(lie.random_se3()) traj_transformed.scale(1.234) self.assertNotEqual(traj, traj_transformed) traj_transformed.align(traj, correct_scale=True) self.assertEqual(traj_transformed, traj)
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()))
def random_se3_list(length): return [lie.random_se3() for i in range(length)]
id_pairs = filters.filter_pairs_by_path(POSES_3, target_path, tol, all_pairs=True) self.assertEqual(id_pairs, [(0, 7)]) axis = np.array([1, 0, 0]) POSES_5 = [ lie.se3(lie.so3_exp(axis * 0.0), np.array([0, 0, 0])), lie.se3(lie.so3_exp(axis * math.pi), np.array([0, 0, 0])), lie.se3(lie.so3_exp(axis * 0.0), np.array([0, 0, 0])), lie.se3(lie.so3_exp(axis * math.pi / 3), np.array([0, 0, 0])), lie.se3(lie.so3_exp(axis * math.pi), np.array([0, 0, 0])) ] TRANSFORM = lie.random_se3() POSES_5_TRANSFORMED = [TRANSFORM.dot(p) for p in POSES_5] axis = np.array([1, 0, 0]) p0 = lie.se3(lie.so3_exp(axis * 0.0), np.array([0, 0, 0])) pd = lie.se3(lie.so3_exp(axis * (math.pi / 3.)), np.array([1, 2, 3])) p1 = np.dot(p0, pd) p2 = np.dot(p1, pd) p3 = np.dot(p2, pd) POSES_6 = [p0, p1, p2, p3, p3] POSES_6_TRANSFORMED = [TRANSFORM.dot(p) for p in POSES_6] class TestFilterPairsByAngle(unittest.TestCase): def test_poses5(self): tol = 0.001