第1章：坐标系与刚体变换
做激光雷达SLAM，说白了就是回答三个问题：我在哪？周围有什么？怎么过去的？而这三个问题的答案，都绕不开坐标系和刚体变换。我记得刚入行那会儿，被各种坐标系绕得晕头转向，一个旋转矩阵写反了，点云直接飞到天上去。嗯，今天咱们就把这块硬骨头啃下来。
1.1 世界坐标系：一切测量的基准
世界坐标系，就是那个绝对参考系。你可以把它想象成地球上的经纬度——不管车怎么动，这个坐标系纹丝不动。在自动驾驶里，我们通常用东北天（ENU）或者北东地（NED）来定义它。
关键点：世界坐标系是全局一致的。所有传感器数据最终都要统一到这个世界坐标系下，才能做融合和规划。
我个人习惯用ENU。为什么？因为直观。X轴指东，Y轴指北，Z轴朝天。你开车的时候，东和北是固定的，不会因为车头朝向而改变。这在做高精地图匹配时特别方便。
1.2 车体坐标系：车辆自身的参考系
车体坐标系跟着车走。一般定义是：X轴朝前（车头方向），Y轴朝左，Z轴朝上。原点通常在后轴中心或者车辆质心。
这里有个坑——不同车厂的定义可能不一样。我在项目中遇到过，某供应商的IMU数据用的是X轴朝右，结果融合出来的姿态全是反的。所以拿到数据第一件事，先确认坐标系定义，别想当然。
| 坐标系 | X轴 | Y轴 | Z轴 | 原点 |
|---|---|---|---|---|
| 世界坐标系 | 东 | 北 | 天 | 任意固定点 |
| 车体坐标系 | 前 | 左 | 上 | 后轴中心 |
| 雷达坐标系 | 前 | 左 | 上 | 激光雷达中心 |
1.3 雷达坐标系：传感器的视角
每个激光雷达都有自己的坐标系。比如Velodyne的坐标系，X轴朝前，Y轴朝左，Z轴朝上。但有些国产雷达可能不一样。你想想看，如果雷达装歪了，它的坐标系和车体坐标系之间就有一个旋转偏移。
这个偏移就是我们要标定的东西。说白了，就是求一个旋转矩阵R和一个平移向量t，把雷达坐标系下的点转换到车体坐标系下。
小技巧：标定的时候，我习惯先做粗标定——用卷尺量一下雷达的安装位置和角度，得到一个大概的初值。然后再用优化算法精调。这样收敛快，不容易掉进局部最优。
1.4 欧拉角与四元数：旋转的两种表达
旋转怎么表示？最直观的是欧拉角。绕X轴转叫横滚角（Roll），绕Y轴转叫俯仰角（Pitch），绕Z轴转叫偏航角（Yaw）。听起来很合理对吧？
但欧拉角有个致命问题——万向锁。当俯仰角接近±90°时，横滚和偏航会耦合在一起，丢失一个自由度。我在做无人机SLAM时就吃过这个亏，飞机垂直朝下时，姿态解算直接炸了。
所以，工程上我们更常用四元数。四元数用四个数表示旋转，没有奇点，插值也平滑。它的形式是：
q = w + x*i + y*j + z*k
其中 w^2 + x^2 + y^2 + z^2 = 1你可能会问，四元数怎么用？其实很简单。比如你要把一个点p绕某个轴旋转，用四元数做乘法就行：
p' = q * p * q^(-1)这里的乘法是四元数乘法，不是普通乘法。代码实现时，我建议直接用Eigen或者Sophus库，别自己手写——我曾经手写过一次，debug了一整天，最后发现是符号写反了。
1.5 齐次变换矩阵：把旋转和平移打包
旋转用四元数或旋转矩阵，平移用向量。但每次变换都要分开算，太麻烦。于是有了齐次变换矩阵：
T = [ R t ]
[ 0 1 ]这是一个4x4的矩阵。R是3x3的旋转矩阵，t是3x1的平移向量。最后一行是[0,0,0,1]。用这个矩阵，一个乘法就能完成旋转+平移：
p' = T * p这里的p是齐次坐标，也就是在三维坐标后面加一个1，变成[x,y,z,1]^T。
注意：齐次变换矩阵的乘法顺序很重要！先旋转再平移，和先平移再旋转，结果完全不同。我见过有人把顺序搞反，点云直接飞到几百米外。记住：先旋转，后平移。
1.6 知识体系总览
下面这张图总结了本章的核心逻辑。你可以看到，所有坐标系变换最终都归结为齐次变换矩阵的链式乘法。
1.7 实战：坐标系变换的代码实现
说了这么多，来点实际的。下面是一个C++代码片段，演示如何把雷达坐标系下的一个点变换到世界坐标系：
#include <Eigen/Dense>
// 定义齐次变换矩阵
Eigen::Matrix4d T_car_to_radar; // 雷达 -> 车体
Eigen::Matrix4d T_world_to_car; // 车体 -> 世界
// 假设已经标定好了这两个矩阵
T_car_to_radar <<
0.999, -0.017, 0.017, 0.5,
0.017, 0.999, 0.017, 0.0,
-0.017, -0.017, 0.999, 0.3,
0, 0, 0, 1;
T_world_to_car <<
0.998, -0.052, 0.035, 100.0,
0.052, 0.998, -0.017, 200.0,
-0.035, 0.017, 0.999, 0.5,
0, 0, 0, 1;
// 雷达坐标系下的一个点
Eigen::Vector4d p_radar(1.0, 2.0, 0.5, 1.0);
// 变换到车体坐标系
Eigen::Vector4d p_car = T_car_to_radar * p_radar;
// 变换到世界坐标系
Eigen::Vector4d p_world = T_world_to_car * p_car;
std::cout << "世界坐标系下的点: " << p_world.transpose() << std::endl;经验之谈：写这种代码时，我习惯把每个变换矩阵的源坐标系和目标坐标系用注释写清楚。比如 T_car_to_radar 表示「从雷达坐标系到车体坐标系」。这样半年后回来看代码，不会一脸懵逼。
1.8 避坑指南
最后，分享几个我踩过的坑：
- 坐标系方向搞反：我曾经把车体坐标系的Y轴方向搞反，结果点云左右颠倒。后来我养成了一个习惯——标定完先画个简单的点云图，看看形状对不对。
- 四元数归一化：四元数必须归一化，否则旋转会带缩放。我见过有人忘了归一化，点云越转越小，最后缩成一个点。
- 齐次坐标的w分量：变换时一定要保证w=1。如果w=0，表示方向向量，不会被平移——这在某些场景下有用，但如果你不小心搞混了，结果会非常诡异。
嗯，坐标系和刚体变换就讲到这里。这些东西看似基础，但真到了工程里，每一个细节都可能让你debug到怀疑人生。记住：坐标系定义清楚，变换矩阵写对，顺序别搞反——这三点做到了，后面SLAM的路就好走多了。