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