def test_jacobian_relative_angle_random(self):
"""Test the Jacobian of the relative_angle function."""
tensor_dimensions = np.random.randint(low=1, high=3)
tensor_shape = np.random.randint(1, 10, size=(tensor_dimensions)).tolist()
x_1_init = test_helpers.generate_random_test_quaternions(tensor_shape)
x_2_init = test_helpers.generate_random_test_quaternions(tensor_shape)
self.assert_jacobian_is_correct_fn(quaternion.relative_angle,
[x_1_init, x_2_init])
def test_jacobian_relative_angle_random(self):
"""Test the Jacobian of the relative_angle function."""
tensor_dimensions = np.random.randint(low=1, high=3)
tensor_shape = np.random.randint(1, 10, size=(tensor_dimensions)).tolist()
x_1_init = test_helpers.generate_random_test_quaternions(tensor_shape)
x_2_init = test_helpers.generate_random_test_quaternions(tensor_shape)
x_1 = tf.convert_to_tensor(value=x_1_init)
x_2 = tf.convert_to_tensor(value=x_2_init)
y = quaternion.relative_angle(x_1, x_2)
self.assert_jacobian_is_correct(x_1, x_1_init, y)
self.assert_jacobian_is_correct(x_2, x_2_init, y)
def test_valid_relative_angle_random(self):
"""Test the output is in valid range for relative_angle function."""
tensor_dimensions = np.random.randint(low=1, high=3)
tensor_shape = np.random.randint(1, 10, size=(tensor_dimensions)).tolist()
x_1_init = test_helpers.generate_random_test_quaternions(tensor_shape)
x_2_init = test_helpers.generate_random_test_quaternions(tensor_shape)
x_1 = tf.convert_to_tensor(value=x_1_init)
x_2 = tf.convert_to_tensor(value=x_2_init)
y = quaternion.relative_angle(x_1, x_2)
self.assertAllGreaterEqual(y, 0.0)
self.assertAllLessEqual(y, np.pi)
def test_inverse_jacobian_random(self):
"""Test the Jacobian of the inverse function."""
x_init = test_helpers.generate_random_test_quaternions()
x = tf.convert_to_tensor(value=x_init)
y = quaternion.inverse(x)
self.assert_jacobian_is_correct(x, x_init, y)
def test_rotate_jacobian_random(self):
"""Test the Jacobian of the rotate function."""
x_matrix_init = test_helpers.generate_random_test_quaternions()
tensor_shape = x_matrix_init.shape[:-1] + (3,)
x_point_init = np.random.uniform(size=tensor_shape)
self.assert_jacobian_is_correct_fn(quaternion.rotate,
[x_point_init, x_matrix_init])
def test_from_quaternion_jacobian_random(self):
"""Test the Jacobian of the from_quaternion function."""
x_init = test_helpers.generate_random_test_quaternions()
x = tf.convert_to_tensor(value=x_init)
y = rotation_matrix_3d.from_quaternion(x)
self.assert_jacobian_is_correct(x, x_init, y)
def test_inverse_normalized_random(self):
"""Tests that the inverse function returns normalized quaternions."""
random_quaternion = test_helpers.generate_random_test_quaternions()
inverse_quaternion = quaternion.inverse(random_quaternion)
self.assertAllEqual(
quaternion.is_normalized(inverse_quaternion),
np.ones(shape=random_quaternion.shape[:-1] + (1,), dtype=bool))
def test_from_quaternion_normalized_random(self):
"""Tests that from_quaternion returns normalized axis-angles."""
random_quaternions = test_helpers.generate_random_test_quaternions()
random_axis, random_angle = axis_angle.from_quaternion(random_quaternions)
self.assertAllEqual(
axis_angle.is_normalized(random_axis, random_angle),
np.ones(random_angle.shape))
def test_from_quaternion_normalized_random(self):
"""Tests that random quaternions can be converted to rotation matrices."""
random_quaternion = test_helpers.generate_random_test_quaternions()
tensor_shape = random_quaternion.shape[:-1]
random_matrix = rotation_matrix_3d.from_quaternion(random_quaternion)
self.assertAllEqual(rotation_matrix_3d.is_valid(random_matrix),
np.ones(tensor_shape + (1, )))
def test_normalize_jacobian_random(self):
"""Test the Jacobian of the normalize function."""
tensor_dimensions = np.random.randint(low=1, high=3)
tensor_shape = np.random.randint(1, 10, size=(tensor_dimensions)).tolist()
x_init = test_helpers.generate_random_test_quaternions(tensor_shape)
x = tf.convert_to_tensor(value=x_init)
y = quaternion.normalize(x)
self.assert_jacobian_is_correct(x, x_init, y)
def test_from_quaternion_jacobian_random(self):
"""Test the Jacobian of the from_quaternion function.
Note:
Preset angles are not tested as the gradient of tf.norm is NaN a 0.
"""
x_init = test_helpers.generate_random_test_quaternions()
self.assert_jacobian_is_finite_fn(
lambda x: axis_angle.from_quaternion(x)[0], [x_init])
self.assert_jacobian_is_finite_fn(
lambda x: axis_angle.from_quaternion(x)[1], [x_init])
def test_rotate_jacobian_random(self):
"""Test the Jacobian of the rotate function."""
x_matrix_init = test_helpers.generate_random_test_quaternions()
x_matrix = tf.convert_to_tensor(value=x_matrix_init)
tensor_shape = x_matrix_init.shape[:-1] + (3,)
x_point_init = np.random.uniform(size=tensor_shape)
x_point = tf.convert_to_tensor(value=x_point_init)
y = quaternion.rotate(x_point, x_matrix)
self.assert_jacobian_is_correct(x_matrix, x_matrix_init, y)
self.assert_jacobian_is_correct(x_point, x_point_init, y)
def test_inverse_random(self):
"""Tests that multiplying with the inverse gives identity."""
random_quaternion = test_helpers.generate_random_test_quaternions()
inverse_quaternion = quaternion.inverse(random_quaternion)
final_quaternion = quaternion.multiply(random_quaternion,
inverse_quaternion)
tensor_shape = random_quaternion.shape[:-1]
identity_quaternion = np.array((0.0, 0.0, 0.0, 1.0), dtype=np.float32)
identity_quaternion = np.tile(identity_quaternion, tensor_shape + (1,))
self.assertAllClose(final_quaternion, identity_quaternion, rtol=1e-3)
def test_from_quaternion_jacobian_random(self):
"""Test the Jacobian of the from_quaternion function.
Note:
Preset angles are not tested as the gradient of tf.norm is NaN a 0.
"""
x_init = test_helpers.generate_random_test_quaternions()
x = tf.convert_to_tensor(value=x_init)
y_axis, y_angle = axis_angle.from_quaternion(x)
self.assert_jacobian_is_finite(x, x_init, y_axis)
self.assert_jacobian_is_finite(x, x_init, y_angle)
def test_rotate_random(self):
"""Tests the rotation using a quaternion vs a rotation matrix."""
random_quaternion = test_helpers.generate_random_test_quaternions()
tensor_shape = random_quaternion.shape[:-1]
random_point = np.random.normal(size=tensor_shape + (3,))
rotated_point_quaternion = quaternion.rotate(random_point,
random_quaternion)
matrix = rotation_matrix_3d.from_quaternion(random_quaternion)
rotated_point_matrix = rotation_matrix_3d.rotate(random_point, matrix)
self.assertAllClose(
rotated_point_matrix, rotated_point_quaternion, rtol=1e-3)
def test_is_normalized_random(self):
"""Tests that is_normalized works as intended."""
random_quaternion = test_helpers.generate_random_test_quaternions()
tensor_shape = random_quaternion.shape[:-1]
unnormalized_random_quaternion = random_quaternion * 1.01
quat = np.concatenate((random_quaternion, unnormalized_random_quaternion),
axis=0)
mask = np.concatenate(
(np.ones(shape=tensor_shape + (1,),
dtype=bool), np.zeros(shape=tensor_shape + (1,), dtype=bool)),
axis=0)
is_normalized = quaternion.is_normalized(quat)
self.assertAllEqual(mask, is_normalized)
def test_from_quaternion_jacobian_random(self):
"""Test the Jacobian of the from_quaternion function."""
x_init = test_helpers.generate_random_test_quaternions()
self.assert_jacobian_is_correct_fn(rotation_matrix_3d.from_quaternion,
[x_init])
def test_from_quaternion_jacobian_random(self):
"""Test the Jacobian of the from_quaternion function."""
x_init = test_helpers.generate_random_test_quaternions()
self.assert_jacobian_is_finite_fn(euler.from_quaternion, [x_init])
def test_inverse_jacobian_random(self):
"""Test the Jacobian of the inverse function."""
x_init = test_helpers.generate_random_test_quaternions()
self.assert_jacobian_is_correct_fn(quaternion.inverse, [x_init])
