---

---

0 写在前面

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

类别	Eigen 类型	数学对应
刚体变换	Eigen::Isometry3d	SE(3)
一般仿射	Eigen::Affine3d	A(3)

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

---

1 数学背景对比

	刚体变换 SE(3)	仿射变换 A(3)
定义	保持点间距离不变的变换	线性变换 A + 平移 t（可带缩放/剪切）
矩阵结构	$\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$，$R\in SO(3)$	$\left[\begin{array}{cc}A& t\\ 0& 1\end{array}\right]$，$A$ 任意 3 × 3
自由度	6 (3 旋转 + 3 平移)	≤ 12 (9 线性 + 3 平移)
典型应用	传感器外参、SLAM 位姿、TF 广播	图像/点云缩放、CAD 模型变形

---

2 Eigen 实现差异

特性	Isometry3d	Affine3d
旋转部分	强制正交（det = 1）	任意 3 × 3
存储	封装 4 × 4 矩阵（刚体特化）	封装 4 × 4 矩阵
求逆	只需 $R^{\top },-R^{\top }t$	通用 4 × 4 逆
内存	12 double (9 R + 3 t)	16 double
语义保障	保证组合后仍为刚体	可能引入缩放/剪切

---

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
  

类型	本质	维度	刚体约束	典型用途
Matrix4d	纯 4×4 矩阵	4×4	❌	渲染 / 存盘
Isometry3d	刚体 SE(3)	4×4 (封装)	✅	SLAM / VIO
Affine3d	仿射 A(3)	4×4 (封装)	❌	图像/点云缩放

---

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 性能与数值建议

场景	推荐类型
大量位姿累乘 / 求逆	Isometry3d
含尺度 / 剪切	Affine3d
不确定线性部分是否正交	先 Affine3d，再正交化
与 Sophus / g2o 联合	始终保持 Isometry3d

---

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 总结

类型	选它的理由	典型任务
Isometry3d	强制刚体、逆运算 O(1)	SLAM 位姿、外参、TF
Affine3d	允许缩放/剪切	点云/图像几何、CAD
Matrix4d	无语义，最通用	统一存盘 / 可视化

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

---

参考资料

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

---