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 ))