旋转轴、旋转矩阵、四元数之间相互转换

1. 旋转轴To四元数

假设某个旋转是绕单位向量$n=[n_x,n_y,n_z]$进行了角度为$\theta$的旋转，那么这个旋转的四元数形式为:
$q=[cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2}]$
对应的代码为：

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.
    // 这里面相当于$\theta$ --> 0, 这q_0 = 1.0
    // 当$\theta$ --> 0时，n_x*sin(theta/2) = n_x * theta / 2 = a0 / 2
    const T k(0.5);
    quaternion[0] = T(1.0);
    quaternion[1] = a0 * k;
    quaternion[2] = a1 * k;
    quaternion[3] = a2 * k;
  }
}

2. 四元数To旋转轴

当单位向量四元数: $q=[q_0,q_1,q_2,q_3]$，则
$\theta =2\arccos (q_0)$
$[n_x,n_y,n_z]=[q_1,q_2,q_3]/sin\frac{\theta }{2}$
示例代码如下:

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_theta = q1 * q1 + q2 * q2 + q3 * q3;

  // For quaternions representing non-zero rotation, the conversion
  // is numerically stable.
  if (sin_squared_theta > T(0.0)) {
    const T sin_theta = sqrt(sin_squared_theta);
    const T& cos_theta = quaternion[0];

    // If cos_theta is negative, theta is greater than pi/2, which
    // means that angle for the angle_axis vector which is 2 * theta
    // would be greater than pi.
    //
    // While this will result in the correct rotation, it does not
    // result in a normalized angle-axis vector.
    //
    // In that case we observe that 2 * theta ~ 2 * theta - 2 * pi,
    // which is equivalent saying
    //
    //   theta - pi = atan(sin(theta - pi), cos(theta - pi))
    //              = atan(-sin(theta), -cos(theta))
    //
    // 这个主要是旋转轴的theta角范围是-pi~pi，theta/2的范围要求是-pi/2~pi/2;
    // C++中atan2(y,x)的范围是-pi~pi，sin_theta>0，则atan2(sin_theta,cos_theta)的范围是0~pi,
    // 当限定cos_theta>0时，则范围是0~pi/2; 当限定cos_theta<0时，atan2(-sin_theta,-cos_theta)的范围为-pi/2~0; 
    // (-sin_theta)/(-cos_theta) = sin_theta/cos_theta
    const T two_theta =
        T(2.0) * ((cos_theta < T(0.0)) ? atan2(-sin_theta, -cos_theta)
                                       : atan2(sin_theta, cos_theta));
    const T k = two_theta / 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.
    // 这个是因为当theta-->0时，theta/sin(theta/2)=2
    const T k(2.0);
    angle_axis[0] = q1 * k;
    angle_axis[1] = q2 * k;
    angle_axis[2] = q3 * k;
  }
}

参考

1. 参考文件：/usr/local/include/ceres/rotation.h
2. 视觉SLAM十四讲：从理论到实践