Eigen 几何模块深拆:Isometry3d vs Affine3d + 变换矩阵本质详解
文章目录
- 0 写在前面
- 1 数学背景对比
- 2 Eigen 实现差异
- 3 `Isometry3d` 是不是 4 × 4 矩阵?
- 4 核心 API 速查
- 5 实战示例
- 5.1 SLAM 位姿链:相机点 → 世界点
- 5.2 体素滤波:各向异性缩放(X/Y → 5 cm,Z → 10 cm)
- 5.3 把通用 4×4 矩阵 M “正交化”为刚体变换
- 6 性能与数值建议
- 7 常见坑 · 排雷
- 8 Cheat Sheet (贴工位)
- 9 总结
- 参考资料
目录
- 0 写在前面
- 1 数学背景对比
- 2 Eigen 实现差异
- 3 `Isometry3d` 是不是 4 × 4 矩阵?
- 4 核心 API 速查
- 5 实战示例
- 5.1 SLAM 位姿链:相机点 → 世界点
- 5.2 体素滤波:各向异性缩放(X/Y → 5 cm,Z → 10 cm)
- 5.3 把通用 4×4 矩阵 M “正交化”为刚体变换
- 6 性能与数值建议
- 7 常见坑 · 排雷
- 8 Cheat Sheet (贴工位)
- 9 总结
- 参考资料
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(可带缩放/剪切) |
矩阵结构 | [Rt01]\displaystyle\begin{bmatrix}R&t\\0&1\end{bmatrix}[R0t1],R∈SO(3)R∈SO(3)R∈SO(3) | [At01]\displaystyle\begin{bmatrix}A&t\\0&1\end{bmatrix}[A0t1],AAA 任意 3 × 3 |
自由度 | 6 (3 旋转 + 3 平移) | ≤ 12 (9 线性 + 3 平移) |
典型应用 | 传感器外参、SLAM 位姿、TF 广播 | 图像/点云缩放、CAD 模型变形 |
2 Eigen 实现差异
特性 | Isometry3d | Affine3d |
---|---|---|
旋转部分 | 强制正交(det = 1) | 任意 3 × 3 |
存储 | 封装 4 × 4 矩阵(刚体特化) | 封装 4 × 4 矩阵 |
求逆 | 只需 R⊤,−R⊤tR^\top,\,-R^\top tR⊤,−R⊤t | 通用 4 × 4 逆 |
内存 | 12 double (9 R + 3 t) | 16 double |
语义保障 | 保证组合后仍为刚体 | 可能引入缩放/剪切 |
3 Isometry3d
是不是 4 × 4 矩阵?
结论:是!
Isometry3d
是 4 × 4 齐次刚体矩阵的封装类。
-
数学结构
T=[Rt01],R∈SO(3)T=\begin{bmatrix}R&t\\0&1\end{bmatrix},\;R∈SO(3) T=[R0t1],R∈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(); // 平移
要点
- 外部矩阵不保证正交 → 先用
Affine3d
接收- 利用
Quaterniond
把 3 × 3 线性块正交化- 重新封装成
Isometry3d
,之后即可安全用于 SLAM 位姿累乘
6 性能与数值建议
场景 | 推荐类型 |
---|---|
大量位姿累乘 / 求逆 | Isometry3d |
含尺度 / 剪切 | Affine3d |
不确定线性部分是否正交 | 先 Affine3d ,再正交化 |
与 Sophus / g2o 联合 | 始终保持 Isometry3d |
7 常见坑 · 排雷
- 乘法顺序与注释不符
T_a_b * T_b_c
表示“先 c→b,再 b→a”——注释别写反! - 误往
Isometry3d.linear()
写入非正交矩阵
写完请确保R.col(i).norm()==1
且det≈1
。 - 角度单位
Eigen 全部使用 弧度,杜绝度→弧度混用。 - 存盘丢语义
存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 章