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
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
def general_case(rotation_matrix, 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 = rotation_matrix[..., 0, 0]
r10 = rotation_matrix[..., 1, 0]
r21 = rotation_matrix[..., 2, 1]
r22 = rotation_matrix[..., 2, 2]
r00 = safe_ops.nonzero_sign(r00) * eps_addition + r00
r22 = safe_ops.nonzero_sign(r22) * eps_addition + r22
# cos_theta_y evaluates to 0 on Gimbal locks, in which case the output of
# this function will not be used.
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)
angles = tf.stack((theta_x, theta_y, theta_z), axis=-1)
return angles
def from_quaternion(quaternion, name=None):
"""Converts a quaternion to an axis-angle representation.
Note:
In the following, A1 to An are optional batch dimensions.
Args:
quaternion: A tensor of shape `[A1, ..., An, 4]`, where the last dimension
represents a normalized quaternion.
name: A name for this op that defaults to "axis_angle_from_quaternion".
Returns:
Tuple of two tensors 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 `quaternion` is not supported.
"""
with tf.compat.v1.name_scope(name, "axis_angle_from_quaternion",
[quaternion]):
quaternion = tf.convert_to_tensor(value=quaternion)
shape.check_static(
tensor=quaternion, tensor_name="quaternion", has_dim_equals=(-1, 4))
quaternion = asserts.assert_normalized(quaternion)
# This prevents zero norm xyz and zero w, and is differentiable.
quaternion += asserts.select_eps_for_addition(quaternion.dtype)
xyz, w = tf.split(quaternion, (3, 1), axis=-1)
norm = tf.norm(tensor=xyz, axis=-1, keepdims=True)
angle = 2.0 * tf.atan2(norm, tf.abs(w))
axis = safe_ops.safe_unsigned_div(safe_ops.nonzero_sign(w) * xyz, norm)
return axis, angle
def quaternion_weights(
quaternion1: type_alias.TensorLike,
quaternion2: type_alias.TensorLike,
percent: Union[type_alias.Float, type_alias.TensorLike],
eps: Optional[type_alias.Float] = None,
name: str = "quaternion_weights") -> Tuple[tf.Tensor, tf.Tensor]:
"""Calculates slerp weights for two normalized quaternions.
Given a percent and two normalized quaternions, this function returns the
slerp weights. It can also produce extrapolation weights when percent is
outside of the [0, 1] range. It reduces to lerp when input quaternions are
almost parallel or anti-parallel. Input quaternions are assumed to be
normalized. The tf.graphics debug flag TFG_ADD_ASSERTS_TO_GRAPH defined
in tfg_flags.py can be set to add assertions to the graph that check whether
the inputs are normalized, and whether Inf or Nan values are produced.
Note:
In the following, A1 to An are optional batch dimensions.
Args:
quaternion1: A tensor of shape `[A1, ... , An, 4]` storing normalized
quaternions in its last dimension.
quaternion2: A tensor of shape `[A1, ... , An, 4]` storing normalized
quaternions in its last dimension.
percent: A `float` or a tensor with a shape broadcastable to the shape `[A1,
... , An]`.
eps: A `float` used to make operations safe. When left as None, the function
automatically picks the best epsilon based on the dtype and the operation.
name: A name for this op. Defaults to "quaternion_weights".
Raises:
ValueError: If the shapes of quaternions do not match, if the last
dimensions of quaternions are not 4, or if percent is neither a float, nor
a tensor with last dimension 1.
Returns:
Two tensors of shape `[A1, ... , An, 1]` each, which are the two slerp
weights for each quaternion.
"""
with tf.name_scope(name):
quaternion1 = tf.convert_to_tensor(value=quaternion1)
quaternion2 = tf.convert_to_tensor(value=quaternion2)
percent = tf.convert_to_tensor(value=percent, dtype=quaternion1.dtype)
if percent.shape.ndims == 0:
percent = tf.expand_dims(percent, axis=0)
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))
shape.compare_batch_dimensions(
tensors=(quaternion1, quaternion2, percent),
last_axes=(-2, -2, -1),
broadcast_compatible=True,
tensor_names=("quaternion1", "quaternion2", "percent"))
quaternion1 = asserts.assert_normalized(quaternion1)
quaternion2 = asserts.assert_normalized(quaternion2)
dot_product = _safe_dot(quaternion1, quaternion2, eps)
# Take the shorter path
theta = tf.acos(tf.abs(dot_product))
# safe_sinpx_div_sinx returns p for very small x, which means slerp reduces
# to lerp automatically.
scale1 = safe_ops.safe_sinpx_div_sinx(theta, 1.0 - percent, eps)
scale2 = safe_ops.safe_sinpx_div_sinx(theta, percent, eps)
# Flip the sign of scale1 if quaternions are in different hemispheres.
# tf.sign can make scale1 zero if quaternions are orthogonal.
scale1 *= safe_ops.nonzero_sign(dot_product)
return scale1, scale2
