别再死记硬背公式了！用Eigen库实战搞定旋转矩阵、四元数、欧拉角互转（附完整代码）

三维空间旋转表示全攻略从理论到Eigen实战在机器人、计算机视觉和游戏开发等领域三维空间的旋转表示与计算是基础而关键的技术。本文将深入解析旋转矩阵、欧拉角、旋转向量和四元数四种主流表示方法并通过Eigen库实战演示它们之间的转换帮助开发者避开常见陷阱。1. 旋转表示方法概述三维空间中的旋转有六种自由度三个平移和三个旋转而旋转部分的表示方法主要有四种旋转矩阵3×3正交矩阵直观但冗余欧拉角三个绕轴旋转的角度直观但存在万向锁旋转向量旋转轴旋转角度紧凑但计算复杂四元数四个数的超复数无奇异性且计算高效每种方法各有优劣实际项目中常需要根据场景灵活选择和转换。下面我们通过Eigen库来具体实现这些表示方法及其转换。2. Eigen库环境准备2.1 安装与配置Eigen是一个轻量级的C模板库主要用于线性代数运算。安装非常简单# Ubuntu系统安装 sudo apt-get install libeigen3-dev在CMake项目中引用find_package(Eigen3 REQUIRED) target_link_libraries(your_target Eigen3::Eigen)2.2 基本数据结构Eigen为各种旋转表示提供了专门的数据类型#include Eigen/Geometry // 旋转矩阵 Eigen::Matrix3d rotation_matrix; // 旋转向量轴角 Eigen::AngleAxisd rotation_vector; // 欧拉角 Eigen::Vector3d euler_angles; // 四元数 Eigen::Quaterniond quaternion;3. 四种旋转表示详解3.1 旋转矩阵旋转矩阵是一个3×3的正交矩阵满足RᵀR I且det(R) 1。创建旋转矩阵Eigen::Matrix3d R; R 0, -1, 0, 1, 0, 0, 0, 0, 1; // 绕Z轴旋转90度验证旋转矩阵性质// 检查正交性 bool is_orthogonal (R.transpose() * R).isIdentity(1e-6); // 检查行列式 double det R.determinant(); // 应为13.2 欧拉角欧拉角通过三个连续的绕轴旋转表示任意旋转常见顺序有ZYX、XYZ等。创建欧拉角Eigen::Vector3d euler_angles( alpha, // X轴旋转 beta, // Y轴旋转 gamma // Z轴旋转 );欧拉角转旋转矩阵Eigen::Matrix3d R; R Eigen::AngleAxisd(euler_angles[2], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(euler_angles[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(euler_angles[0], Eigen::Vector3d::UnitX());3.3 旋转向量轴角旋转向量用一个单位向量表示旋转轴用向量的长度表示旋转角度。创建旋转向量Eigen::Vector3d axis(0, 0, 1); // Z轴 double angle M_PI/2; // 90度 Eigen::AngleAxisd rotation_vector(angle, axis);旋转向量转旋转矩阵Eigen::Matrix3d R rotation_vector.toRotationMatrix();3.4 四元数四元数是一个四元组q [w, x, y, z]其中w是实部(x,y,z)是虚部。创建四元数Eigen::Quaterniond q; q.w() cos(angle/2); q.x() axis.x() * sin(angle/2); q.y() axis.y() * sin(angle/2); q.z() axis.z() * sin(angle/2);四元数旋转向量Eigen::Vector3d v(1, 0, 0); Eigen::Vector3d v_rotated q * v; // 重载了乘法运算符4. 旋转表示间的转换4.1 转换关系总览下表总结了四种表示之间的转换方法转换方向Eigen方法数学原理矩阵→欧拉角.eulerAngles(a0,a1,a2)矩阵分解矩阵→旋转向量AngleAxisd(matrix)罗德里格斯公式逆运算矩阵→四元数Quaterniond(matrix)矩阵迹计算欧拉角→矩阵三个AngleAxisd相乘旋转矩阵连乘旋转向量→矩阵.toRotationMatrix()罗德里格斯公式旋转向量→四元数Quaterniond(angle_axis)四元数定义四元数→矩阵.toRotationMatrix()四元数乘法矩阵表示四元数→旋转向量AngleAxisd(quaternion)对数映射4.2 关键转换实现旋转矩阵与四元数互转// 矩阵转四元数 Eigen::Matrix3d R; Eigen::Quaterniond q(R); // 四元数转矩阵 Eigen::Matrix3d R_from_q q.toRotationMatrix();欧拉角与旋转矩阵互转// 欧拉角转矩阵(Z-Y-X顺序) Eigen::Matrix3d R Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()); // 矩阵转欧拉角 Eigen::Vector3d euler R.eulerAngles(2, 1, 0); // Z,Y,X顺序旋转向量与四元数互转// 旋转向量转四元数 Eigen::AngleAxisd aa(angle, axis); Eigen::Quaterniond q(aa); // 四元数转旋转向量 Eigen::AngleAxisd aa_from_q(q);5. 实际应用中的注意事项5.1 万向锁问题当使用欧拉角且第二个旋转为±90度时会出现万向锁double pitch M_PI/2; // 90度俯仰 Eigen::Vector3d euler(roll, pitch, yaw); Eigen::Matrix3d R /* 从欧拉角创建矩阵 */; // 转换回欧拉角时可能丢失一个自由度 Eigen::Vector3d euler_recovered R.eulerAngles(2, 1, 0);解决方案避免使用会导致万向锁的欧拉角顺序改用四元数或旋转矩阵表示5.2 四元数归一化四元数应始终保持单位长度Eigen::Quaterniond q; q.normalize(); // 确保单位四元数5.3 旋转插值四元数非常适合旋转插值Eigen::Quaterniond q1, q2; // 球面线性插值 Eigen::Quaterniond q_interp q1.slerp(t, q2); // t∈[0,1]6. 完整代码示例以下是一个完整的旋转表示转换示例#include iostream #include Eigen/Geometry int main() { // 1. 定义初始旋转绕Z轴旋转90度 Eigen::AngleAxisd rotation_vector(M_PI/2, Eigen::Vector3d::UnitZ()); // 2. 转换为各种表示 Eigen::Matrix3d R rotation_vector.toRotationMatrix(); Eigen::Quaterniond q(rotation_vector); Eigen::Vector3d euler R.eulerAngles(2, 1, 0); // ZYX顺序 // 3. 打印结果 std::cout Rotation matrix:\n R \n\n; std::cout Quaternion: q.coeffs().transpose() \n\n; std::cout Euler angles (ZYX): euler.transpose() \n; // 4. 验证转换一致性 Eigen::Matrix3d R_from_q q.toRotationMatrix(); std::cout \nMatrix from quaternion:\n R_from_q \n; return 0; }7. 性能与精度对比在实际项目中不同旋转表示的性能和精度各有特点旋转矩阵优点直观易于组合变换缺点9个参数冗余插值困难欧拉角优点人类易理解存储紧凑缺点存在万向锁计算复杂旋转向量优点几何意义明确紧凑缺点计算复杂不适合插值四元数优点无奇异性插值平滑计算高效缺点不够直观需要归一化推荐使用策略用户界面使用欧拉角内部计算使用四元数或旋转矩阵存储传输使用旋转向量或四元数8. 常见问题解决方案8.1 旋转累积误差多次旋转操作可能导致累积误差解决方案// 定期重新归一化四元数 q.normalize(); // 或者从旋转矩阵重新构造 R R * delta_R; q Eigen::Quaterniond(R).normalized();8.2 不同坐标系转换处理传感器数据时常需坐标系转换// 相机坐标系到世界坐标系的旋转 Eigen::Quaterniond q_camera_to_world /* ... */; // 转换点 Eigen::Vector3d point_in_camera(1, 0, 0); Eigen::Vector3d point_in_world q_camera_to_world * point_in_camera;8.3 旋转求逆各种旋转表示的逆运算// 旋转矩阵求逆即转置 Eigen::Matrix3d R_inv R.transpose(); // 四元数求逆 Eigen::Quaterniond q_inv q.conjugate(); // 单位四元数 // 旋转向量求逆 Eigen::AngleAxisd aa_inv(-aa.angle(), aa.axis());掌握这些三维旋转表示方法及其转换将大大提升你在机器人、计算机视觉等领域的开发效率。Eigen库提供了完善的实现理解其背后的数学原理则能帮助你在复杂场景中做出正确选择。