Take the 2-minute tour ×
Mathematics Stack Exchange is a question and answer site for people studying math at any level and professionals in related fields. It's 100% free, no registration required.

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;
}
share|improve this question
    
In the formula with $q$ there are only three compenents $[x,y,z]^T$, is that right? –  Maesumi Jan 31 '13 at 4:36
    
    
@Maesumi, I've fixed the problem you've pointed to. Thanks. –  Damien Jan 31 '13 at 5:37
    
@WillJagy, with your link, I don't want to rotate a vector. I want to extract the axis of rotation. I am after a version of the closed-form formula that I can implement in software that is numerically stable as $\theta$ approaches $0$. –  Damien Jan 31 '13 at 5:39
1  
Have a look at logAndTheta here: github.com/strasdat/Sophus/blob/develop/sophus/so3.hpp. –  Hauke Strasdat Jul 23 '13 at 7:50

1 Answer 1

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;
  }
}
share|improve this answer

Your Answer

 
discard

By posting your answer, you agree to the privacy policy and terms of service.

Not the answer you're looking for? Browse other questions tagged or ask your own question.