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
