def test_givenDataset_whenCallingItem_ShouldReturnTheTupleWithProperAttitude(
self):
dataset = MidAirImageSequenceDatasetEulerDifferences(
"./Ressources/MidAir/Kite_test",
new_size=(512, 512),
img_mean=[1, 1, 1],
img_std=[1, 1, 1],
trajectories=["trajectory_3000"])
relative_attitude = dataset.__getitem__(1)[1][:, :3]
with dataset.HDF5["./Ressources/MidAir/Kite_test/cloudy"] as hdf5:
expected_attitude = hdf5["trajectory_3000"]["groundtruth"][
"attitude"][4:8]
initial_rotation = Quaternion(expected_attitude[0])
relative_attitude = Geometry.tait_bryan_rotations_to_quaternions(
relative_attitude)
actual_attitudes = Geometry.assemble_delta_quaternion_rotations(
relative_attitude)
actual_attitudes = self._rotate_quaternions_to_world_frame(
actual_attitudes)
actual_attitudes = Geometry.reset_orientations_to_origin(
initial_rotation, actual_attitudes)
actual_attitudes = [att.elements for att in actual_attitudes]
self.assertTrue(numpy.allclose(actual_attitudes, expected_attitude))
def test_givenDataset_whenCallingGetAttitudeDifferences_shouldReturnProperAttitudeDifference(
self):
with self.dataset.HDF5["./Ressources/MidAir/Kite_test/cloudy"] as hdf5:
expected_attitude = hdf5["trajectory_3000"]["groundtruth"][
"attitude"][4:8]
initial_attitude = expected_attitude[0]
actual_attitude_differences = self.second_segment.get_attitude_differences(
)
self.assertEqual(len(actual_attitude_differences), 3)
self.assertFalse(
numpy.allclose(actual_attitude_differences[0].elements,
numpy.asarray([1.0, 0.0, 0.0, 0.0])))
actual_attitude = Geometry.assemble_delta_quaternion_rotations(
actual_attitude_differences)
self.assertEqual(len(actual_attitude), 4)
self.assertEqual(
None,
numpy.testing.assert_array_almost_equal(
actual_attitude[0].elements,
numpy.asarray([1.0, 0.0, 0.0, 0.0])))
actual_attitude_camera_frame = Geometry.quaternion_elements_to_quaternions(
actual_attitude)
actual_attitude = self._rotate_quaternions_to_world_frame(
actual_attitude_camera_frame)
for i, quat in enumerate(actual_attitude_camera_frame):
imaginaries = quat.elements[1:]
#assert that the quaternions were switch to the KITTI camera coordinate systen
numpy.testing.assert_array_equal(actual_attitude[i].elements[1:],
imaginaries[[2, 0, 1]])
actual_attitude = Geometry.reset_orientations_to_origin(
Quaternion(initial_attitude), actual_attitude)
self.assertTrue(
numpy.allclose(
numpy.asarray([att.elements for att in actual_attitude]),
expected_attitude))
def test_givenDataset_whenCallingGetAttitudeDifferences_shouldReturnProperAttitudeDifference(self):
expected_attitude = self.dataframe5x5.iloc[1, :].pose[:, :9]
initial_attitude = Geometry.matrix_to_quaternion(self.dataframe5x5.iloc[1, :].pose[0, :9])
actual_attitude_differences = self.segment.get_attitude_differences()
self.assertEqual(len(actual_attitude_differences), 4)
self.assertFalse(numpy.allclose(actual_attitude_differences[0].elements,
numpy.asarray([1.0, 0.0, 0.0, 0.0])))
actual_attitude = Geometry.assemble_delta_quaternion_rotations(actual_attitude_differences)
self.assertEqual(len(actual_attitude), 5)
self.assertEqual(None, numpy.testing.assert_array_almost_equal(actual_attitude[0].elements,
numpy.asarray([1.0, 0.0, 0.0, 0.0])))
actual_attitude = Geometry.reset_orientations_to_origin(Quaternion(initial_attitude), actual_attitude)
self.assertTrue(numpy.allclose(numpy.asarray(
[Geometry.quaternion_to_matrix(att) for att in actual_attitude]
),
expected_attitude
))