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（右乘惯例）。

四、常见应用场景

五、使用注意事项

六、与其他类型的关系

七、小结

• 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.0
    T.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;

其他常用操作