def test_batch_matrix_to_euler(self):
y = 0.1745329
x_prime = 0.3490659
z_prime_prime = 0.7853982
input_angles = [y, x_prime, z_prime_prime]
input_tensor = torch.Tensor([
input_angles, input_angles, input_angles, input_angles,
input_angles, input_angles, input_angles, input_angles
]).requires_grad_(True)
# 2 batches of 2 sequences of 2 frames
input_tensor = input_tensor.view((2, 4, 3))
matrix_tensor = TensorGeometry.batchEulerAnglesToRotationMatrixTensor(
input_tensor)
self.assertTrue(matrix_tensor.requires_grad)
euler_tensor = TensorGeometry.batchRotationMatrixTensorToEulerAngles(
matrix_tensor)
self.assertTrue(euler_tensor.requires_grad)
flat_actual_tensor = torch.flatten(euler_tensor,
start_dim=0,
end_dim=1).detach()
expected_matrix = input_tensor[1, 1, :].detach()
for actual_matrix in flat_actual_tensor:
numpy.testing.assert_allclose(expected_matrix,
actual_matrix,
rtol=3.22578393e-07)
def _compute_rotational_ATE(self, prediction: torch.Tensor,
target: torch.Tensor):
"""
Ins[ired by: https://github.com/uzh-rpg/rpg_trajectory_evaluation
@param prediction:
@param target:
@return:
"""
target_rot_matrix = TensorGeometry.batchEulerAnglesToRotationMatrixTensor(
target)
prediction_rot_matrix = TensorGeometry.batchEulerAnglesToRotationMatrixTensor(
prediction)
e_rot = torch.empty(target_rot_matrix.shape,
requires_grad=prediction.requires_grad).to(
prediction.device)
for i, segment in enumerate(prediction_rot_matrix):
for j, rotation in enumerate(segment):
e_rot[i, j] = torch.mm(target_rot_matrix[i, j],
rotation.inverse())
e_rot = TensorGeometry.batchRotationMatrixTensorToEulerAngles(e_rot)
return torch.mean(e_rot)
def test_batchEulerDifferences(self):
y = 0.1745329
x_prime = 0.3490659
z_prime_prime = 0.7853982
input_angles = [y, x_prime, z_prime_prime]
expected_euler_rotation_batch = torch.Tensor([
input_angles, input_angles, input_angles, input_angles,
input_angles, input_angles, input_angles, input_angles
]).requires_grad_(True)
# 2 segments of 4 euler rotations
expected_euler_rotation_batch = expected_euler_rotation_batch.view(
(2, 4, 3))
absolute_euler_orientation_batch = TensorGeometry.batch_assembleDeltaEulerAngles(
expected_euler_rotation_batch)
self.assertTrue(absolute_euler_orientation_batch.requires_grad)
actual_euler_rotation_batch = TensorGeometry.batch_eulerDifferences(
absolute_euler_orientation_batch)
self.assertTrue(actual_euler_rotation_batch.requires_grad)
numpy.testing.assert_allclose(
actual_euler_rotation_batch.detach().numpy(),
expected_euler_rotation_batch.detach().numpy(),
rtol=7.1029973e-07)
def compute(self, prediction: torch.Tensor, target: torch.Tensor):
"""
Minimises the distance between locations that are within the radius defined by self.radius_threshold.
The lowest value to which the distance can be minimized is the actual groundtruth distance between
the two points.
@param prediction: [rotations, translations] matrix where translations are in meters
@param target: [rotations, translations] matrix where translations are in meters
"""
predicted_positions = TensorGeometry.batch_assembleDeltaTranslationMatrices(
prediction[:, :, -3:])
target_positions = TensorGeometry.batch_assembleDeltaTranslationMatrices(
target[:, :, -3:])
loops = self._compute_loop_matrix(target_positions)
loops = loops.type(torch.FloatTensor).to(prediction.device)
predicted_distances = self._location_distances(predicted_positions)
target_distances = self._location_distances(target_positions)
# Use the loops' positions as boolean indices to select only the loops in the predicted and target
# distances matrix
predicted_loops_distances = loops * predicted_distances
target_loops_distances = loops * target_distances
return torch.nn.functional.mse_loss(predicted_loops_distances,
target_loops_distances)
def _compute_absolute_translation_loss(self, prediction: torch.Tensor,
target: torch.Tensor):
prediction = TensorGeometry.batch_assembleDeltaTranslationMatrices(
prediction[:, :, -3:])
target = TensorGeometry.batch_assembleDeltaTranslationMatrices(
target[:, :, -3:])
return torch.nn.functional.mse_loss(prediction, target)
def _compute_absolute_angle_loss(self, prediction: torch.Tensor,
target: torch.Tensor):
prediction = TensorGeometry.batch_assembleDeltaEulerAngles(
prediction[:, :, :-3])
target = TensorGeometry.batch_assembleDeltaEulerAngles(
target[:, :, :-3])
return 100 * torch.nn.functional.mse_loss(prediction, target)
def _compute_absolute_angle_loss(self, prediction: torch.Tensor,
target: torch.Tensor):
prediction_absolute_rot_matrices = TensorGeometry.batch_assembleDeltaRotationMatrices(
TensorGeometry.batchEulerAnglesToRotationMatrixTensor(
prediction[:, :, :-3]))
target_absolute_rot_matrices = TensorGeometry.batch_assembleDeltaRotationMatrices(
TensorGeometry.batchEulerAnglesToRotationMatrixTensor(
target[:, :, :-3]))
return 100 * torch.nn.functional.mse_loss(
prediction_absolute_rot_matrices, target_absolute_rot_matrices)
def _compute_global_loss(self, prediction: torch.Tensor,
target: torch.Tensor):
global_rotations = TensorGeometry.batch_assembleDeltaEulerAngles(
target[:, :, :-3])
global_translations = TensorGeometry.batch_assembleDeltaTranslationMatrices(
target[:, :, -3:])
global_pose_minus_start = torch.cat(
(global_rotations, global_translations), dim=2)[:, 1:, :]
loss = BatchSegmentMSELoss()
return loss.compute(prediction, global_pose_minus_start)
def test_BatchRelativeTranslationMatricesToAbsolute(self):
input_translation = [1, 1, 1]
expected_relative_translation_batch = torch.Tensor([
input_translation, input_translation, input_translation,
input_translation, input_translation, input_translation,
input_translation, input_translation
]).requires_grad_(True)
# 2 segments of 4 translations
expected_relative_translation_batch = expected_relative_translation_batch.view(
(2, 4, 3))
absolute_translation_batch = TensorGeometry.batch_assembleDeltaTranslationMatrices(
expected_relative_translation_batch)
self.assertTrue(absolute_translation_batch.requires_grad)
actual_relative_translation_batch = torch.zeros(
expected_relative_translation_batch.shape)
# take the absolute translations tensor and make the translations relative
for i, segment in enumerate(absolute_translation_batch):
for j, translation in enumerate(segment[1:]):
actual_relative_translation_batch[i,
j] = translation - segment[j]
actual_relative_translation_batch = actual_relative_translation_batch.detach(
).numpy()
expected_relative_translation_batch = expected_relative_translation_batch.detach(
).numpy()
numpy.testing.assert_allclose(actual_relative_translation_batch,
expected_relative_translation_batch)
def test_batch_assembleDeltaEulerAngles(self):
y = 0.1745329
x_prime = 0.3490659
z_prime_prime = 0.7853982
input_angles = [y, x_prime, z_prime_prime]
relative_euler_rotation_batch = torch.Tensor([
input_angles, input_angles, input_angles, input_angles,
input_angles, input_angles, input_angles, input_angles
]).requires_grad_(True)
# 2 segments of 4 euler rotations
relative_euler_rotation_batch = relative_euler_rotation_batch.view(
(2, 4, 3))
actual_absolute_euler_orientation_batch = TensorGeometry.batch_assembleDeltaEulerAngles(
relative_euler_rotation_batch)
self.assertTrue(actual_absolute_euler_orientation_batch.requires_grad)
actual_absolute_matrix_orientation_batch = TensorGeometry.batchEulerAnglesToRotationMatrixTensor(
actual_absolute_euler_orientation_batch)
self.assertTrue(actual_absolute_matrix_orientation_batch.requires_grad)
actual_relative_rotation_batch = torch.zeros(
actual_absolute_matrix_orientation_batch[:, 1:, :].shape)
#take the absolute rotation tensor and make the rotations relative
for i, segment in enumerate(actual_absolute_matrix_orientation_batch):
for j, rotation in enumerate(segment[1:]):
actual_relative_rotation_batch[i, j] = torch.mm(
rotation, segment[j].inverse())
actual_relative_rotation_batch = TensorGeometry.batchRotationMatrixTensorToEulerAngles(
actual_relative_rotation_batch)
self.assertTrue(actual_relative_rotation_batch.requires_grad)
actual_relative_rotation_batch = actual_relative_rotation_batch.detach(
).numpy()
expected_relative_rotation_batch = relative_euler_rotation_batch.detach(
).numpy()
numpy.testing.assert_allclose(actual_relative_rotation_batch,
expected_relative_rotation_batch,
rtol=7.1029973e-07)
def test_batch_computeDeltaTranslations(self):
input_translation = [1, 1, 1]
expected_relative_translation_batch = torch.Tensor([
input_translation, input_translation, input_translation,
input_translation, input_translation, input_translation,
input_translation, input_translation
]).requires_grad_(True)
# 2 segments of 4 translations
expected_relative_translation_batch = expected_relative_translation_batch.view(
(2, 4, 3))
global_positions_batch = TensorGeometry.batch_assembleDeltaTranslationMatrices(
expected_relative_translation_batch)
self.assertTrue(global_positions_batch.requires_grad)
acutal_relative_translation_batch = TensorGeometry.batch_computeDeltaTranslations(
global_positions_batch)
self.assertTrue(acutal_relative_translation_batch.requires_grad)
expected_relative_translation_batch = expected_relative_translation_batch.detach(
).numpy()
acutal_relative_translation_batch = acutal_relative_translation_batch.detach(
).numpy()
numpy.testing.assert_allclose(acutal_relative_translation_batch,
expected_relative_translation_batch)
def test_rotation_matrix_to_euler(self):
# Intrinsic Tait-Bryan x-y'-z'' angles
y = 0.1745329
x_prime = 0.3490659
z_prime_prime = 0.7853982
input_tensor = torch.Tensor(
Rotation.from_euler(
"YXZ",
[y, x_prime, z_prime_prime]).as_matrix()).requires_grad_(True)
actual_matrix = TensorGeometry.rotation_matrix_to_euler_tait_bryan(
input_tensor)
self.assertTrue(actual_matrix.requires_grad)
expected_matrix = numpy.asarray([y, x_prime, z_prime_prime])
numpy.testing.assert_allclose(expected_matrix,
actual_matrix.detach(),
rtol=3.22578393e-07)
def test_euler_angles_to_rotation_matrix(self):
# Intrinsic Tait-Bryan y-x'-z'' angles
y = 0.1745329
x_prime = 0.3490659
z_prime_prime = 0.7853982
input_tensor = torch.Tensor([y, x_prime,
z_prime_prime]).requires_grad_(True)
expected_matrix = Rotation.from_euler(
"YXZ", [y, x_prime, z_prime_prime]).as_matrix()
actual_matrix = TensorGeometry.eulerAnglesToRotationMatrixTensor(
input_tensor[0], input_tensor[1], input_tensor[2])
self.assertTrue(actual_matrix.requires_grad)
actual_matrix = actual_matrix.detach().numpy()
numpy.testing.assert_allclose(expected_matrix,
actual_matrix,
rtol=3.22578393e-07)