Eigen中Isometry3d的使用详解和实战示例
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⁻¹）
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 |