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 from_axis_angle_translation( axis: type_alias.TensorLike, angle: type_alias.TensorLike, translation_vector: type_alias.TensorLike, name: str = "dual_quat_from_axis_angle_trans" ) -> type_alias.TensorLike: """Converts an axis-angle rotation and translation to a dual quaternion. Note: In the following, A1 to An are optional batch dimensions. Args: axis: A tensor of shape `[A1, ..., An, 3]`, where the last dimension represents a normalized axis. angle: A tensor of shape `[A1, ..., An, 1]`, where the last dimension represents an angle. translation_vector: A `[A1, ..., An, 3]`-tensor, where the last dimension represents a translation vector. name: A name for this op that defaults to "dual_quat_from_axis_angle_trans". Returns: A `[A1, ..., An, 8]`-tensor, where the last dimension represents a normalized dual quaternion. Raises: ValueError: If the shape of `axis`, `angle`, or `translation_vector` is not supported. """ with tf.name_scope(name): axis = tf.convert_to_tensor(value=axis) angle = tf.convert_to_tensor(value=angle) translation_vector = tf.convert_to_tensor(value=translation_vector) shape.check_static(tensor=axis, tensor_name="axis", has_dim_equals=(-1, 3)) shape.check_static(tensor=angle, tensor_name="angle", has_dim_equals=(-1, 1)) shape.check_static(tensor=translation_vector, tensor_name="translation_vector", has_dim_equals=(-1, 3)) shape.compare_batch_dimensions(tensors=(axis, angle, translation_vector), last_axes=-2, broadcast_compatible=True) scalar_shape = tf.concat((tf.shape(translation_vector)[:-1], (1, )), axis=-1) dtype = translation_vector.dtype quaternion_rotation = quaternion.from_axis_angle(axis, angle) quaternion_translation = tf.concat( (translation_vector, tf.zeros(scalar_shape, dtype)), axis=-1) dual_quaternion_dual_part = 0.5 * quaternion.multiply( quaternion_translation, quaternion_rotation) return tf.concat((quaternion_rotation, dual_quaternion_dual_part), axis=-1)
def test_from_axis_angle_normalized_random(self): """Test that from_axis_angle produces normalized quaternions.""" random_axis, random_angle = test_helpers.generate_random_test_axis_angle() random_quaternion = quaternion.from_axis_angle(random_axis, random_angle) self.assertAllEqual( quaternion.is_normalized(random_quaternion), np.ones(shape=random_angle.shape, dtype=bool))
def test_between_two_vectors_3d_that_are_collinear(self): """Checks the extracted rotation between two collinear 3d vectors.""" axis = [(1.0, 0.0, 0.0), (0.0, 1.0, 0.0)] antiparallel_axis = [(0.0, 1.0, 0.0), (0.0, 0.0, 1.0)] source = np.multiply(axis, 10.) target = np.multiply(axis, -10.) rotation = quaternion.between_two_vectors_3d(source, target) rotation_pi = quaternion.from_axis_angle(antiparallel_axis, [[np.pi], [np.pi]]) self.assertAllClose(rotation_pi, rotation)
def test_from_axis_angle_jacobian_random(self): """Test the Jacobian of the from_axis_angle function.""" x_axis_init, x_angle_init = test_helpers.generate_random_test_axis_angle() x_axis = tf.convert_to_tensor(value=x_axis_init) x_angle = tf.convert_to_tensor(value=x_angle_init) y = quaternion.from_axis_angle(x_axis, x_angle) self.assert_jacobian_is_correct(x_axis, x_axis_init, y) self.assert_jacobian_is_correct(x_angle, x_angle_init, y)
def test_rotate_random(self): """Tests that the rotate provide the same results as quaternion.rotate.""" random_axis, random_angle = test_helpers.generate_random_test_axis_angle() tensor_shape = random_angle.shape[:-1] random_point = np.random.normal(size=tensor_shape + (3,)) random_quaternion = quaternion.from_axis_angle(random_axis, random_angle) ground_truth = quaternion.rotate(random_point, random_quaternion) prediction = axis_angle.rotate(random_point, random_axis, random_angle) self.assertAllClose(ground_truth, prediction, rtol=1e-6)
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_axis_angle_random(self): """Tests conversion to matrix.""" tensor_shape = np.random.randint(1, 10, size=np.random.randint(3)).tolist() random_axis = np.random.normal(size=tensor_shape + [3]) random_axis /= np.linalg.norm(random_axis, axis=-1, keepdims=True) random_angle = np.random.normal(size=tensor_shape + [1]) matrix_axis_angle = rotation_matrix_3d.from_axis_angle( random_axis, random_angle) random_quaternion = quaternion.from_axis_angle(random_axis, random_angle) matrix_quaternion = rotation_matrix_3d.from_quaternion(random_quaternion) self.assertAllClose(matrix_axis_angle, matrix_quaternion, rtol=1e-3) # Checks that resulting rotation matrices are normalized. self.assertAllEqual( rotation_matrix_3d.is_valid(matrix_axis_angle), np.ones(tensor_shape + [1]))
def from_axis_angle(axis, angle, name=None): """Converts axis-angle to Euler angles. Note: In the following, A1 to An are optional batch dimensions. Args: axis: A tensor of shape `[A1, ..., An, 3]`, where the last dimension represents a normalized axis. angle: A tensor of shape `[A1, ..., An, 1]`, where the last dimension represents an angle. name: A name for this op that defaults to "euler_from_axis_angle". Returns: A tensor of shape `[A1, ..., An, 3]`, where the last dimension represents the three Euler angles. """ with tf.compat.v1.name_scope(name, "euler_from_axis_angle", [axis, angle]): return from_quaternion(quaternion.from_axis_angle(axis, angle))