Eigen中Isometry3d的使用详解和实战示例

发布于:2025-07-10 ⋅ 阅读:(25) ⋅ 点赞:(0)

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.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;

其他常用操作

操作 示例代码
获取平移向量 T.translation()
获取旋转矩阵 T.rotation()T.linear()
获取变换矩阵 T.matrix()
点变换 T * point
逆变换 T.inverse()
向量变换(不含平移) T.linear() * direction


网站公告

今日签到

点亮在社区的每一天
去签到