def test_assemble_delta_tait_bryan_rotations(self):
assembled = Geometry.assemble_delta_tait_bryan_rotations(
self.dataset.__getitem__(1)[1].numpy()[:, :3])
expected = numpy.asarray(
Geometry.quaternions_to_tait_bryan_rotations(
self.segment.get_attitudes()))
numpy.testing.assert_almost_equal(assembled, expected)
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_reset_euler_orientations_to_origin(self):
assembled = Geometry.assemble_delta_tait_bryan_rotations(
self.dataset.__getitem__(1)[1].numpy()[:, :3])
initial_rotation = Geometry.matrix_to_tait_bryan_euler(
self.dataframe5x5.iloc[1, 2][0, :9])
expected = Geometry.rotation_matrices_to_euler(
self.dataframe5x5.iloc[1, 2][:, :9])
resetted = Geometry.reset_euler_orientations_to_origin(
initial_rotation, assembled)
numpy.testing.assert_almost_equal(resetted, expected)
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)