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

旋转向量 θ a \theta a θa,旋转矩阵R
W = [ θ a ] × W=[\theta a]_{\times} W=[θa]×
d = ∣ θ a ∣ d =|\theta a| d=θa
R = I + W s i n ( d ) d + W 2 ( 1 − c o s ( d ) ) d 2 R = I + \frac{W sin(d)}{d} + \frac{W^2 (1-cos(d))}{d^2} R=I+dWsin(d)+d2W2(1cos(d))

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

θ = a r c c o s ( t r − 1 2 ) \theta =arccos( \frac{tr - 1}{2}) θ=arccos(2tr1)
a = R − R T s i n ( θ ) a =\frac{R-R^T}{sin(\theta)} a=sin(θ)RRT

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

你可能感兴趣的:(线性代数,算法)