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)