def test_givenDataset_whenCallingItem_ShouldReturnTheTupleWithProperPosition( self): dataset = MidAirImageSequenceDatasetEulerDifferences( "./Ressources/MidAir/Kite_test", new_size=(512, 512), img_mean=[1, 1, 1], img_std=[1, 1, 1], trajectories=["trajectory_3000"]) with dataset.HDF5["./Ressources/MidAir/Kite_test/cloudy"] as hdf5: expected_position = hdf5["trajectory_3000"]["groundtruth"][ "position"][4:8] initial_rotation = Quaternion( hdf5["trajectory_3000"]["groundtruth"]["attitude"][4]) initial_position = expected_position[0] #Reverse position differences actual_position_differences = dataset.__getitem__(1)[1][:, 3:].numpy() self.assertEqual(actual_position_differences.shape, (3, 3)) actual_positions = Geometry.assemble_delta_translations( actual_position_differences) actual_positions = self._rotate_camera_frame_to_world_frame( torch.Tensor(actual_positions)).numpy() #remap axis to original absolute frame actual_positions = Geometry.remap_position_axes( initial_rotation, actual_positions) actual_positions = Geometry.reset_positions_to_origin( initial_position, actual_positions) self.assertEqual(actual_positions.shape, (4, 3)) self.assertTrue(numpy.allclose(actual_positions, expected_position))
def test_givenDataset_whenCallingGetPositionDifferences_shouldReturnProperPositionDifferences( self): with self.dataset.HDF5["./Ressources/MidAir/Kite_test/cloudy"] as hdf5: expected_position = hdf5["trajectory_3000"]["groundtruth"][ "position"][4:8] initial_rotation = Quaternion( hdf5["trajectory_3000"]["groundtruth"]["attitude"][4]) initial_position = expected_position[0] #Reverse position differences actual_position_differences = self.second_segment.get_position_differences( ).numpy() self.assertEqual(actual_position_differences.shape, (3, 3)) actual_positions_camera_frame = Geometry.assemble_delta_translations( actual_position_differences) #remap axis to original absolute frame actual_positions = self._rotate_camera_frame_to_world_frame( torch.Tensor(actual_positions_camera_frame)).numpy() for i, pos in enumerate(actual_positions_camera_frame): #assert that the positions were switch to the KITTI camera coordinate systen numpy.testing.assert_array_equal(actual_positions[i], pos[[2, 0, 1]]) actual_positions = Geometry.remap_position_axes( initial_rotation, actual_positions) actual_positions = Geometry.reset_positions_to_origin( initial_position, actual_positions) self.assertEqual(actual_positions.shape, (4, 3)) self.assertTrue(numpy.allclose(actual_positions, expected_position))
def _relative_predicted_pose_to_absolute_pose(self, previous_iteration_pose, pose): """ Transforms relative pose data to absolute pose @param previous_iteration_pose: @param pose: @return: """ if previous_iteration_pose is None and self._is_input_data_relative: translations = Geometry.assemble_delta_translations(pose[:, 3:]) rotations = Geometry.assemble_delta_tait_bryan_rotations( pose[:, :3]) return numpy.concatenate((rotations, translations), axis=1) elif previous_iteration_pose is not None and not self._is_input_data_relative: translations = Geometry.get_position_differences(pose[:, 3:]) rotations = Geometry.get_tait_bryan_orientation_differences( pose[:, :3]) else: translations = pose[:, 3:] rotations = pose[:, :3] # only keep the non overlapping predictions relative_pose_overlap = self._sliding_window_overlap - 1 translations = translations[relative_pose_overlap:] rotations = rotations[relative_pose_overlap:] translations = Geometry.remap_position_axes( Geometry.tait_bryan_rotation_to_quaternion( previous_iteration_pose[-1, :3]), translations) # assemble with the previous iteration poses temp_translation = numpy.concatenate( (previous_iteration_pose[-1, 3:].reshape((1, 3)), translations), axis=0) positions = Geometry.assemble_delta_translations(temp_translation)[2:] temp_rotations = numpy.concatenate( (previous_iteration_pose[-1, :3].reshape((1, 3)), rotations), axis=0) orientations = Geometry.assemble_delta_tait_bryan_rotations( temp_rotations)[2:] orientations = numpy.around(orientations, 7) return numpy.concatenate((orientations, positions), axis=1)
def test_givenDataset_whenCallingGetPositionDifferences_shouldReturnProperPositionDifferences(self): expected_position = self.dataframe5x5.iloc[1, :].pose[:, 9:] initial_rotation = Geometry.matrix_to_quaternion(self.dataframe5x5.iloc[1, :].pose[0, :9]) initial_position = expected_position[0] #Reverse position differences actual_position_differences = self.segment.get_position_differences().numpy() self.assertEqual(actual_position_differences.shape, (4, 3)) actual_positions = Geometry.assemble_delta_translations(actual_position_differences) #remap axis to original absolute frame actual_positions = Geometry.remap_position_axes(initial_rotation, actual_positions) actual_positions = Geometry.reset_positions_to_origin(initial_position, actual_positions) self.assertEqual(actual_positions.shape, (5, 3)) self.assertTrue(numpy.allclose(actual_positions, expected_position))
def _relative_target_pose_to_absolute_pose(self, previous_iteration_pose, pose): """ Transforms relative pose data to absolute pose @param previous_iteration_pose: @param pose: @return: """ if self._is_input_data_relative: translations = Geometry.assemble_delta_translations(pose[:, 3:]) rotations = Geometry.assemble_delta_tait_bryan_rotations( pose[:, :3]) else: translations = pose[:, 3:] rotations = pose[:, :3] if previous_iteration_pose is None: translations = Geometry.remap_position_axes( Geometry.tait_bryan_rotation_to_quaternion(rotations[0]), translations) translations = Geometry.reset_positions_to_origin( translations[0], translations) rotations = Geometry.reset_euler_orientations_to_origin( rotations[0], rotations) else: translations = Geometry.remap_position_axes( Geometry.tait_bryan_rotation_to_quaternion( previous_iteration_pose[ -self._sliding_window_overlap, :3]), translations) translations = Geometry.reset_positions_to_origin( previous_iteration_pose[-self._sliding_window_overlap, 3:], translations) rotations = Geometry.reset_euler_orientations_to_origin( previous_iteration_pose[-self._sliding_window_overlap, :3], rotations) rotations = numpy.around(rotations, 7) return numpy.concatenate((rotations, translations), axis=1)
def test_assemble_delta_translations(self): assembled = Geometry.assemble_delta_translations( self.dataset.__getitem__(1)[1].numpy()[:, 3:]) expected = numpy.asarray(self.segment.get_positions()) numpy.testing.assert_almost_equal(assembled, expected)