def test_equals(self):
path_1 = helpers.fake_path(10)
path_1_copy = copy.deepcopy(path_1)
path_2 = helpers.fake_path(15)
self.assertTrue(path_1 == path_1_copy)
self.assertFalse(path_1 == path_2)
self.assertTrue(path_1 != path_2)
self.assertFalse(path_1 != path_1_copy)
def test_transform_sim3(self):
path = helpers.fake_path(10)
path_transformed = copy.deepcopy(path)
t = lie.sim3(r=lie.random_so3(), t=np.ones(3), s=1.234)
path_transformed.transform(t)
self.assertAlmostEqual(path_transformed.path_length,
path.path_length * 1.234)
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_scale(self):
path = helpers.fake_path(10)
path_scaled = copy.deepcopy(path)
s = 5.234
path_scaled.scale(s)
len_initial = path.path_length
len_scaled = path_scaled.path_length
self.assertAlmostEqual(len_initial * s, len_scaled)
def test_write_read_integrity(self):
traj_out = helpers.fake_path(1000)
self.assertTrue(traj_out.check())
file_interface.write_kitti_poses_file(self.mock_file, traj_out)
self.mock_file.seek(0)
traj_in = file_interface.read_kitti_poses_file(self.mock_file)
self.assertIsInstance(traj_in, PosePath3D)
self.assertTrue(traj_in.check())
self.assertTrue(traj_out == traj_in)
def test_reduce_to_ids(self):
path = helpers.fake_path(10)
path_reduced = copy.deepcopy(path)
path_reduced.reduce_to_ids([0, 2])
self.assertEqual(path_reduced.num_poses, 2)
# direct connection from 0 to 2 in initial should be reduced path length
len_initial_segment = np.linalg.norm(path.positions_xyz[2] -
path.positions_xyz[0])
len_reduced = path_reduced.path_length
self.assertAlmostEqual(len_initial_segment, len_reduced)
def test_init_wrong_args(self):
path = helpers.fake_path(10)
# no args
with self.assertRaises(trajectory.TrajectoryException):
trajectory.PosePath3D()
# only quaternion
with self.assertRaises(trajectory.TrajectoryException):
trajectory.PosePath3D(
orientations_quat_wxyz=path.orientations_quat_wxyz)
# only xyz
with self.assertRaises(trajectory.TrajectoryException):
trajectory.PosePath3D(positions_xyz=path.positions_xyz)
def test_init_correct(self):
# only poses_se3
path = helpers.fake_path(10)
try:
trajectory.PosePath3D(poses_se3=path.poses_se3)
except trajectory.TrajectoryException:
self.fail("unexpected init failure with only poses_se3")
# xyz + quaternion
try:
trajectory.PosePath3D(path.positions_xyz,
path.orientations_quat_wxyz)
except trajectory.TrajectoryException:
self.fail("unexpected init failure with xyz + quaternion")
# all
try:
trajectory.PosePath3D(path.positions_xyz,
path.orientations_quat_wxyz, path.poses_se3)
except trajectory.TrajectoryException:
self.fail(
"unexpected init failure with xyz + quaternion + poses_se3")
def test_distances(self):
path = helpers.fake_path(10)
self.assertEqual(path.distances[0], 0.0)
self.assertEqual(path.distances.size, path.num_poses)
self.assertAlmostEqual(path.distances[-1], path.path_length)
def test_get_statistics(self):
helpers.fake_path(10).get_statistics()
def test_get_infos(self):
helpers.fake_path(10).get_infos()
def test_check(self):
self.assertTrue(helpers.fake_path(10).check()[0])
path_wrong = helpers.fake_path(10)
_ = path_wrong.orientations_quat_wxyz
path_wrong._orientations_quat_wxyz[1][1] = 666
self.assertFalse(path_wrong.check()[0])
def test_wrong_type(self):
path_1 = helpers.fake_path(10)
path_2 = helpers.fake_path(10)
with self.assertRaises(sync.SyncException):
sync.associate_trajectories(path_1, path_2)
