Eigen中四元数、欧拉角、旋转矩阵、旋转向量之间的转换

在三维空间中，姿态（方向）的表示方式多种多样：旋转向量（AngleAxis）、旋转矩阵（Matrix3d）、欧拉角（Euler Angles）、四元数（Quaternion） 都可以实现旋转描述，但它们的应用场景、数值稳定性和效率略有不同。

本篇整理了 Eigen 库 中常见的旋转表示之间的 初始化方法与相互转换方式，适用于 SLAM/VIO/机器人/图形学等应用。

Eigen 几何模块简介

#include <Eigen/Geometry>  // 包含 Quaterniond, AngleAxisd, eulerAngles 等

这个头文件提供了所有所需的姿态类与函数。无论你使用的是 Eigen::Matrix3d、Eigen::Quaterniond 还是 AngleAxisd，都能从中调用相应的函数。

一、旋转向量（AngleAxis）

定义：一个旋转轴 axis = (x,y,z)（单位向量）+ 一个旋转角度 α（单位为弧度）

Eigen::AngleAxisd rotation_vector(alpha, Eigen::Vector3d(x, y, z));

常用转换

// 转换为旋转矩阵
Eigen::Matrix3d rotation_matrix = rotation_vector.toRotationMatrix();

// 转换为欧拉角 (Z-Y-X, 即Yaw-Pitch-Roll)
Eigen::Vector3d eulerAngle = rotation_vector.toRotationMatrix().eulerAngles(2, 1, 0);

// 转换为四元数
Eigen::Quaterniond quaternion(rotation_vector);

二、旋转矩阵（Matrix3d）

定义：一个 3×3 的正交矩阵 R，满足 R * Rᵀ = I, det(R) = 1

Eigen::Matrix3d R;
R << r00, r01, r02,
     r10, r11, r12,
     r20, r21, r22;

常用转换

// 转换为旋转向量
Eigen::AngleAxisd rotation_vector(R);

// 转换为欧拉角
Eigen::Vector3d eulerAngle = R.eulerAngles(2, 1, 0);

// 转换为四元数
Eigen::Quaterniond quaternion(R);

三、欧拉角（EulerAngles）

定义：三维角度顺序旋转，例如 Z-Y-X（Yaw-Pitch-Roll）

Eigen::Vector3d eulerAngle(yaw, pitch, roll); // 弧度制

构造旋转矩阵 / 四元数

Eigen::AngleAxisd Rx(eulerAngle(2), Eigen::Vector3d::UnitX());
Eigen::AngleAxisd Ry(eulerAngle(1), Eigen::Vector3d::UnitY());
Eigen::AngleAxisd Rz(eulerAngle(0), Eigen::Vector3d::UnitZ());

Eigen::Matrix3d R = Rz * Ry * Rx;                   // 欧拉角转矩阵
Eigen::Quaterniond q(R);                            // 欧拉角转四元数

💡 顺序为先绕 X，再绕 Y，再绕 Z（右乘顺序）

四、四元数（Quaterniond）

定义：旋转角度用 (w, x, y, z) 表示（单位四元数）

初始化方式

// 使用 (w, x, y, z) 初始化
Eigen::Quaterniond q(w, x, y, z);

// 从旋转矩阵初始化
Eigen::Quaterniond q(R);

// 从旋转向量初始化
Eigen::Quaterniond q(rotation_vector);

// 从向量数据映射（共享内存）
Eigen::Map<Eigen::Quaterniond> q(q_vec.data()); // q_vec 是 [x, y, z, w]

常用操作

q.toRotationMatrix();   // 转为旋转矩阵
q.inverse();            // 求逆（共轭）
q.normalize();          // 单位化
q * v;                  // 对向量 v 进行旋转

五、转换关系总览图

欧拉角 ↔ 旋转向量 ↔ 旋转矩阵 ↔ 四元数
         ↘           ↙
           AngleAxisd

六、实战示例：三种方式构建位姿变换

// 方法 1：旋转矩阵 + 平移向量
Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
T1.linear() = R;
T1.translation() = t;

// 方法 2：四元数 + 平移
Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
T2.rotate(q);
T2.pretranslate(t);

// 方法 3：AngleAxis + 平移
Eigen::Isometry3d T3 = Eigen::Isometry3d::Identity();
T3.rotate(rotation_vector);
T3.pretranslate(t);

注意事项与建议

1. 角度单位：Eigen 所有旋转相关函数默认使用 弧度，不要混用角度。
2. 顺序问题：欧拉角转换时注意顺序一致（常用 ZYX），对应 .eulerAngles(2,1,0)。
3. 四元数数据顺序：构造时为 (w,x,y,z)，Map 共享内存时为 [x,y,z,w]。
4. 数值稳定性：使用四元数或旋转向量通常比欧拉角更稳定，尤其是在优化和插值场景。

总结

参考

• Eigen 官方文档：Geometry Module
• 博客：Eigen中四元数、欧拉角、旋转矩阵、旋转向量之间的转换
• Eigen位姿表示
• 【Eigen】从入门到放弃（二）：矩阵&向量的运算
• 【Eigen】从入门到放弃（一）：Eigen是个什么鬼？
• Eigen的使用总结1——基础
• Eigen的使用总结2——geometry
• Eigen的使用总结3——Map类