4、传感器模型：针孔相机模型、激光雷达模型、IMU模型
做多传感器融合，第一步要干什么？
不是调算法，也不是写代码。你得先搞清楚每个传感器到底是怎么「看」这个世界的。说白了，你得知道它们输出的数据长什么样，坐标系怎么定义，误差从哪来。
我刚开始做融合的时候，就吃过这个亏。拿着激光雷达和相机的数据直接对齐，结果怎么都对不上。后来才发现，我连相机内参都没搞清楚。嗯，这节课咱们就把这三个传感器的模型彻底捋一遍。
4.1 针孔相机模型
相机模型，最经典的就是针孔模型。你想想看，一个小孔，光线穿过它，在后面的感光元件上成像。这就是最朴素的成像原理。
4.1.1 坐标系与投影
相机成像涉及四个坐标系：世界坐标系、相机坐标系、图像坐标系、像素坐标系。我个人习惯把它们的转换关系记成「三步走」：
- 世界→相机：刚体变换（旋转+平移）
- 相机→图像：透视投影（除以Z）
- 图像→像素：缩放+平移（内参矩阵）
数学上，一个3D点 \( P = [X, Y, Z]^T \) 投影到像素坐标 \( [u, v]^T \) 的过程可以写成：
// 归一化平面
x' = X / Z
y' = Y / Z
// 加畸变（径向+切向）
r² = x'² + y'²
x'' = x' * (1 + k1*r² + k2*r⁴) + 2*p1*x'*y' + p2*(r² + 2*x'²)
y'' = y' * (1 + k1*r² + k2*r⁴) + p1*(r² + 2*y'²) + 2*p2*x'*y'
// 像素坐标
u = fx * x'' + cx
v = fy * y'' + cy
核心参数：内参矩阵 K = [fx, 0, cx; 0, fy, cy; 0, 0, 1]，畸变系数 [k1, k2, p1, p2, k3]。
我的经验：标定相机内参时，棋盘格一定要贴平。我曾经用一张皱巴巴的纸标定，结果畸变参数完全不对，融合出来的点云全是歪的。后来换了玻璃材质的标定板，一次就过了。
4.1.2 畸变模型
实际镜头不是完美的针孔。主要有两种畸变：
- 径向畸变：光线在镜头边缘弯曲更厉害。桶形畸变和枕形畸变就是它。
- 切向畸变：镜头和感光元件不平行导致的。
为什么要去畸变？因为后续的融合算法，比如把激光点投影到图像上，都假设图像是「理想针孔模型」下的。不去畸变，投影位置会偏好几个像素。
注意：去畸变后，图像边缘会被裁剪或拉伸。如果你做的是BEV（鸟瞰图）感知，这一步尤其要小心，边缘的语义信息可能会丢失。
4.2 激光雷达模型
激光雷达，说白了就是「用激光测距」。它发射一束激光，打到物体上反射回来，根据时间差算距离。
4.2.1 工作原理
主流的有两种：
- 机械式：通过旋转电机带动激光收发模块旋转。比如Velodyne的64线雷达，一圈转下来能扫几十万个点。
- 固态式：没有旋转部件，用光学相控阵或MEMS微镜来改变激光方向。体积小，但视场角有限。
每个激光点包含的信息：
// 一个典型的激光点云数据包
struct Point {
float x; // 前向（米）
float y; // 左侧（米）
float z; // 向上（米）
float intensity; // 反射强度（0-255）
uint8_t ring; // 线束编号（0-63）
double timestamp; // 时间戳（微秒）
};
4.2.2 坐标系定义
激光雷达的坐标系通常是：前x，左y，上z。原点在雷达的几何中心。
这里有个坑：不同厂家的定义可能不一样。比如有些雷达把y轴定义为前向。我在项目里遇到过，用Hesai的雷达和Velodyne的雷达做融合，坐标系定义不同，直接对齐就错了。所以拿到雷达数据，第一件事就是看它的坐标系定义文档。
关键点：激光雷达的测量误差通常与距离有关。近距离（<10m）误差在±2cm左右，远距离（>50m）可能到±10cm。做外参标定时，尽量用近距离的标定物。
4.3 IMU模型
IMU，惯性测量单元。它测量的是加速度和角速度。说白了，就是告诉你「我动了多少」。
4.3.1 测量原理
IMU内部有：
- 加速度计：测量三轴加速度（包括重力加速度）
- 陀螺仪：测量三轴角速度
输出数据通常是：
// IMU数据格式
struct ImuData {
double timestamp;
float accel_x, accel_y, accel_z; // m/s²
float gyro_x, gyro_y, gyro_z; // rad/s
float orientation[4]; // 四元数（可选）
};
4.3.2 误差模型
IMU最让人头疼的就是误差。我总结了一下，主要有这几类：
| 误差类型 | 来源 | 影响 |
|---|---|---|
| 零偏（Bias） | 制造工艺、温度变化 | 积分后位置漂移 |
| 尺度因子（Scale） | 灵敏度不一致 | 测量值偏大或偏小 |
| 白噪声（Noise） | 电子噪声 | 高频抖动 |
| 温度漂移 | 温度变化 | 零偏随时间变化 |
避坑指南：我曾经在冬天做路测，IMU放在车里，刚开始数据很正常。跑了半小时，车内温度升高，零偏直接漂了0.1 rad/s。积分出来的姿态，10分钟就偏了30度。后来我学乖了，每次用IMU之前，先做5分钟的静态初始化，估计出当前的零偏。
4.3.3 坐标系约定
IMU的坐标系通常是：前x，左y，上z。但注意，加速度计测量的是「比力」，也就是物体受到的合力减去重力。所以当IMU静止时，加速度计读数是 [0, 0, 9.8] m/s²（z轴向上）。
重要：做外参标定时，IMU的坐标系必须和车体坐标系对齐。如果IMU安装歪了，你融合出来的轨迹就是歪的。我建议用六面法标定IMU的内参，把零偏和尺度因子都标出来。
4.4 三种模型的对比与联系
这三种传感器，各有各的脾气。我画了一张图，帮你理清它们的关系：
从这张图你能看出来，相机、激光雷达、IMU，它们输出的数据形式完全不同。相机是2D像素，激光雷达是3D点云，IMU是运动信息。但它们的共同点是：都需要一个「外参」来描述它们之间的相对位置和姿态。
做外参标定，本质上就是求解这个旋转矩阵R和平移向量t。怎么求？后面几章我会详细讲。但前提是，你得先把每个传感器的模型吃透。模型搞错了，标定出来的外参就是错的，融合出来的结果也是错的。
总结一下：
- 相机模型：记住内参+畸变，投影公式要烂熟于心
- 激光雷达模型：注意坐标系定义，不同厂家可能不同
- IMU模型：误差是最大的敌人，零偏和噪声必须处理
好了，这节课就到这里。下节课我们开始讲具体的标定方法，到时候我会拿一个实际项目的数据，带你们一步步走通整个流程。
课后练习：找一份你手头传感器的数据手册，把它的坐标系定义、输出格式、误差指标都列出来。然后对照这节课的内容，看看有没有遗漏的。