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)