---

---

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^{?},?R^{?}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 章

---