旋转向量与旋转矩阵的相互转化

旋转向量\(\theta a\)，旋转矩阵R
\(W=[\theta a]_{\times}\)
\(d =|\theta a|\)
\(R = I + \frac{W sin(d)}{d} + \frac{W^2 (1-cos(d))}{d^2}\)

Eigen::Matrix3d ExpSO3(const double x, const double y, const double z) {
  const double d2 = x * x + y * y + z * z;
  const double d = sqrt(d2);
  Eigen::Matrix3d W;
  W << 0.0, -z, y, z, 0.0, -x, -y, x, 0.0;
  if (d < 1e-5) {
    Eigen::Matrix3d res = Eigen::Matrix3d::Identity() + W + 0.5 * W * W;
    return NormalizeRotation(res);
  } else {
    Eigen::Matrix3d res = Eigen::Matrix3d::Identity() + W * sin(d) / d + W * W * (1.0 - cos(d)) / d2;
    return NormalizeRotation(res);
  }
}

\(\theta =arccos( \frac{tr - 1}{2})\)
\(a =\frac{R-R^T}{sin(\theta)}\)

Eigen::Vector3d LogSO3(const Eigen::Matrix3d& R) {
  const double tr = R(0, 0) + R(1, 1) + R(2, 2);
  Eigen::Vector3d w;
  w << (R(2, 1) - R(1, 2)) / 2, (R(0, 2) - R(2, 0)) / 2, (R(1, 0) - R(0, 1)) / 2;
  const double costheta = (tr - 1.0) * 0.5f;
  if (costheta > 1 || costheta < -1)  // 说明R不是旋转矩阵，抛出异常
    return w;
  const double theta = acos(costheta);
  const double s = sin(theta);
  if (fabs(s) < 1e-5)
    return w;
  else
    return theta * w / s;
}