def test_from_rotation_matrix_jacobian_random(self):
"""Test the Jacobian of the from_rotation_matrix function.
Note:
Preset angles are not tested as the gradient of tf.norm is NaN a 0.
"""
x_init = test_helpers.generate_random_test_rotation_matrix_3d()
self.assert_jacobian_is_finite_fn(
lambda x: axis_angle.from_rotation_matrix(x)[0], [x_init])
self.assert_jacobian_is_finite_fn(
lambda x: axis_angle.from_rotation_matrix(x)[1], [x_init])
def test_from_rotation_matrix_random(self):
"""Tests rotation around Z axis."""
def get_rotation_matrix_around_z(angle_rad):
return np.array([
[np.cos(angle_rad), -np.sin(angle_rad), 0],
[np.sin(angle_rad), np.cos(angle_rad), 0],
[0, 0, 1],
])
tensor_size = np.random.randint(10)
angle = (
np.array([
np.deg2rad(np.random.randint(720) - 360) for _ in range(tensor_size)
]).reshape((tensor_size, 1)))
rotation_matrix = [get_rotation_matrix_around_z(i[0]) for i in angle]
rotation_matrix = np.array(rotation_matrix).reshape((tensor_size, 3, 3))
tf_axis, tf_angle = axis_angle.from_rotation_matrix(rotation_matrix)
axis = np.tile([[0., 0., 1.]], (angle.shape[0], 1))
tf_quat_gt = quaternion.from_axis_angle(axis, angle)
tf_quat = quaternion.from_axis_angle(tf_axis, tf_angle)
# Compare quaternions since axis orientation and angle ambiguity will
# lead to more complex comparisons.
for quat_gt, quat in zip(self.evaluate(tf_quat_gt), self.evaluate(tf_quat)):
# Remember that q=-q for any quaternion.
pos = np.allclose(quat_gt, quat)
neg = np.allclose(quat_gt, -quat)
self.assertTrue(pos or neg)
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 half_rotation(rotation):
"""Return half of the input rotation.
Args:
rotation: [BATCH, 3, 3] rotation matrices.
Returns:
[BATCH, 3, 3] rotation matrices.
"""
axes, angles = axis_angle.from_rotation_matrix(rotation)
return rotation_matrix_3d.from_axis_angle(axes, angles / 2)
def test_from_rotation_matrix_jacobian_random(self):
"""Test the Jacobian of the from_rotation_matrix function.
Note:
Preset angles are not tested as the gradient of tf.norm is NaN a 0.
"""
x_init = test_helpers.generate_random_test_rotation_matrix_3d()
x = tf.convert_to_tensor(value=x_init)
y_axis, y_angle = axis_angle.from_rotation_matrix(x)
self.assert_jacobian_is_finite(x, x_init, y_axis)
self.assert_jacobian_is_finite(x, x_init, y_angle)
