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))
