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