第三节：标定的数学基础与参数分类
说实话，刚入行那会儿，我觉得标定就是调几个参数。后来被现实狠狠教育了一顿——有一次标定出来的外参，车跑起来直接偏了半米。排查了三天，最后发现是坐标系定义搞反了。从那以后，我养成了一个习惯：动手之前，先把数学基础捋清楚。
这一节，我们就聊聊标定背后的数学。别怕，我不会堆公式。我会用我踩过的坑，帮你把这些概念串起来。
3.1 坐标系：一切问题的起点
车载传感器标定，说白了就是回答一个问题：这个传感器看到的物体，在车身上到底在哪儿？
要回答这个问题，你得先定义好坐标系。我见过最典型的错误，就是团队里两个人用了不同的坐标系定义，结果标定结果对不上。
常见的坐标系有这么几种：
- 世界坐标系：通常以车辆启动位置为原点，或者以GPS经纬度为基准。用于描述车辆在全局地图中的位置。
- 车体坐标系：一般以车辆后轴中心为原点，x轴朝前，y轴朝左，z轴朝上。这是最常用的参考系。
- 传感器坐标系：每个传感器都有自己的坐标系。比如激光雷达，原点在旋转中心；摄像头，原点在光心。
- 像素坐标系：图像上的二维坐标，原点在左上角，单位是像素。
重点来了：标定的本质，就是建立这些坐标系之间的变换关系。你想想看，如果连坐标系都没对齐，后面算出来的距离、角度，全是错的。
3.2 刚体变换：旋转和平移
坐标系之间怎么转换？靠刚体变换。刚体变换包含两部分：旋转和平移。
旋转可以用旋转矩阵、欧拉角、四元数来表示。我个人习惯用四元数，因为它没有万向锁问题。有一次我在项目里用欧拉角做插值，结果在俯仰角接近90度时，航向角突然跳了180度。嗯，那画面太美我不敢看。
平移就简单了，就是三个方向上的偏移量：x、y、z。
一个完整的刚体变换，通常写成4x4的齐次变换矩阵：
| R t |
| 0 1 |其中R是3x3旋转矩阵，t是3x1平移向量。这个矩阵可以把一个点从传感器坐标系，变换到车体坐标系。
我的小技巧：写代码时，我习惯把变换矩阵存成4x4的numpy数组。这样矩阵乘法可以直接用@运算符，代码简洁，不容易出错。
3.3 李群与李代数：为什么需要它？
你可能会问：旋转矩阵不是挺好的吗？为什么还要搞什么李群李代数？
原因很简单：优化问题不好解。
旋转矩阵有正交约束，你不能直接对它做加法。比如两个旋转矩阵相加，结果可能不再是旋转矩阵。这就导致在标定优化时，没法用常规的梯度下降法。
李群和李代数，就是来解决这个问题的。李群（SO(3)）是旋转矩阵的集合，李代数（so(3)）是它的切空间。说白了，你把旋转矩阵映射到李代数上，就可以自由地做加法、求导了。
我记得第一次用李代数做标定优化时，收敛速度比直接用欧拉角快了好几倍。而且再也不用担心万向锁的问题。
常用的转换关系：
- 指数映射：从李代数到李群，即从旋转向量到旋转矩阵
- 对数映射：从李群到李代数，即从旋转矩阵到旋转向量
我曾经踩过的坑：李代数求导时，雅可比矩阵的公式很容易写错。建议直接用Sophus库或者g2o，它们已经帮你封装好了。自己手写的话，一定要用数值微分验证一遍。
3.4 标定参数分类
搞清楚了数学基础，我们来看看标定到底要标哪些参数。我把它们分成三类：
3.4.1 内参
内参是传感器自身的固有属性。比如：
- 摄像头：焦距、主点、畸变系数（径向畸变、切向畸变）
- 激光雷达：每个激光束的俯仰角、水平角偏移
- IMU：零偏、尺度因子、安装误差
内参一般在出厂时会有标定值，但温度、振动会导致它漂移。我建议每半年重新标定一次内参，尤其是摄像头。
3.4.2 外参
外参描述的是传感器之间的相对位置和姿态。比如：
- 激光雷达相对于车体的旋转和平移
- 摄像头相对于激光雷达的旋转和平移
- IMU相对于车体的安装角度
外参是标定的核心。你想想看，如果激光雷达和摄像头的外参差了5厘米，融合出来的目标位置就会差一大截。在高速场景下，这个误差会被放大。
避坑指南：我曾经在标定激光雷达和摄像头外参时，只用了静态数据。结果车一跑起来，外参就变了。后来发现是安装支架有松动。所以，外参标定一定要做动态验证。
3.4.3 时延
时延是最容易被忽略的参数。不同传感器的采样频率不同，数据到达的时间也不同。比如：
- 摄像头：30fps，每帧33ms
- 激光雷达：10fps，每帧100ms
- IMU：200fps，每帧5ms
如果不做时延标定，融合出来的结果会有时间错位。想象一下，摄像头看到障碍物在左边，激光雷达却认为它在右边——这就是时延导致的。
时延标定的方法：
- 硬件同步：通过PPS信号或IEEE 1588协议，让所有传感器使用同一个时钟
- 软件对齐：通过互相关分析，找到两个传感器数据序列的最佳对齐偏移量
我的经验：硬件同步能解决大部分问题，但总会有几毫秒的残余时延。我一般会在软件里再加一个可调的时延参数，在标定过程中一起优化掉。
3.5 小结
这一节的内容，说白了就是三件事：
- 搞清楚坐标系怎么定义的
- 学会用刚体变换和李群李代数做坐标转换
- 知道要标哪些参数：内参、外参、时延
这些是标定的地基。地基没打好，后面盖的房子再漂亮也是危房。下一节，我们会把这些数学工具用起来，开始搭建实际的标定工具链。
嗯，先消化一下。有问题随时问我。