Eigen库实战：如何用C++实现两个坐标系间的变换矩阵（附完整代码）

Eigen库实战：从零构建两个坐标系间的变换矩阵

最近在做一个机器人定位的项目，需要把传感器坐标系下的点云数据转换到世界坐标系里。刚开始我尝试自己手写矩阵运算，结果不是旋转方向反了，就是平移分量对不上，调试起来特别头疼。后来发现Eigen库在这方面简直是神器——它不仅能帮你处理所有底层数学运算，还提供了极其优雅的API设计。今天我就把自己在实际项目中积累的经验整理出来，特别是如何用Eigen实现两个坐标系间的完整变换矩阵，包括那些容易踩坑的细节。

如果你正在处理3D视觉、机器人学或者游戏开发中的坐标转换问题，这篇文章应该能帮你少走不少弯路。我会从最基本的数学原理讲起，然后逐步深入到Eigen的具体实现，最后还会分享几个性能优化的小技巧。不用担心数学基础不够，我会用最直观的方式解释清楚每个步骤。

1. 坐标系变换的数学基础：不只是矩阵乘法那么简单

很多人一提到坐标系变换，第一反应就是“矩阵乘法”。这没错，但理解背后的几何意义更重要。想象一下你手里拿着一个手机，手机有自己的坐标系（X轴向右，Y轴向上，Z轴指向屏幕外）。现在你要把这个手机放在桌面上，桌面也有自己的世界坐标系。这两个坐标系之间的关系，就是我们要用变换矩阵描述的东西。

1.1 旋转矩阵：坐标轴的重新对齐

旋转矩阵的本质是什么？其实很简单：它描述了一个坐标系的三个坐标轴在另一个坐标系中的方向。假设我们有坐标系A（世界坐标系）和坐标系B（局部坐标系），那么从B到A的旋转矩阵R的每一列，就是B坐标系的X、Y、Z轴在A坐标系中的单位向量。

用数学公式表示就是：

R = [ X_B_in_A, Y_B_in_A, Z_B_in_A ]

其中每个列向量都是单位向量，而且两两正交。

注意：这里有个常见的混淆点。有些资料把旋转矩阵定义为行向量形式，有些用列向量。Eigen默认使用列优先存储，所以我们在构造矩阵时要特别注意方向的一致性。

在实际应用中，获取这些方向向量的方法有很多：

• 通过传感器直接测量（如IMU给出的姿态）
• 通过已知的点对应关系计算
• 通过欧拉角或四元数转换得到

1.2 平移向量：坐标系原点的偏移

旋转解决了方向问题，平移则解决位置问题。平移向量t表示的是：坐标系B的原点在坐标系A中的坐标。

这个理解起来更直观。如果你知道手机中心点（B坐标系原点）在桌面（A坐标系）上的位置坐标(x, y, z)，那么这个坐标就是平移向量。

1.3 齐次坐标与4x4变换矩阵

为了把旋转和平移统一到一个矩阵运算中，我们引入了齐次坐标。在3D空间中，一个点P的齐次坐标表示为(x, y, z, 1)。这样，完整的4x4变换矩阵就可以写成：

T = [ R   t ]
    [ 0   1 ]

其中R是3x3旋转矩阵，t是3x1平移向量，底部的[0 0 0 1]是为了保持齐次坐标的格式。

这种表示法的最大好处是：连续的坐标系变换可以通过矩阵连乘来实现。比如从坐标系C到A的变换，如果已知C到B和B到A的变换，那么：

T_C_to_A = T_B_to_A * T_C_to_B

2. Eigen库入门：为什么选择它而不是自己造轮子

我第一次接触Eigen是因为需要处理大量的矩阵运算。当时尝试用标准库的vector<vector>，性能简直惨不忍睹。后来切换到Eigen，同样的运算速度提升了近百倍。这主要得益于Eigen的几个核心特性：

2.1 表达式模板：零成本抽象

Eigen最厉害的地方是它的表达式模板技术。当你写这样的代码时：

MatrixXd C = A * B + D;

Eigen并不会立即计算A*B，也不会创建临时矩阵存储中间结果。相反，它构建了一个表达式树，直到赋值给C时才一次性计算所有操作。这意味着：

• 没有不必要的临时对象
• 编译器可以优化整个表达式
• 循环融合，减少内存访问

2.2 丰富的矩阵类型

Eigen为常见的矩阵大小提供了特化版本，比如3x3旋转矩阵常用的Matrix3d，3D向量用的Vector3d，4x4变换矩阵用的Matrix4d。这些特化版本不仅更直观，性能也更好。

2.3 几何模块：专门为3D变换设计

Eigen的Geometry模块提供了一系列高级抽象：

• AngleAxisd：轴角表示
• <