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 = quaternion.from_rotation_matrix(x)
self.assert_jacobian_is_finite(x, x_init, y)
def test_from_rotation_matrix_normalized_random(self):
"""Tests that from_rotation_matrix produces normalized quaternions."""
random_matrix = test_helpers.generate_random_test_rotation_matrix_3d()
random_quaternion = quaternion.from_rotation_matrix(random_matrix)
self.assertAllEqual(
quaternion.is_normalized(random_quaternion),
np.ones(shape=random_matrix.shape[:-2] + (1,), dtype=bool))
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_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_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_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 = quaternion.from_rotation_matrix(x)
self.assert_jacobian_is_finite(x, x_init, y)
def read_meshes(mesh_folder):
mesh_vertices_list = []
for it_mesh_file in linemod_cls_names:
it_mesh_file_full = os.path.join(mesh_folder, it_mesh_file + '_color_4000.ply')
vertices_model = read_ply_file(it_mesh_file_full)
vertices_model = tf.convert_to_tensor(vertices_model, dtype=tf.float32)
init_rot_quat = tf.cast(quaternion.from_rotation_matrix(R_init), tf.float32)
vertices_model = quaternion.rotate(vertices_model, init_rot_quat)
vertices_model = tf.expand_dims(vertices_model, axis=0)
mesh_vertices_list.append(vertices_model)
return tf.concat(mesh_vertices_list, axis=0)
def from_rotation_matrix(rotation_matrix: type_alias.TensorLike,
name: str = "axis_angle_from_rotation_matrix"
) -> Tuple[tf.Tensor, tf.Tensor]:
"""Converts a rotation matrix to an axis-angle representation.
Note:
In the current version the returned axis-angle representation is not unique
for a given rotation matrix. Since a direct conversion would not really be
faster, we first transform the rotation matrix to a quaternion, and finally
perform the conversion from that quaternion to the corresponding axis-angle
representation.
Note:
In the following, A1 to An are optional batch dimensions.
Args:
rotation_matrix: A tensor of shape `[A1, ..., An, 3, 3]`, where the last two
dimensions represent a rotation matrix.
name: A name for this op that defaults to "axis_angle_from_rotation_matrix".
Returns:
A tuple of two tensors, respectively of shape `[A1, ..., An, 3]` and
`[A1, ..., An, 1]`, where the first tensor represents the axis, and the
second represents the angle. The resulting axis is a normalized vector.
Raises:
ValueError: If the shape of `rotation_matrix` is not supported.
"""
with tf.name_scope(name):
rotation_matrix = tf.convert_to_tensor(value=rotation_matrix)
shape.check_static(
tensor=rotation_matrix,
tensor_name="rotation_matrix",
has_rank_greater_than=1,
has_dim_equals=((-2, 3), (-1, 3)))
rotation_matrix = rotation_matrix_3d.assert_rotation_matrix_normalized(
rotation_matrix)
quaternion = quaternion_lib.from_rotation_matrix(rotation_matrix)
return from_quaternion(quaternion)
