Numerically stable extraction of Axis-Angle from Unit Quaternion

Question

I am looking to extract an axis-angle representation from a unit quaternion. From the definition, a naive attempt might be: $ 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}$

And therefore: $\theta = \operatorname{atan2}(\|\begin{bmatrix}x & y & z\end{bmatrix}\|, w)$

Which seems innocent enough, but the axis is more problematic: $ \omega = \frac{\begin{bmatrix} x & y & z\end{bmatrix}^T}{\|\begin{bmatrix}x & y & z\end{bmatrix}\|}$

Especially if the sin of the angle is very small - Consider something close to the unit quaternion, for example.

One strategy might be to convert the quaternion to a rotation matrix, and $\omega = \operatorname{nullspace}(R - I)$, accomplished via an SVD, but this seems a terrible waste of computation - and might be numerically dubious as R approaches I.

So, is there a numerically sound strategy? Especially one documented in literature somewhere? Even Eigen, which is referenced in This solution to a similar question, seems to simply check for a small 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

You cannot do better than the formula $$ \omega = \frac{\begin{bmatrix} x & y & z\end{bmatrix}^T}{\|\begin{bmatrix}x & y & z\end{bmatrix}\|}$$ since all the information about $\omega$ is encoded in the relative magnitudes of $x,y,z$. Assuming these are stored in floating point with each component having a relative accuracy $\epsilon$, then the formula should be able to recover $\omega$ to the same relative accuracy. All you need is a robust way to compute the norm in the denominator. See for example LAPACK's DLAPY3 routine. If the norm is exceedingly small, then this corresponds to an exceedingly small rotation angle, and so the axis is ill-conditioned and essentially arbitrary corresponding to not-a-rotation.

Ultimately, it's a question of what you're trying to do with the axis-angle pair. If you need the axis to high absolute accuracy, then you can't represent it using a unit quaternion without losing accuracy in some extreme cases. This should only happen if the sine is near underflow (within a few orders of magnitude of DBL_MIN; even near it, gradual underflow will still help you to some extent).

Answer 2

The standard mathematical equation to convert a quaternion into axis-angle form is numerically unstable when the vector part of the quaternion is small. The correct way to handle this calculation is to use the Taylor expansion of the equation when the vector part of the quaternion is small.

This numerically-stable approach is presented clearly in Neil Dantam's course notes for Georgia Tech. See equations 50-57. http://www.neil.dantam.name/note/dantam-quaternion.pdf

Answer 3

I also had problems with the numerical stability of converting quaternion to axis and angle. I used to get infinities and NaN's sometimes and when I tried to add some "if" statement to determine e.g. whether some number was too small for a divisor, it would just work in wrong situations.

What solved the problem for me was the following piece of code. (It also includes the other way conversion from axis and angle to quaternion). It has been taken from here. I don't know the math behind it, why it works, or if it just happens to work better in my particular case and not generally.

// 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;
  }
}