def test_from_rotation_matrix_preset(self):
"""Tests that Euler angles can be retrieved from rotation matrices."""
matrix = test_helpers.generate_preset_test_rotation_matrices_3d()
predicted_angles = euler.from_rotation_matrix(matrix)
reconstructed_matrices = rotation_matrix_3d.from_euler(predicted_angles)
self.assertAllClose(reconstructed_matrices, matrix, rtol=1e-3)
def test_from_rotation_matrix_gimbal(self):
"""Testing that Euler angles can be retrieved in Gimbal lock."""
angles = test_helpers.generate_random_test_euler_angles()
angles[..., 1] = np.pi / 2.
matrix = rotation_matrix_3d.from_euler(angles)
predicted_angles = euler.from_rotation_matrix(matrix)
reconstructed_matrices = rotation_matrix_3d.from_euler(predicted_angles)
self.assertAllClose(reconstructed_matrices, matrix, rtol=1e-3)
angles[..., 1] = -np.pi / 2.
matrix = rotation_matrix_3d.from_euler(angles)
predicted_angles = euler.from_rotation_matrix(matrix)
reconstructed_matrices = rotation_matrix_3d.from_euler(predicted_angles)
self.assertAllClose(reconstructed_matrices, matrix, rtol=1e-3)
def test_from_rotation_matrix_jacobian_preset(self):
"""Test the Jacobian of the from_rotation_matrix function."""
x_init = test_helpers.generate_preset_test_rotation_matrices_3d()
x = tf.convert_to_tensor(value=x_init)
y = euler.from_rotation_matrix(x)
self.assert_jacobian_is_finite(x, x_init, y)
def test_from_rotation_matrix_random(self):
"""Tests that Euler angles can be retrieved from rotation matrices."""
matrix = test_helpers.generate_random_test_rotation_matrix_3d()
predicted_angles = euler.from_rotation_matrix(matrix)
# There is not a unique mapping from rotation matrices to Euler angles. The
# following constructs the rotation matrices from the `predicted_angles` and
# compares them with `matrix`.
reconstructed_matrices = rotation_matrix_3d.from_euler(predicted_angles)
self.assertAllClose(reconstructed_matrices, matrix, rtol=1e-3)
def test_from_rotation_matrix_jacobian_preset(self):
"""Test the Jacobian of the from_rotation_matrix function."""
if tf.executing_eagerly():
self.skipTest(reason="Graph mode only test")
with tf.compat.v1.Session() as sess:
x_init = np.array(
sess.run(
test_helpers.generate_preset_test_rotation_matrices_3d()))
x = tf.convert_to_tensor(value=x_init)
y = euler.from_rotation_matrix(x)
self.assert_jacobian_is_finite(x, x_init, y)
def angles_from_matrix(matrix):
"""Get a triplet of rotation angles from a rotation matrix.
Args:
matrix: A tf.tensor of shape [..., 3, 3], where the last two dimensions are
the rotation matrix.
Returns:
A tf.Tensor of shape [..., 3], where the last dimension is the rotation
angles, along x, y, and z.
This function mimics _euler2mat from struct2depth/project.py, for backward
compatibility, but wraps tensorflow_graphics instead of reimplementing it.
The negation and transposition are needed to bridge the differences between
the two.
"""
rank = tf.rank(matrix)
# Swap the two last dimensions
perm = tf.concat([tf.range(rank - 2), [rank - 1], [rank - 2]], axis=0)
return -euler.from_rotation_matrix(tf.transpose(matrix, perm))
