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

旋转向量$\theta a$，旋转矩阵R
W=[θa]×W=[\\theta a]_{\\times}W=[θa]×​
$d=|\theta a|$
R=I+Wsin(d)d+W2(1−cos(d))d2R = I + \\frac{W sin(d)}{d} + \\frac{W^2 (1-cos(d))}{d^2}R=I+dWsin(d)​+d2W2(1−cos(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);}
}

θ=arccos(tr−12)\\theta =arccos( \\frac{tr - 1}{2})θ=arccos(2tr−1​)
a=R−RTsin(θ)a =\\frac{R-R^T}{sin(\\theta)}a=sin(θ)R−RT​

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;elsereturn theta * w / s;
}