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 test_BatchRelativeRotationMatricesToAbsolute(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))
expected_relative_rotation_batch = TensorGeometry.batchEulerAnglesToRotationMatrixTensor(
relative_euler_rotation_batch)
self.assertTrue(expected_relative_rotation_batch.requires_grad)
actual_absolute_orientation_batch = TensorGeometry.batch_assembleDeltaRotationMatrices(
expected_relative_rotation_batch)
self.assertTrue(actual_absolute_orientation_batch.requires_grad)
actual_relative_rotation_batch = torch.zeros(
expected_relative_rotation_batch.shape)
#take the absolute rotation tensor and make the rotations relative
for i, segment in enumerate(actual_absolute_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 = actual_relative_rotation_batch.detach(
).numpy()
expected_relative_rotation_batch = expected_relative_rotation_batch.detach(
).numpy()
numpy.testing.assert_allclose(actual_relative_rotation_batch,
expected_relative_rotation_batch,
rtol=7.1029973e-07)
