Eigen::Isometry3d 是 Eigen 庫中用于表示 三維空間中的剛性變換（Rigid Transformation） 的類，屬于 Eigen::Transform 模板類的一個特化版本。它結合了 旋轉和平移，廣泛應用于機器人學、SLAM、三維幾何計算等場景。
一、核心定義
#include <Eigen/Geometry>Eigen::Isometry3d T; // 表示一個 SE(3) 類型的剛性變換
實際上：
typedef Transform<double, 3, Isometry> Isometry3d;
其中：
double表示浮點數精度；3表示三維空間；Isometry表示保持距離和角度不變的變換（旋轉 + 平移，非仿射變換）。
二、類內部數據結構
Isometry3d ≈ 3×3 旋轉矩陣 + 3×1 平移向量（共 4×4 齊次矩陣）
其內部使用的是一個 4x4 的矩陣，布局如下：
| R(3x3) t(3x1) |
| 0 1 |
三、常用操作與代碼示例
1. 創建一個單位變換
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
2. 設置旋轉和平移
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();T.translate(Eigen::Vector3d(1.0, 2.0, 3.0)); // 設置平移
T.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); // 繞 Z 軸旋轉 45 度
3. 構造時直接賦值旋轉和平移
Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ()).toRotationMatrix();
Eigen::Vector3d t(1, 2, 3);Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.linear() = R; // 設置旋轉
T.translation() = t; // 設置平移
4. 應用變換到一個點
Eigen::Vector3d point(1, 0, 0);
Eigen::Vector3d transformed = T * point; // 使用 operator* 自動應用 SE(3) 變換
5. 變換的逆（求 T?1）
Eigen::Isometry3d T_inv = T.inverse();
6. 與其他剛體變換相乘（組合）
Eigen::Isometry3d T2 = ...;
Eigen::Isometry3d T_composed = T * T2;
這等價于先執行 T2，再執行 T（右乘慣例）。
四、常見應用場景
| 場景 | 示例 |
|---|---|
| 相機外參（旋轉+平移） | 相機從世界坐標到相機坐標系的變換矩陣 |
| 機器人關節的正向運動學 FK | 各連桿間剛體變換的鏈式乘法 |
| 點云變換 / 位姿優化 | 用于把點云從一幀配準到另一幀 |
| ORB-SLAM / GTSAM 中的位姿管理 | SE(3) 變換結構體 |
五、使用注意事項
| 注意點 | 說明 |
|---|---|
.rotate() 和 .translate() 是 左乘形式，等價于 T = T * R | |
.linear() 是旋轉矩陣引用，T.linear() 可直接賦值為 Eigen::Matrix3d | |
.matrix() 返回的是完整 4×4 齊次矩陣 | |
不支持仿射縮放（否則應使用 Affine3d） | |
和 Sophus::SE3d 有類似功能，但 Eigen::Isometry3d 適合基本計算 |
六、與其他類型的關系
| 類型 | 用途/關系 |
|---|---|
Eigen::Matrix4d | T.matrix() 可轉為 4x4 矩陣 |
Eigen::Quaterniond | 可用于構造 T.rotate() |
Sophus::SE3d | Sophus 是基于李群實現的 SE(3)，更適合優化時使用 |
gtsam::Pose3 | GTSAM 中 SE(3) 的表達，內部也是旋轉 + 平移 |
七、小結
Isometry3d是 Eigen 中表達三維剛體變換的核心類；- 支持旋轉、平移組合，矩陣表示清晰；
- 可直接作用于點、組合多個變換；
- 是機器人、視覺 SLAM 中重要的數學工具。
八、附件示例
示例一：構建 Isometry3d 變換
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>int main() {Eigen::Isometry3d T = Eigen::Isometry3d::Identity();// 設置旋轉：繞 Z 軸旋轉 90 度double angle_rad = M_PI / 2;T.rotate(Eigen::AngleAxisd(angle_rad, Eigen::Vector3d::UnitZ()));// 設置平移：向 x 方向平移 1.0T.pretranslate(Eigen::Vector3d(1.0, 0.0, 0.0));std::cout << "Isometry3d matrix:\n" << T.matrix() << std::endl;// 應用變換到一個點Eigen::Vector3d point(1, 0, 0);Eigen::Vector3d transformed = T * point;std::cout << "Original point: " << point.transpose() << std::endl;std::cout << "Transformed point: " << transformed.transpose() << std::endl;return 0;
}
示例二：從旋轉和平移構造 Isometry3d
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitY());Eigen::Vector3d t(1, 2, 3);Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.linear() = R; // 設置旋轉
T.translation() = t; // 設置平移std::cout << "Transform matrix:\n" << T.matrix() << std::endl;
示例三：組合多個變換
Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
T1.pretranslate(Eigen::Vector3d(1, 0, 0));Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
T2.rotate(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ()));Eigen::Isometry3d T_combined = T2 * T1;std::cout << "Combined transform:\n" << T_combined.matrix() << std::endl;
示例四：從 SE(3) 位姿向量構造 Isometry3d
假設有一個六維位姿向量 [tx, ty, tz, rx, ry, rz]，其中旋轉為軸角向量：
Eigen::VectorXd pose(6);
pose << 1, 2, 3, 0.1, 0.2, 0.3; // 平移 + 軸角旋轉Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.pretranslate(pose.head<3>());
T.rotate(Eigen::AngleAxisd(pose.tail<3>().norm(), pose.tail<3>().normalized()));std::cout << "SE(3) transformation:\n" << T.matrix() << std::endl;
其他常用操作
| 操作 | 示例代碼 |
|---|---|
| 獲取平移向量 | T.translation() |
| 獲取旋轉矩陣 | T.rotation() 或 T.linear() |
| 獲取變換矩陣 | T.matrix() |
| 點變換 | T * point |
| 逆變換 | T.inverse() |
| 向量變換（不含平移） | T.linear() * direction |
)
