Numerically stable extraction of Axis-Angle from Unit Quaternion

8.1k Views Asked by At

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