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

文章目录

[0 写在前面](#0 写在前面)
[1 数学背景对比](#1 数学背景对比)
[2 Eigen 实现差异](#2 Eigen 实现差异)
[3 `Isometry3d` 是不是 4 × 4 矩阵？](#3 Isometry3d 是不是 4 × 4 矩阵？)
[4 核心 API 速查](#4 核心 API 速查)
[5 实战示例](#5 实战示例)
- [5.1 SLAM 位姿链：相机点 → 世界点](#5.1 SLAM 位姿链：相机点 → 世界点)
- [5.2 体素滤波：各向异性缩放（X/Y → 5 cm，Z → 10 cm）](#5.2 体素滤波：各向异性缩放（X/Y → 5 cm，Z → 10 cm）)
- [5.3 把通用 4×4 矩阵 M "正交化"为刚体变换](#5.3 把通用 4×4 矩阵 M “正交化”为刚体变换)
[6 性能与数值建议](#6 性能与数值建议)
[7 常见坑 · 排雷](#7 常见坑 · 排雷)
[8 Cheat Sheet (贴工位)](#8 Cheat Sheet (贴工位))
[9 总结](#9 总结)
参考资料

[0 写在前面](#0 写在前面)
[1 数学背景对比](#1 数学背景对比)
[2 Eigen 实现差异](#2 Eigen 实现差异)
[3 `Isometry3d` 是不是 4 × 4 矩阵？](#3 Isometry3d 是不是 4 × 4 矩阵？)
[4 核心 API 速查](#4 核心 API 速查)
[5 实战示例](#5 实战示例)
- [5.1 SLAM 位姿链：相机点 → 世界点](#5.1 SLAM 位姿链：相机点 → 世界点)
- [5.2 体素滤波：各向异性缩放（X/Y → 5 cm，Z → 10 cm）](#5.2 体素滤波：各向异性缩放（X/Y → 5 cm，Z → 10 cm）)
- [5.3 把通用 4×4 矩阵 M "正交化"为刚体变换](#5.3 把通用 4×4 矩阵 M “正交化”为刚体变换)
[6 性能与数值建议](#6 性能与数值建议)
[7 常见坑 · 排雷](#7 常见坑 · 排雷)
[8 Cheat Sheet (贴工位)](#8 Cheat Sheet (贴工位))
[9 总结](#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（可带缩放/剪切）
矩阵结构	$R t 0 1$ \displaystyle\begin{bmatrix}R&t\\0&1\end{bmatrix} $R0t1$ ， R ∈ S O ( 3 ) R∈SO(3) R∈SO(3)	$A t 0 1$ \displaystyle\begin{bmatrix}A&t\\0&1\end{bmatrix} $A0t1$ ， A A 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 ⊤ , − R ⊤ t R^\top,\,-R^\top t R⊤,−R⊤t	通用 4 × 4 逆
内存	12 double (9 R + 3 t)	16 double
语义保障	保证组合后仍为刚体	可能引入缩放/剪切

3 `Isometry3d` 是不是 4 × 4 矩阵？

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

数学结构

T = $R t 0 1$ , R ∈ S O ( 3 ) T=\begin{bmatrix}R&t\\0&1\end{bmatrix},\;R∈SO(3) T= $R0t1$ ,R∈SO(3)

代码验证

cpp 复制代码

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 速查

cpp 复制代码

#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 位姿链：相机点 → 世界点

cpp 复制代码

// 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）

cpp 复制代码

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 "正交化"为刚体变换

cpp 复制代码

// ① 读进来时先用 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 (贴工位)

cpp 复制代码

#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 章

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

参考资料

3 `Isometry3d` 是不是 4 × 4 矩阵？