Eigen 几何模块深拆：Isometry3d vs Affine3d + 变换矩阵本质详解

0 写在前面

在 vSLAM / VIO / 机器人定位等工程实践中，90 % 的变换只是旋转 + 平移的刚体运动；只有不到 10 % 的场景（点云体素缩放、图像几何映射、CAD 变形等）才会用到带缩放或剪切的仿射变换。Eigen 针对这两类需求提供了

本文将从 数学原理 → 内部结构 → 常用 API → 实战示例 → 性能建议 → 易错排查 全方位梳理它们，并回答一个高频疑惑：Isometry3d 究竟是不是 4 × 4 矩阵？

1 数学背景对比

2 Eigen 实现差异

3 Isometry3d 是不是 4 × 4 矩阵？

结论：是！
Isometry3d 是 4 × 4 齐次刚体矩阵的封装类。

• 数学结构

  $$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right],R\in SO(3)$$

• 代码验证

  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
  Eigen::Matrix4d  H = T.matrix();   // H 是标准 4x4
  

4 核心 API 速查

#include <Eigen/Geometry>

// ── Isometry3d ───────────────────────────
Isometry3d T = Isometry3d::Identity();
T.rotate(q.toRotationMatrix());  // 写入旋转
T.pretranslate(t);               // 左乘平移
Matrix4d H = T.matrix();         // 取 4×4

// 组合与逆
Isometry3d Tab  = Ta * Tb;       // 右侧先执行
Isometry3d Tinv = T.inverse();   // 快速刚体逆
Vector3d  pw    = T * pc;        // 点变换

// ── Affine3d ────────────────────────────
Affine3d A = Affine3d::Identity();
A.linear() <<
    0.5, 0,   0,
    0,   0.5, 0,
    0,   0,   1;                 // 含缩放
A.translation() = Vector3d(1,2,3);

5 实战示例

5.1 SLAM 位姿链：相机点 → 世界点

// T_w_b :  body 坐标系 → world 坐标系   （车辆/IMU Pose）
Isometry3d T_w_b;     

// T_b_c :  camera 坐标系 → body 坐标系   （相机外参）
Isometry3d T_b_c;     

// p_c    :  相机坐标系下的点坐标（单位 m）
Vector3d  p_c(0.1, 0.2, 1.0);

// 右乘先算：p_c 先映射到 body，再映射到 world
Vector3d  p_w = T_w_b * T_b_c * p_c;  

要点

• 乘法写成 “外层坐标系 * 内层坐标系 * 点”
• 代码顺序 = 实际执行的坐标系级联（最右边先计算）

5.2 体素滤波：各向异性缩放（X/Y → 5 cm，Z → 10 cm）

Affine3d voxel = Affine3d::Identity();

// 设置线性部分：对 (x,y,z) 轴分别乘以 0.05, 0.05, 0.10
voxel.linear() <<
    0.05, 0,    0,
    0,    0.05, 0,
    0,    0,    0.10;

// 若还需平移，可再设置 voxel.translation()

// 直接对点或点云做缩放
point = voxel * point;

要点

• 取 Affine3d，因为含 缩放 ⇒ 非刚体
• .linear() 修改 3 × 3 线性块，.translation() 负责平移

5.3 把通用 4×4 矩阵 M “正交化”为刚体变换

// ① 读进来时先用 Affine3d 保存，保留所有信息
Affine3d A(M);      

// ② 提取线性部分做极分解 / 四元数归一化，得到最接近的正交矩阵
Quaterniond q(A.linear());         // 自动正交化
Matrix3d    R = q.toRotationMatrix();

// ③ 重新组装为 Isometry3d（刚体），平移直接拷贝
Isometry3d T;
T.linear()      = R;               // 旋转 (正交)
T.translation() = A.translation(); // 平移

要点

1. 外部矩阵不保证正交 → 先用 Affine3d 接收
2. 利用 Quaterniond 把 3 × 3 线性块正交化
3. 重新封装成 Isometry3d，之后即可安全用于 SLAM 位姿累乘

6 性能与数值建议

7 常见坑 · 排雷

1. 乘法顺序与注释不符
   T_a_b * T_b_c 表示“先 c→b，再 b→a”——注释别写反！
2. 误往 Isometry3d.linear() 写入非正交矩阵
   写完请确保 R.col(i).norm()==1 且 det≈1。
3. 角度单位
   Eigen 全部使用 弧度，杜绝度→弧度混用。
4. 存盘丢语义
   存 T.matrix()，读后用 Isometry3d(H) 恢复。

8 Cheat Sheet (贴工位)

#include <Eigen/Geometry>

Isometry3d T = Isometry3d::Identity();
T.rotate(q);              // 写旋转
T.pretranslate(t);        // 写平移
Vector3d p = T * pc;      // 变换点
Matrix4d H = T.matrix();  // 导出 4×4
T = Isometry3d(H);        // 从矩阵恢复

9 总结

牢记 “右乘先算，左乘决定结果坐标系”，你的姿态链将稳固无坑。祝开发顺利！

参考资料

• Eigen 官方文档：https://eigen.tuxfamily.org/dox/group__Geometry__Module.html
• 《视觉 SLAM 十四讲》第 3 章