def test_is_valid_random(self):
"""Tests that is_valid works as intended."""
random_euler_angle = test_helpers.generate_random_test_euler_angles()
tensor_tile = random_euler_angle.shape[:-1]
rotation_matrix = rotation_matrix_3d.from_euler(random_euler_angle)
pred_normalized = rotation_matrix_3d.is_valid(rotation_matrix)
with self.subTest(name="all_normalized"):
self.assertAllEqual(pred_normalized,
np.ones(shape=tensor_tile + (1, ), dtype=bool))
with self.subTest(name="non_orthonormal"):
test_matrix = np.array([[2., 0., 0.], [0., 0.5, 0], [0., 0., 1.]])
pred_normalized = rotation_matrix_3d.is_valid(test_matrix)
self.assertAllEqual(pred_normalized,
np.zeros(shape=(1, ), dtype=bool))
with self.subTest(name="negative_orthonormal"):
test_matrix = np.array([[1., 0., 0.], [0., -1., 0.], [0., 0., 1.]])
pred_normalized = rotation_matrix_3d.is_valid(test_matrix)
self.assertAllEqual(pred_normalized,
np.zeros(shape=(1, ), dtype=bool))
def test_from_euler_with_small_angles_approximation_jacobian_random(self):
"""Test the Jacobian of from_euler_with_small_angles_approximation."""
x_init = test_helpers.generate_random_test_euler_angles(
min_angle=-0.17, max_angle=0.17)
self.assert_jacobian_is_correct_fn(
rotation_matrix_3d.from_euler_with_small_angles_approximation, [x_init])
def test_from_euler_normalized_random(self):
"""Tests that euler angles can be converted to rotation matrices."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
matrix = rotation_matrix_3d.from_euler(random_euler_angles)
self.assertAllEqual(rotation_matrix_3d.is_valid(matrix),
np.ones(random_euler_angles.shape[0:-1] + (1, )))
def test_from_euler_jacobian_random(self):
"""Test the Jacobian of the from_euler function."""
x_init = test_helpers.generate_random_test_euler_angles(dimensions=1)
x = tf.convert_to_tensor(value=x_init)
y = rotation_matrix_2d.from_euler(x)
self.assert_jacobian_is_correct(x, x_init, y)
def test_inverse_random(self):
"""Checks that inverse works as intended."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
prediction = euler.inverse(random_euler_angles)
groundtruth = -random_euler_angles
self.assertAllClose(prediction, groundtruth, rtol=1e-3)
def test_inverse_jacobian_random(self):
"""Test the Jacobian of the inverse function."""
x_init = test_helpers.generate_random_test_euler_angles()
x = tf.convert_to_tensor(value=x_init)
y = euler.inverse(x)
self.assert_jacobian_is_correct(x, x_init, y)
def test_from_euler_normalized_random(self):
"""Tests that from_euler allows build normalized axis-angles."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_axis, random_angle = axis_angle.from_euler(random_euler_angles)
self.assertAllEqual(
axis_angle.is_normalized(random_axis, random_angle),
np.ones(shape=random_angle.shape))
def test_from_euler_normalized_random(self):
"""Tests that quaternions.from_euler returns normalized quaterions."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
tensor_shape = random_euler_angles.shape[:-1]
random_quaternion = quaternion.from_euler(random_euler_angles)
self.assertAllEqual(quaternion.is_normalized(random_quaternion),
np.ones(shape=tensor_shape + (1, ), dtype=bool))
def test_from_euler_with_small_angles_approximation_jacobian_random(self):
"""Test the Jacobian of from_euler_with_small_angles_approximation."""
x_init = test_helpers.generate_random_test_euler_angles(
min_angle=-0.17, max_angle=0.17)
x = tf.convert_to_tensor(value=x_init)
y = rotation_matrix_3d.from_euler_with_small_angles_approximation(x)
self.assert_jacobian_is_correct(x, x_init, y)
def test_from_rotation_matrix_normalized_random(self):
"""Tests that from_rotation_matrix returns normalized axis-angles."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
matrix = rotation_matrix_3d.from_euler(random_euler_angles)
axis, angle = axis_angle.from_rotation_matrix(matrix)
self.assertAllEqual(
axis_angle.is_normalized(axis, angle), np.ones(angle.shape, dtype=bool))
def test_from_axis_angle_random(self):
"""Checks that Euler angles can be retrieved from an axis-angle."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_matrix = rotation_matrix_3d.from_euler(random_euler_angles)
random_axis, random_angle = axis_angle.from_euler(random_euler_angles)
predicted_matrix = rotation_matrix_3d.from_axis_angle(
random_axis, random_angle)
self.assertAllClose(random_matrix, predicted_matrix, atol=1e-3)
def test_from_quaternion_random(self):
"""Checks that Euler angles can be retrieved from quaternions."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_matrix = rotation_matrix_3d.from_euler(random_euler_angles)
random_quaternion = quaternion.from_rotation_matrix(random_matrix)
predicted_angles = euler.from_quaternion(random_quaternion)
predicted_matrix = rotation_matrix_3d.from_euler(predicted_angles)
self.assertAllClose(random_matrix, predicted_matrix, atol=2e-3)
def test_from_quaternion_random(self):
"""Tests conversion to matrix."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_quaternions = quaternion.from_euler(random_euler_angles)
random_rotation_matrices = rotation_matrix_3d.from_euler(
random_euler_angles)
self.assertAllClose(random_rotation_matrices,
rotation_matrix_3d.from_quaternion(random_quaternions))
def test_from_quaternion_random(self):
"""Tests that axis_angle.from_quaternion produces the expected result."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_quaternions = quaternion.from_euler(random_euler_angles)
random_axis_angle = axis_angle.from_euler(random_euler_angles)
self.assertAllClose(random_axis_angle,
axis_angle.from_quaternion(random_quaternions),
rtol=1e-3)
def test_from_quaternion_gimbal(self, gimbal_configuration):
"""Checks that from_quaternion works when Ry = pi/2 or -pi/2."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_euler_angles[..., 1] = gimbal_configuration
random_quaternion = quaternion.from_euler(random_euler_angles)
random_matrix = rotation_matrix_3d.from_euler(random_euler_angles)
reconstructed_random_matrices = rotation_matrix_3d.from_quaternion(
random_quaternion)
self.assertAllClose(reconstructed_random_matrices, random_matrix, atol=2e-3)
def test_inverse_normalized_random(self):
"""Checks that inverted rotation matrices are valid rotations."""
random_euler_angle = test_helpers.generate_random_test_euler_angles()
tensor_tile = random_euler_angle.shape[:-1]
random_matrix = rotation_matrix_3d.from_euler(random_euler_angle)
predicted_invert_random_matrix = rotation_matrix_3d.inverse(random_matrix)
self.assertAllEqual(
rotation_matrix_3d.is_valid(predicted_invert_random_matrix),
np.ones(tensor_tile + (1,)))
def test_from_axis_angle_random(self):
"""Tests converting an axis-angle to a quaternion."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
axis, angle = axis_angle.from_euler(random_euler_angles)
grountruth = rotation_matrix_3d.from_quaternion(
quaternion.from_euler(random_euler_angles))
prediction = rotation_matrix_3d.from_quaternion(
quaternion.from_axis_angle(axis, angle))
self.assertAllClose(grountruth, prediction, rtol=1e-3)
def test_from_euler_jacobian_random(self):
"""Test the Jacobian of the from_euler function.
Note:
Preset angles are not tested as the gradient of tf.norm is NaN at 0.
"""
x_init = test_helpers.generate_random_test_euler_angles()
self.assert_jacobian_is_finite_fn(lambda x: axis_angle.from_euler(x)[0],
[x_init])
self.assert_jacobian_is_finite_fn(lambda x: axis_angle.from_euler(x)[1],
[x_init])
def test_from_axis_angle_gimbal(self, gimbal_configuration):
"""Checks that from_axis_angle works when Ry = pi/2 or -pi/2."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_euler_angles[..., 1] = gimbal_configuration
random_matrix = rotation_matrix_3d.from_euler(random_euler_angles)
random_axis, random_angle = axis_angle.from_euler(random_euler_angles)
predicted_random_angles = euler.from_axis_angle(random_axis, random_angle)
reconstructed_random_matrices = rotation_matrix_3d.from_euler(
predicted_random_angles)
self.assertAllClose(reconstructed_random_matrices, random_matrix, atol=1e-3)
def test_from_euler_with_small_angles_approximation_random(self):
# Only generate small angles. For a test tolerance of 1e-3, 0.23 was found
# empirically to be the range where the small angle approximation works.
random_euler_angles = test_helpers.generate_random_test_euler_angles(
min_angle=-0.23, max_angle=0.23)
exact_axis_angle = axis_angle.from_euler(random_euler_angles)
approximate_axis_angle = (
axis_angle.from_euler_with_small_angles_approximation(
random_euler_angles))
self.assertAllClose(exact_axis_angle, approximate_axis_angle, atol=1e-3)
def test_rotate_vs_rotate_quaternion_random(self):
"""Tests that the rotate provide the same results as quaternion.rotate."""
random_euler_angle = test_helpers.generate_random_test_euler_angles()
tensor_tile = random_euler_angle.shape[:-1]
random_matrix = rotation_matrix_3d.from_euler(random_euler_angle)
random_quaternion = quaternion.from_rotation_matrix(random_matrix)
random_point = np.random.normal(size=tensor_tile + (3, ))
ground_truth = quaternion.rotate(random_point, random_quaternion)
prediction = rotation_matrix_3d.rotate(random_point, random_matrix)
self.assertAllClose(ground_truth, prediction, rtol=1e-6)
def test_from_euler_random(self):
"""Tests that quaternions can be constructed from Euler angles."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
tensor_shape = random_euler_angles.shape[:-1]
random_matrix = rotation_matrix_3d.from_euler(random_euler_angles)
random_quaternion = quaternion.from_euler(random_euler_angles)
random_point = np.random.normal(size=tensor_shape + (3,))
rotated_with_matrix = rotation_matrix_3d.rotate(random_point, random_matrix)
rotated_with_quaternion = quaternion.rotate(random_point, random_quaternion)
self.assertAllClose(rotated_with_matrix, rotated_with_quaternion)
def test_from_rotation_matrix_random(self):
"""Tests that from_rotation_matrix produces the expected quaternions."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_rotation_matrix_3d = rotation_matrix_3d.from_euler(
random_euler_angles)
groundtruth = rotation_matrix_3d.from_quaternion(
quaternion.from_euler(random_euler_angles))
prediction = rotation_matrix_3d.from_quaternion(
quaternion.from_rotation_matrix(random_rotation_matrix_3d))
self.assertAllClose(groundtruth, prediction)
def test_from_euler_random(self):
"""Tests that from_euler allows to perform the expect rotation of points."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
tensor_shape = random_euler_angles.shape[:-1]
random_point = np.random.normal(size=tensor_shape + (3,))
random_matrix = rotation_matrix_3d.from_euler(random_euler_angles)
random_axis, random_angle = axis_angle.from_euler(random_euler_angles)
rotated_with_matrix = rotation_matrix_3d.rotate(random_point, random_matrix)
rotated_with_axis_angle = axis_angle.rotate(random_point, random_axis,
random_angle)
self.assertAllClose(rotated_with_matrix, rotated_with_axis_angle)
def test_from_euler_with_small_angles_approximation_random(self):
"""Tests small_angles approximation by comparing to exact calculation."""
# Only generate small angles. For a test tolerance of 1e-3, 0.16 was found
# empirically to be the range where the small angle approximation works.
random_euler_angles = test_helpers.generate_random_test_euler_angles(
min_angle=-0.16, max_angle=0.16)
exact_matrix = rotation_matrix_3d.from_euler(random_euler_angles)
approximate_matrix = (
rotation_matrix_3d.from_euler_with_small_angles_approximation(
random_euler_angles))
self.assertAllClose(exact_matrix, approximate_matrix, atol=1e-3)
def test_from_euler_jacobian_random(self):
"""Test the Jacobian of the from_euler function.
Note:
Preset angles are not tested as the gradient of tf.norm is NaN at 0.
"""
x_init = test_helpers.generate_random_test_euler_angles()
x = tf.convert_to_tensor(value=x_init)
y_axis, y_angle = axis_angle.from_euler(x)
self.assert_jacobian_is_finite(x, x_init, y_axis)
self.assert_jacobian_is_finite(x, x_init, y_angle)
def test_inverse_random(self):
"""Checks that inverting rotated points results in no transformation."""
random_euler_angle = test_helpers.generate_random_test_euler_angles()
tensor_tile = random_euler_angle.shape[:-1]
random_matrix = rotation_matrix_3d.from_euler(random_euler_angle)
random_point = np.random.normal(size=tensor_tile + (3,))
rotated_random_points = rotation_matrix_3d.rotate(random_point,
random_matrix)
predicted_invert_random_matrix = rotation_matrix_3d.inverse(random_matrix)
predicted_invert_rotated_random_points = rotation_matrix_3d.rotate(
rotated_random_points, predicted_invert_random_matrix)
self.assertAllClose(
random_point, predicted_invert_rotated_random_points, rtol=1e-6)
def test_is_normalized_random(self):
"""Tests that is_normalized works as intended."""
# Samples normalized axis-angles.
random_euler_angles = test_helpers.generate_random_test_euler_angles()
with self.subTest(name=("is_normalized")):
random_axis, random_angle = axis_angle.from_euler(random_euler_angles)
pred = axis_angle.is_normalized(random_axis, random_angle)
self.assertAllEqual(np.ones(shape=random_angle.shape, dtype=bool), pred)
with self.subTest(name=("is_not_normalized")):
random_axis *= 1.01
pred = axis_angle.is_normalized(random_axis, random_angle)
self.assertAllEqual(np.zeros(shape=random_angle.shape, dtype=bool), pred)
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_euler_random(self):
"""Tests that Euler angles produce the same result as axis-angle."""
angles = test_helpers.generate_random_test_euler_angles()
matrix = rotation_matrix_3d.from_euler(angles)
tensor_tile = angles.shape[:-1]
x_axis = np.tile(td.AXIS_3D_X, tensor_tile + (1,))
y_axis = np.tile(td.AXIS_3D_Y, tensor_tile + (1,))
z_axis = np.tile(td.AXIS_3D_Z, tensor_tile + (1,))
x_angle = np.expand_dims(angles[..., 0], axis=-1)
y_angle = np.expand_dims(angles[..., 1], axis=-1)
z_angle = np.expand_dims(angles[..., 2], axis=-1)
x_rotation = rotation_matrix_3d.from_axis_angle(x_axis, x_angle)
y_rotation = rotation_matrix_3d.from_axis_angle(y_axis, y_angle)
z_rotation = rotation_matrix_3d.from_axis_angle(z_axis, z_angle)
expected_matrix = tf.matmul(z_rotation, tf.matmul(y_rotation, x_rotation))
self.assertAllClose(expected_matrix, matrix, rtol=1e-3)
