def _safe_dot(vector1, vector2, eps):
"""Calculates dot product while ensuring it is in the range [-1, 1]."""
dot_product = vector.dot(vector1, vector2)
# Safely shrink to make sure machine precision does not cause the dot
# product to be outside the [-1.0, 1.0] range.
return safe_ops.safe_shrink(
vector=dot_product, minval=-1.0, maxval=1.0, open_bounds=False, eps=eps)
def test_safe_shrink_exception_not_raised(self, dtype):
"""Checks whether safe shrinking makes tensor safe for tf.acos(x)."""
tensor = tf.convert_to_tensor(value=_pick_random_vector(), dtype=dtype)
tensor = tensor * tensor
norm_tensor = tensor / tf.reduce_max(
input_tensor=tensor, axis=-1, keepdims=True)
eps = asserts.select_eps_for_addition(dtype)
norm_tensor += eps
safe_tensor = safe_ops.safe_shrink(norm_tensor, -1.0, 1.0)
self.assert_exception_is_not_raised(tf.acos, shapes=[], x=safe_tensor)
def test_safe_shrink_exception_raised(self, dtype):
"""Checks whether safe shrinking fails when eps is zero."""
tensor = tf.convert_to_tensor(value=_pick_random_vector(), dtype=dtype)
tensor = tensor * tensor
norm_tensor = tensor / tf.reduce_max(
input_tensor=tensor, axis=-1, keepdims=True)
eps = asserts.select_eps_for_addition(dtype)
norm_tensor += eps
with self.assertRaises(tf.errors.InvalidArgumentError):
self.evaluate(safe_ops.safe_shrink(norm_tensor, -1.0, 1.0, eps=0.0))
def relative_angle(quaternion1: type_alias.TensorLike,
quaternion2: type_alias.TensorLike,
name: str = "quaternion_relative_angle") -> tf.Tensor:
r"""Computes the unsigned relative rotation angle between 2 unit quaternions.
Given two normalized quanternions $$\mathbf{q}_1$$ and $$\mathbf{q}_2$$, the
relative angle is computed as
$$\theta = 2\arccos(\mathbf{q}_1^T\mathbf{q}_2)$$.
Note:
In the following, A1 to An are optional batch dimensions.
Args:
quaternion1: A tensor of shape `[A1, ..., An, 4]`, where the last dimension
represents a normalized quaternion.
quaternion2: A tensor of shape `[A1, ..., An, 4]`, where the last dimension
represents a normalized quaternion.
name: A name for this op that defaults to "quaternion_relative_angle".
Returns:
A tensor of shape `[A1, ..., An, 1]` where the last dimension represents
rotation angles in the range [0.0, pi].
Raises:
ValueError: If the shape of `quaternion1` or `quaternion2` is not supported.
"""
with tf.name_scope(name):
quaternion1 = tf.convert_to_tensor(value=quaternion1)
quaternion2 = tf.convert_to_tensor(value=quaternion2)
shape.check_static(tensor=quaternion1,
tensor_name="quaternion1",
has_dim_equals=(-1, 4))
shape.check_static(tensor=quaternion2,
tensor_name="quaternion2",
has_dim_equals=(-1, 4))
quaternion1 = asserts.assert_normalized(quaternion1)
quaternion2 = asserts.assert_normalized(quaternion2)
dot_product = vector.dot(quaternion1, quaternion2, keepdims=False)
# Ensure dot product is in range [-1. 1].
eps_dot_prod = 4.0 * asserts.select_eps_for_addition(dot_product.dtype)
dot_product = safe_ops.safe_shrink(dot_product,
-1.0,
1.0,
False,
eps=eps_dot_prod)
return 2.0 * tf.acos(tf.abs(dot_product))
def d_q(q1, q2):
"""Distance between 2 quaternions
The quaternion distance takes values between [0, pi]
Parameters
----------
q1: tf.tensor/np.ndarray
1st quaternion
q2: tf.tensor/np.ndarray
2nd quaternion
Returns
-------
: distnace between these 2 quaternions
"""
q1 = tf.cast(tf.convert_to_tensor(value=q1), dtype=tf.float64)
q2 = tf.cast(tf.convert_to_tensor(value=q2), dtype=tf.float64)
shape.check_static(tensor=q1,
tensor_name="quaternion1",
has_dim_equals=(-1, 4))
shape.check_static(tensor=q2,
tensor_name="quaternion2",
has_dim_equals=(-1, 4))
q1 = quaternion.normalize(q1)
q2 = quaternion.normalize(q2)
dot_product = vector.dot(q1, q2, keepdims=False)
# Ensure dot product is in range [-1. 1].
eps_dot_prod = 1.8 * asserts.select_eps_for_addition(dot_product.dtype)
dot_product = safe_ops.safe_shrink(dot_product,
-1,
1,
open_bounds=False,
eps=eps_dot_prod)
return 2.0 * tf.acos(tf.abs(dot_product))
def from_quaternion(quaternions, name=None):
"""Converts quaternions to Euler angles.
Args:
quaternions: A tensor of shape `[A1, ..., An, 4]`, where the last dimension
represents a normalized quaternion.
name: A name for this op that defaults to "euler_from_quaternion".
Returns:
A tensor of shape `[A1, ..., An, 3]`, where the last dimension represents
the three Euler angles.
"""
def general_case(r00, r10, r21, r22, r20, eps_addition):
"""Handles the general case."""
theta_y = -tf.asin(r20)
sign_cos_theta_y = safe_ops.nonzero_sign(tf.cos(theta_y))
r00 = safe_ops.nonzero_sign(r00) * eps_addition + r00
r22 = safe_ops.nonzero_sign(r22) * eps_addition + r22
theta_z = tf.atan2(r10 * sign_cos_theta_y, r00 * sign_cos_theta_y)
theta_x = tf.atan2(r21 * sign_cos_theta_y, r22 * sign_cos_theta_y)
return tf.stack((theta_x, theta_y, theta_z), axis=-1)
def gimbal_lock(r01, r02, r20, eps_addition):
"""Handles Gimbal locks."""
sign_r20 = safe_ops.nonzero_sign(r20)
r02 = safe_ops.nonzero_sign(r02) * eps_addition + r02
theta_x = tf.atan2(-sign_r20 * r01, -sign_r20 * r02)
theta_y = -sign_r20 * tf.constant(math.pi / 2.0, dtype=r20.dtype)
theta_z = tf.zeros_like(theta_x)
angles = tf.stack((theta_x, theta_y, theta_z), axis=-1)
return angles
with tf.compat.v1.name_scope(name, "euler_from_quaternion", [quaternions]):
quaternions = tf.convert_to_tensor(value=quaternions)
shape.check_static(tensor=quaternions,
tensor_name="quaternions",
has_dim_equals=(-1, 4))
x, y, z, w = tf.unstack(quaternions, axis=-1)
tx = safe_ops.safe_shrink(2.0 * x, -2.0, 2.0, True)
ty = safe_ops.safe_shrink(2.0 * y, -2.0, 2.0, True)
tz = safe_ops.safe_shrink(2.0 * z, -2.0, 2.0, True)
twx = tx * w
twy = ty * w
twz = tz * w
txx = tx * x
txy = ty * x
txz = tz * x
tyy = ty * y
tyz = tz * y
tzz = tz * z
# The following is clipped due to numerical instabilities that can take some
# enties outside the [-1;1] range.
r00 = safe_ops.safe_shrink(1.0 - (tyy + tzz), -1.0, 1.0, True)
r10 = safe_ops.safe_shrink(txy + twz, -1.0, 1.0, True)
r21 = safe_ops.safe_shrink(tyz + twx, -1.0, 1.0, True)
r22 = safe_ops.safe_shrink(1.0 - (txx + tyy), -1.0, 1.0, True)
r20 = safe_ops.safe_shrink(txz - twy, -1.0, 1.0, True)
r01 = safe_ops.safe_shrink(txy - twz, -1.0, 1.0, True)
r02 = safe_ops.safe_shrink(txz + twy, -1.0, 1.0, True)
eps_addition = asserts.select_eps_for_addition(quaternions.dtype)
general_solution = general_case(r00, r10, r21, r22, r20, eps_addition)
gimbal_solution = gimbal_lock(r01, r02, r20, eps_addition)
# The general solution is unstable close to the Gimbal lock, and the gimbal
# solution is not toooff in these cases.
is_gimbal = tf.less(tf.abs(tf.abs(r20) - 1.0), 1.0e-6)
gimbal_mask = tf.stack((is_gimbal, is_gimbal, is_gimbal), axis=-1)
return tf.compat.v1.where(gimbal_mask, gimbal_solution,
general_solution)
def quaternion2euler(quaternions):
""" Convert quaternion to Euler angles
Parameters
----------
quaternions: np.ndarray
The array of shape (N, 4), where 4 is the 1 real part of quaternion and 3 imaginary parts of quaternion.
Returns
-------
angles: np.ndarray
The array of shape (N, 3), where 3 are the 3 anles of rotation around Z-Y-Z axes respectivelly.
"""
def general_case(r02, r12, r20, r21, r22, eps_addition):
"""Handles the general case."""
theta_y = tf.acos(r22)
#sign_sin_theta_y = safe_ops.nonzero_sign(tf.sin(theta_y))
r02 = safe_ops.nonzero_sign(r02) * eps_addition + r02
r22 = safe_ops.nonzero_sign(r22) * eps_addition + r22
theta_z0 = tf.atan2(r12, r02)
theta_z1 = tf.atan2(r21, -r20)
return tf.stack((theta_z1, theta_y, theta_z0), axis=-1)
def gimbal_lock(r22, r11, r10, eps_addition):
"""Handles Gimbal locks.
It is gimbal when r22 is -1 or 1"""
sign_r22 = safe_ops.nonzero_sign(r22)
r11 = safe_ops.nonzero_sign(r11) * eps_addition + r11
theta_z0 = tf.atan2(sign_r22 * r10, r11)
theta_y = tf.constant(math.pi / 2.0,
dtype=r20.dtype) - sign_r22 * tf.constant(
math.pi / 2.0, dtype=r20.dtype)
theta_z1 = tf.zeros_like(theta_z0)
angles = tf.stack((theta_z1, theta_y, theta_z0), axis=-1)
return angles
with tf.compat.v1.name_scope(None, "euler_from_quaternion", [quaternions]):
quaternions = tf.convert_to_tensor(value=quaternions)
shape.check_static(tensor=quaternions,
tensor_name="quaternions",
has_dim_equals=(-1, 4))
x, y, z, w = tf.unstack(quaternions, axis=-1)
tx = safe_ops.safe_shrink(2.0 * x, -2.0, 2.0, True)
ty = safe_ops.safe_shrink(2.0 * y, -2.0, 2.0, True)
tz = safe_ops.safe_shrink(2.0 * z, -2.0, 2.0, True)
twx = tx * w
twy = ty * w
twz = tz * w
txx = tx * x
txy = ty * x
txz = tz * x
tyy = ty * y
tyz = tz * y
tzz = tz * z
# The following is clipped due to numerical instabilities that can take some
# enties outside the [-1;1] range.
r00 = safe_ops.safe_shrink(1.0 - (tyy + tzz), -1.0, 1.0, True)
r01 = safe_ops.safe_shrink(txy - twz, -1.0, 1.0, True)
r02 = safe_ops.safe_shrink(txz + twy, -1.0, 1.0, True)
r10 = safe_ops.safe_shrink(txy + twz, -1.0, 1.0, True)
r11 = safe_ops.safe_shrink(1.0 - (txx + tzz), -1.0, 1.0, True)
r12 = safe_ops.safe_shrink(tyz - twx, -1.0, 1.0, True)
r20 = safe_ops.safe_shrink(txz - twy, -1.0, 1.0, True)
r21 = safe_ops.safe_shrink(tyz + twx, -1.0, 1.0, True)
r22 = safe_ops.safe_shrink(1.0 - (txx + tyy), -1.0, 1.0, True)
eps_addition = asserts.select_eps_for_addition(quaternions.dtype)
general_solution = general_case(r02, r12, r20, r21, r22, eps_addition)
gimbal_solution = gimbal_lock(r22, r11, r10, eps_addition)
# The general solution is unstable close to the Gimbal lock, and the gimbal
# solution is not toooff in these cases.
# Check if r22 is 1 or -1
is_gimbal = tf.less(tf.abs(tf.abs(r22) - 1.0), 1.0e-6)
gimbal_mask = tf.stack((is_gimbal, is_gimbal, is_gimbal), axis=-1)
return tf.compat.v1.where(gimbal_mask, gimbal_solution,
general_solution)
