Eigen/Geometry 模块-14

这是一个非常棒的 C++ 代码片段，演示了在 Eigen 库（机器人学和SLAM中最常用的线性代数库）中如何操作我们在前面讨论过的理论概念：旋转矩阵、角轴、欧拉角、变换矩阵和四元数。

Eigen 的设计非常优雅，但也有一些“坑”（Traps），特别是在数据存储顺序和函数调用上。

下面我将分模块为你详细解读这段代码，并指出其中的关键点和注意事项。

---

1. 基础旋转：旋转矩阵与角轴

// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();

// 旋转向量使用 AngleAxis
// 这里的 M_PI/4 是 45度，Vector3d(0,0,1) 是 Z轴
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );

// 转换与赋值
cout << "rotation matrix =\n" << rotation_vector.matrix() << endl; 
rotation_matrix = rotation_vector.toRotationMatrix();

• Eigen::AngleAxisd:

• 这就是我们在理论部分讲的角轴/旋转向量。它不是一个 Matrix 类，但 Eigen 重载了它的运算符，让你感觉它像个矩阵。

• 构造函数：AngleAxisd(angle, axis)。注意 axis 必须是归一化的单位向量。

• .matrix() 或 .toRotationMatrix()：将角轴转换回 的旋转矩阵 （利用罗德里格斯公式）。

• 坐标变换：

Eigen::Vector3d v ( 1,0,0 );
Eigen::Vector3d v_rotated = rotation_vector * v; // 使用角轴直接乘
v_rotated = rotation_matrix * v;                 // 使用矩阵乘

• 解读：Eigen 允许你直接用 AngleAxis 乘以一个向量，底层会自动进行转换计算。结果和用旋转矩阵乘是一模一样的。

---

2. 欧拉角 (Euler Angles)

// 将旋转矩阵直接转换成欧拉角
// eulerAngles(2, 1, 0) 表示按照 Z -> Y -> X 的轴顺序提取角度
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 );
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

• **关键函数 eulerAngles(2, 1, 0)**：

• Eigen 的轴定义：0=X, 1=Y, 2=Z。

• 参数 (2, 1, 0) 意味着你告诉 Eigen：“请把这个旋转矩阵分解为 的形式”。

• 对应关系：

• 结果向量的第 0 个元素对应参数里的第一个轴（即 2号轴 Z轴，Yaw）。

• 结果向量的第 1 个元素对应参数里的第二个轴（即 1号轴 Y轴，Pitch）。

• 结果向量的第 2 个元素对应参数里的第三个轴（即 0号轴 X轴，Roll）。

• 注意：欧拉角的解不唯一（存在多解），且 Eigen 返回的角度范围通常在 和 之间，有时会和你想的不太一样，但在数学上是等价的。

---

3. 欧氏变换矩阵 (Isometry)

这是 SLAM 中最常用的类型，描述 。

Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); 
// 虽然名字叫 Isometry3d，但它内部其实维护了一个 4x4 的矩阵

• 构建变换：

T.rotate ( rotation_vector );            // 加上旋转部分 R
T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 加上平移部分 t

• pretranslate vs translate (巨坑预警)：

• pretranslate(t)：相当于左乘平移矩阵。数学意义是 。这通常是我们想要的“先旋转，再平移”的组合逻辑（相对于原点）。

• translate(t)：相当于右乘。混合使用时一定要搞清楚是相对于当前坐标系移动，还是相对于世界坐标系移动。

• 坐标变换：

Eigen::Vector3d v_transformed = T * v; // 自动执行 R*v + t

• Eigen 重载了乘法，这行代码直接完成了 矩阵与 齐次坐标（自动补1）的乘法运算。

---

4. 四元数 (Quaternion)

这是最容易出错的部分，请仔细看注释。

// 从 AngleAxis 初始化四元数
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
// 或者从 旋转矩阵 初始化
q = Eigen::Quaterniond ( rotation_matrix );

• 内部存储顺序 (最大的坑)：

cout << "quaternion = \n" << q.coeffs() << endl;
// 输出顺序：(x, y, z, w)

* 在数学通式中，我们习惯写 $q = w + xi + yj + zk$ (实部在前)。
* 但在 Eigen 的内部存储（`.coeffs()`）中，顺序是 **$[x, y, z, w]$**，**实部 $w$ 在最后！**
* **但是**，当你通过构造函数 `Eigen::Quaterniond(w, x, y, z)` 创建时，**实部 $w$ 又是在第一个参数**。
* **总结**：初始化传参用 $(w, x, y, z)$，打印内部系数 `.coeffs()` 是 $(x, y, z, w)$。

• 旋转操作：

v_rotated = q * v;

• 这行简单的 q * v，底层自动执行了我们刚才讨论的复杂数学公式：。你不需要手动去写共轭和三明治积，Eigen 帮你做好了。

---

总结：Eigen 几何模块速查表

数学概念	Eigen 类型	关键点
旋转矩阵	Eigen::Matrix3d	正交且行列式为1，但也只是普通矩阵
角轴	Eigen::AngleAxisd	不是 Matrix，但可以 .toRotationMatrix()
欧拉角	Vector3d	使用 .eulerAngles(2,1,0) 转换，注意轴顺序
四元数	Eigen::Quaterniond	构造时 w 在前，存储/访问 .coeffs() 时 w 在后
欧氏变换	Eigen::Isometry3d	实质是 4x4 矩阵，用 .pretranslate() 设置平移

这段代码非常标准，涵盖了 SLAM 前端开发中 90% 的几何运算需求。如果你能熟练掌握 Isometry3d 和 Quaterniond 的转换，基础就算打牢了。