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