Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def random_se3_list(length):
return [lie.random_se3() for _ in range(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_aligned = trajectory.align_trajectory(traj_transformed, traj,
correct_scale=True)
self.assertEqual(traj_aligned, traj)
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_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):
trajectory.align_trajectory(traj_1, traj_2)
with self.assertRaises(GeometryException):
trajectory.align_trajectory(traj_1, traj_2, correct_scale=True)
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):
trajectory.align_trajectory(traj_1, traj_2)
with self.assertRaises(GeometryException):
trajectory.align_trajectory(traj_1, traj_2, correct_scale=True)
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_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_aligned = trajectory.align_trajectory(traj_transformed, traj)
self.assertEqual(traj_aligned, traj)