Extracción numéricamente estable de ángulo de eje de cuaternión unidad

Question

Estoy buscando extraer de un eje angular de la representación de una unidad de cuaterniones. A partir de la definición, un ingenuo intento podría ser: $ q = \begin{bmatrix} cos(\theta/2) \\ \omega_x \sin(\theta/2) \\ \omega_y \sin(\theta/2) \\ \omega_z \sin(\theta/2)\end{bmatrix} = \begin{bmatrix} w \\ x \\ y \\ z\end{bmatrix}$

Y por lo tanto: $\theta = \operatorname{atan2}(\|\begin{bmatrix}x & y & z\end{bmatrix}\|, w)$

Lo que parece bastante inocente, pero el eje es más problemática: $ \omega = \frac{\begin{bmatrix} x & y & z\end{bmatrix}^T}{\|\begin{bmatrix}x & y & z\end{bmatrix}\|}$

Especialmente si el pecado del ángulo es muy pequeño - Considerar algo cerca de la unidad de cuaterniones, por ejemplo.

Una estrategia podría ser la de convertir el quaternion a una matriz de rotación, y $\omega = \operatorname{nullspace}(R - I)$, logrado a través de un SVD, pero esto parece un terrible desperdicio de cómputo y podría ser numéricamente dudosa como R enfoques I.

Así, hay una numéricamente buena estrategia? Especialmente uno documentada en la literatura, en alguna parte? Incluso Eigen, la que se hace referencia en Esta solución a una pregunta similar, parece comprobar simplemente por un pequeño delta:

/** Set \c *this from a \b unit quaternion.
  * The axis is normalized.
  * 
  * \warning As any other method dealing with quaternion, if the input quaternion
  *          is not normalized then the result is undefined.
  */
template<typename Scalar>
template<typename QuatDerived>
AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q)
{
  using std::acos;
  using std::min;
  using std::max;
  Scalar n2 = q.vec().squaredNorm();
  if (n2 < NumTraits<Scalar>::dummy_precision()*NumTraits<Scalar>::dummy_precision())
  {
    m_angle = 0;
    m_axis << 1, 0, 0;
  }
  else
  {
    m_angle = Scalar(2)*acos((min)((max)(Scalar(-1),q.w()),Scalar(1)));
    m_axis = q.vec() / internal::sqrt(n2);
  }
  return *this;
}

Answer 1

No se puede hacer mejor que la fórmula $$ \omega = \frac{\begin{bmatrix} x & y & z\end{bmatrix}^T}{\|\begin{bmatrix}x & y & z\end{bmatrix}\|}$$ ya que toda la información sobre $\omega$ está codificado en las magnitudes relativas de los $x,y,z$. Suponiendo que estos son almacenados en punto flotante con cada componente teniendo una exactitud relativa $\epsilon$, entonces la fórmula debe ser capaz de recuperarse $\omega$ a la misma exactitud relativa. Todo lo que necesitas es un camino sólido para el cálculo de la norma en el denominador. Véase, por ejemplo, LAPACK del DLAPY3 rutina. Si la norma es muy pequeña, entonces esto corresponde a un muy pequeño el ángulo de rotación, y para que el eje está mal condicionado y arbitraria que corresponde a no-a-la rotación.

En última instancia, es una cuestión de lo que estamos tratando de hacer con el eje de ángulo de par. Si usted necesita el eje de alta precisión absoluta, entonces usted no puede representar mediante una unidad de cuaterniones sin perder exactitud en algunos casos extremos. Esto sólo debería ocurrir si el seno está cerca de subdesbordamiento (dentro de un par de órdenes de magnitud de DBL_MIN; incluso cerca de él, gradual subdesbordamiento aún le ayudará en cierta medida).

Answer 2

Yo también tuve problemas con la estabilidad numérica de la conversión de cuaterniones para el eje y el ángulo. Solía tener infinitos y NaN a veces y cuando traté de añadir un poco de "si" instrucción para determinar, por ejemplo, si un número era demasiado pequeña para un divisor, que acaba de encontrar trabajo en el mal de las situaciones.

Lo que resolvió el problema para mí fue el siguiente fragmento de código. (También se incluye a las otras vías de conversión de eje y ángulo de cuaterniones). Ha sido tomado de aquí. No sé las matemáticas detrás de él, por qué funciona, o si esto solo ocurre para trabajar mejor en mi caso en particular y no general.

// Convert a value in combined axis-angle representation to a quaternion.
// The value angle_axis is a triple whose norm is an angle in radians,
// and whose direction is aligned with the axis of rotation,
// and quaternion is a 4-tuple that will contain the resulting quaternion.
// The implementation may be used with auto-differentiation up to the first
// derivative, higher derivatives may have unexpected results near the origin.

template<typename T>
inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
  const T &a0 = angle_axis[0];
  const T &a1 = angle_axis[1];
  const T &a2 = angle_axis[2];
  const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;

  // For points not at the origin, the full conversion is numerically stable.
  if (theta_squared > T(0.0)) {
    const T theta = sqrt(theta_squared);
    const T half_theta = theta * T(0.5);
    const T k = sin(half_theta) / theta;
    quaternion[0] = cos(half_theta);
    quaternion[1] = a0 * k;
    quaternion[2] = a1 * k;
    quaternion[3] = a2 * k;
  } else {
    // At the origin, sqrt() will produce NaN in the derivative since
    // the argument is zero.  By approximating with a Taylor series,
    // and truncating at one term, the value and first derivatives will be
    // computed correctly when Jets are used.
    const T k(0.5);
    quaternion[0] = T(1.0);
    quaternion[1] = a0 * k;
    quaternion[2] = a1 * k;
    quaternion[3] = a2 * k;
  }
}

// Convert a quaternion to the equivalent combined axis-angle representation.
// The value quaternion must be a unit quaternion - it is not normalized first,
// and angle_axis will be filled with a value whose norm is the angle of
// rotation in radians, and whose direction is the axis of rotation.
// The implemention may be used with auto-differentiation up to the first
// derivative, higher derivatives may have unexpected results near the origin.

template<typename T>
inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
  const T &q1 = quaternion[1];
  const T &q2 = quaternion[2];
  const T &q3 = quaternion[3];
  const T sin_squared = q1 * q1 + q2 * q2 + q3 * q3;

  // For quaternions representing non-zero rotation, the conversion
  // is numerically stable.
  if (sin_squared > T(0.0)) {
    const T sin_theta = sqrt(sin_squared);
    const T k = T(2.0) * atan2(sin_theta, quaternion[0]) / sin_theta;
    angle_axis[0] = q1 * k;
    angle_axis[1] = q2 * k;
    angle_axis[2] = q3 * k;
  } else {
    // For zero rotation, sqrt() will produce NaN in the derivative since
    // the argument is zero.  By approximating with a Taylor series,
    // and truncating at one term, the value and first derivatives will be
    // computed correctly when Jets are used.
    const T k(2.0);
    angle_axis[0] = q1 * k;
    angle_axis[1] = q2 * k;
    angle_axis[2] = q3 * k;
  }
}