2. 坐标系统与变换
做多传感器融合，说白了就是让不同传感器「说同一种语言」。你想想看，激光雷达看到的是自己坐标系下的点，IMU测量的是自己身上的加速度，摄像头看到的像素坐标——这些数据要融合到一起，必须先把它们统一到同一个坐标系下。
我记得刚入行那会儿，第一次做激光雷达和相机的融合项目。数据采集回来，怎么都对不上。折腾了两天，最后发现是坐标变换矩阵写反了。嗯，从那以后，我对坐标系变换这件事就格外上心。
世界坐标系与传感器坐标系
先搞清楚两个基本概念：
- 世界坐标系：一个固定的参考系，所有传感器都相对于它来描述位置。通常我们选第一帧时的车体位置作为原点，或者用GPS/UTM坐标。
- 传感器坐标系：每个传感器自己的小天地。比如激光雷达的坐标系，原点在激光雷达中心，x轴朝前，y轴朝左，z轴朝上。
传感器坐标系和世界坐标系之间，靠的就是刚体变换来转换。
核心思想：世界坐标系是「绝对」的，传感器坐标系是「相对」的。融合的本质，就是把所有「相对」数据映射到「绝对」空间里。
刚体变换：旋转矩阵 + 平移向量
刚体变换，说白了就是「转一下，再挪一下」。它由两部分组成：
- 旋转矩阵 R：3x3的正交矩阵，描述朝向变化
- 平移向量 t：3x1的向量，描述位置偏移
公式很简单：p_world = R * p_sensor + t
这里有个坑——旋转矩阵必须是正交矩阵，也就是说 RTR = I。我在项目中遇到过有人手写旋转矩阵，结果数值误差导致矩阵不正交，最后融合出来的轨迹全是扭曲的。所以，能用库函数就别自己手算。
我曾经踩过的坑：旋转矩阵的乘法顺序。左乘和右乘结果完全不同。ROS里用的是左乘，Eigen默认也是左乘。如果你从论文里抄公式，一定要看清楚是左乘还是右乘。
四元数与欧拉角
欧拉角直观，但有个致命问题——万向锁。我建议你在实际工程中尽量用四元数。
| 表示方式 | 优点 | 缺点 |
|---|---|---|
| 欧拉角 (roll, pitch, yaw) | 直观，容易理解 | 万向锁，插值困难 |
| 四元数 (w, x, y, z) | 无奇点，可平滑插值 | 不够直观 |
| 旋转矩阵 | 计算方便 | 参数冗余，需正交化 |
四元数其实没那么可怕。你只要记住：q = [w, x, y, z]，其中 w 是实部，x,y,z 是虚部。单位四元数满足 w² + x² + y² + z² = 1。
我个人习惯在代码里统一用四元数存储姿态，只在可视化或调试时转成欧拉角。这样既避免了万向锁，又方便做插值。
实用技巧：Eigen库中，四元数转旋转矩阵用 .toRotationMatrix()，旋转矩阵转四元数用 Quaterniond(R)。ROS里 tf2 的 transformPose 函数可以直接帮你做坐标系变换，省去手写矩阵的麻烦。
知识体系结构图
下面这张图梳理了本章的核心逻辑：
代码示例：Eigen实现坐标变换
下面是一个实际可用的C++代码片段，演示了如何用Eigen库做坐标变换：
#include <Eigen/Dense>
#include <iostream>
int main() {
// 定义旋转：绕z轴转45度
Eigen::AngleAxisd roll(0.0, Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitch(0.0, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yaw(M_PI/4, Eigen::Vector3d::UnitZ());
Eigen::Quaterniond q = yaw * pitch * roll;
Eigen::Matrix3d R = q.toRotationMatrix();
// 平移向量：传感器在世界坐标系中的位置
Eigen::Vector3d t(1.0, 2.0, 0.5);
// 传感器坐标系下的一个点
Eigen::Vector3d p_sensor(0.0, 1.0, 0.0);
// 变换到世界坐标系
Eigen::Vector3d p_world = R * p_sensor + t;
std::cout << "世界坐标: " << p_world.transpose() << std::endl;
return 0;
}我个人的工程习惯：在代码里统一用 Eigen::Isometry3d 来存储变换矩阵。它内部就是4x4齐次矩阵，用 .rotate() 和 .pretranslate() 来构建，比手写R和t要安全得多。
好了，坐标变换这块内容就讲到这里。记住一句话：所有传感器数据，最终都要统一到同一个坐标系下。这是多传感器融合的基石，也是你写代码时最容易出错的地方。多检查，多验证，别像我当年那样栽跟头。