三维空间旋转表示实战指南：从理论到Eigen代码实现
在机器人、计算机视觉和游戏开发等领域，三维空间的旋转表示是一个基础而关键的问题。本文将深入探讨四种常见的旋转表示方法：旋转矩阵、旋转向量（轴角）、欧拉角和四元数，并通过Eigen库展示它们之间的相互转换。
1. 旋转表示方法概述
三维空间中的旋转可以用多种方式表示，每种方法都有其优缺点和适用场景：
- 旋转矩阵：3x3正交矩阵，直观但冗余
- 旋转向量（轴角）：用旋转轴和角度表示，紧凑但有奇异性
- 欧拉角：三个绕坐标轴的旋转角度，直观但存在万向锁问题
- 四元数：四个数的超复数表示，紧凑且无奇异性
提示：在实际应用中，四元数因其紧凑性和无奇异性而成为首选，但在需要直观理解时，欧拉角仍然很有价值。
2. Eigen库中的旋转表示
Eigen是一个强大的C++模板库，提供了各种旋转表示及其转换：
// 旋转矩阵 Eigen::Matrix3d rotation_matrix; // 旋转向量（轴角） Eigen::AngleAxisd rotation_vector(angle, axis); // 欧拉角 Eigen::Vector3d euler_angles(alpha, beta, gamma); // 四元数 Eigen::Quaterniond quaternion(w, x, y, z);3. 旋转表示之间的转换
3.1 旋转矩阵与其他表示的转换
旋转矩阵 ↔ 旋转向量：
// 旋转矩阵转旋转向量 Eigen::AngleAxisd rotation_vector; rotation_vector.fromRotationMatrix(rotation_matrix); // 旋转向量转旋转矩阵 Eigen::Matrix3d rotation_matrix = rotation_vector.toRotationMatrix();旋转矩阵 ↔ 欧拉角：
// 旋转矩阵转欧拉角（ZYX顺序） Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // 欧拉角转旋转矩阵 Eigen::Matrix3d rotation_matrix = Eigen::AngleAxisd(euler_angles[0], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(euler_angles[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(euler_angles[2], Eigen::Vector3d::UnitX());旋转矩阵 ↔ 四元数：
// 旋转矩阵转四元数 Eigen::Quaterniond quaternion(rotation_matrix); // 四元数转旋转矩阵 Eigen::Matrix3d rotation_matrix = quaternion.toRotationMatrix();3.2 四元数与其他表示的转换
四元数 ↔ 旋转向量：
// 四元数转旋转向量 Eigen::AngleAxisd rotation_vector(quaternion); // 旋转向量转四元数 Eigen::Quaterniond quaternion(rotation_vector);四元数 ↔ 欧拉角：
// 四元数转欧拉角（通过旋转矩阵） Eigen::Vector3d euler_angles = quaternion.toRotationMatrix().eulerAngles(2, 1, 0); // 欧拉角转四元数 Eigen::Quaterniond quaternion = Eigen::AngleAxisd(euler_angles[0], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(euler_angles[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(euler_angles[2], Eigen::Vector3d::UnitX());4. 常见问题与解决方案
4.1 万向锁问题
欧拉角在俯仰角为±90°时会出现万向锁问题，导致丢失一个自由度。解决方案：
- 避免使用可能导致万向锁的欧拉角顺序
- 使用四元数代替欧拉角进行插值计算
- 限制俯仰角范围，避免接近±90°
4.2 四元数归一化
四元数应保持单位长度，否则会影响旋转的正确性：
// 归一化四元数 quaternion.normalize(); // 检查四元数是否归一化 if (std::abs(quaternion.squaredNorm() - 1.0) > 1e-6) { quaternion.normalize(); }4.3 旋转组合与插值
旋转组合：
// 使用四元数组合旋转 Eigen::Quaterniond combined = quaternion2 * quaternion1; // 使用旋转矩阵组合旋转 Eigen::Matrix3d combined = rotation_matrix2 * rotation_matrix1;旋转插值：
// 球面线性插值(Slerp) Eigen::Quaterniond interpolated = quaternion1.slerp(t, quaternion2);5. 完整示例代码
以下是一个完整的示例，展示各种旋转表示及其转换：
#include <iostream> #include <Eigen/Dense> #include <Eigen/Geometry> int main() { // 初始化一个旋转 double angle = M_PI / 4.0; // 45度 Eigen::Vector3d axis(1.0, 0.0, 0.0); // 绕X轴旋转 // 1. 旋转向量表示 Eigen::AngleAxisd rotation_vector(angle, axis); std::cout << "Rotation vector:\n" << rotation_vector.axis() << " with angle: " << rotation_vector.angle() << "\n\n"; // 2. 转换为旋转矩阵 Eigen::Matrix3d rotation_matrix = rotation_vector.toRotationMatrix(); std::cout << "Rotation matrix:\n" << rotation_matrix << "\n\n"; // 3. 转换为欧拉角 (ZYX顺序) Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); std::cout << "Euler angles (ZYX order):\n" << euler_angles.transpose() << "\n\n"; // 4. 转换为四元数 Eigen::Quaterniond quaternion(rotation_vector); std::cout << "Quaternion:\n" << quaternion.coeffs().transpose() << "\n\n"; // 5. 验证转换一致性 Eigen::Vector3d point(1.0, 2.0, 3.0); Eigen::Vector3d rotated1 = rotation_vector * point; Eigen::Vector3d rotated2 = rotation_matrix * point; Eigen::Vector3d rotated3 = quaternion * point; std::cout << "Original point: " << point.transpose() << "\n"; std::cout << "Rotated by rotation vector: " << rotated1.transpose() << "\n"; std::cout << "Rotated by rotation matrix: " << rotated2.transpose() << "\n"; std::cout << "Rotated by quaternion: " << rotated3.transpose() << "\n"; return 0; }6. 性能考虑与最佳实践
性能比较：
- 旋转矩阵乘法最快，但占用内存多
- 四元数运算效率高，内存占用少
- 欧拉角转换计算成本高，应避免频繁转换
内存占用：
- 旋转矩阵：9个double（72字节）
- 四元数：4个double（32字节）
- 旋转向量：4个double（32字节）
- 欧拉角：3个double（24字节）
使用建议：
- 内部计算使用四元数
- 与外部系统交互时根据需要转换为其他表示
- 避免在关键循环中进行旋转表示转换
7. 实际应用案例
7.1 IMU数据处理
处理IMU数据时，通常需要将四元数转换为欧拉角以供显示：
Eigen::Quaterniond imu_orientation(w, x, y, z); Eigen::Vector3d euler = imu_orientation.toRotationMatrix().eulerAngles(2, 1, 0); std::cout << "Roll: " << euler[2] << ", Pitch: " << euler[1] << ", Yaw: " << euler[0] << std::endl;7.2 3D模型变换
在3D渲染中，组合多个变换：
Eigen::Affine3d transform = Eigen::Affine3d::Identity(); transform.translate(Eigen::Vector3d(1.0, 2.0, 3.0)); transform.rotate(quaternion); transform.scale(0.5); // 应用到顶点 Eigen::Vector3d transformed_vertex = transform * vertex;7.3 相机姿态估计
在SLAM中表示相机姿态：
// 使用四元数表示旋转 Eigen::Quaterniond camera_rotation(w, x, y, z); Eigen::Vector3d camera_position(x, y, z); // 转换为变换矩阵 Eigen::Affine3d camera_pose = Eigen::Affine3d::Identity(); camera_pose.translate(camera_position); camera_pose.rotate(camera_rotation);8. 高级话题
8.1 旋转微分与李代数
对于优化问题，常需要使用旋转的李代数表示：
// 旋转矩阵到李代数 Eigen::Vector3d lie_alg = rotation_vector.angle() * rotation_vector.axis(); // 李代数到旋转矩阵 Eigen::AngleAxisd rotation_vector(lie_alg.norm(), lie_alg.normalized()); Eigen::Matrix3d rotation_matrix = rotation_vector.toRotationMatrix();8.2 旋转平均
计算多个旋转的平均：
std::vector<Eigen::Quaterniond> quaternions; // ... 添加四元数 Eigen::Quaterniond average = Eigen::Quaterniond::Identity(); for (const auto& q : quaternions) { if (q.coeffs().dot(average.coeffs()) < 0) { average.coeffs() -= q.coeffs(); } else { average.coeffs() += q.coeffs(); } } average.normalize();8.3 旋转误差度量
比较两个旋转的差异：
// 使用角度差 double angular_error = Eigen::AngleAxisd(rotation_matrix1.transpose() * rotation_matrix2).angle(); // 使用四元数差 Eigen::Quaterniond q_diff = quaternion1.inverse() * quaternion2; double quat_error = 2 * std::acos(std::abs(q_diff.w()));在实际项目中，我发现四元数的链式乘法顺序容易出错，特别是在组合多个旋转时。一个实用的调试技巧是先用简单的90度绕轴旋转验证旋转方向是否符合预期，再应用到复杂旋转中。
