3. 传感器模型

各位同学，今天我们聊一个非常核心的话题——相机模型。

做自动驾驶定位，你绕不开相机。为什么？因为相机便宜、信息丰富，能看清车道线、路沿、交通标志。但相机有个毛病：它拍出来的图像是二维的，而我们要的是三维世界中的位置。怎么把二维像素坐标映射回三维空间？这就靠相机模型了。

我个人习惯把相机模型拆成两部分来理解：内参和外参。内参管的是「相机自己长什么样」，外参管的是「相机装在车上的什么位置」。两者缺一不可。

3.1 针孔模型——最朴素的相机成像原理

针孔模型，说白了就是初中物理的小孔成像。光线穿过一个小孔，在后面的感光元件上形成倒像。只不过在相机里，我们用透镜代替了小孔，用CMOS传感器代替了胶片。

数学上怎么描述？我们需要四个坐标系：世界坐标系、相机坐标系、图像坐标系、像素坐标系。嗯，这里要注意，初学者最容易搞混的就是图像坐标系和像素坐标系——前者单位是毫米，后者单位是像素。

投影公式长这样：

// 从相机坐标系到像素坐标系的投影
// (Xc, Yc, Zc) 是相机坐标系下的3D点
// (u, v) 是像素坐标
// fx, fy 是焦距（像素单位）
// cx, cy 是主点偏移

u = fx * Xc / Zc + cx
v = fy * Yc / Zc + cy

写成矩阵形式更优雅：

// 齐次坐标形式
// [u]   [fx  0  cx] [Xc]
// [v] = [ 0 fy  cy] [Yc]
// [1]   [ 0  0   1] [Zc]

// 或者更完整的写法：
// s * [u, v, 1]^T = K * [R|t] * [Xw, Yw, Zw, 1]^T
// 其中 K 是内参矩阵，[R|t] 是外参

我在项目中遇到过一个问题：用OpenCV标定出来的fx和fy，跟厂家给的datasheet对不上。后来发现，OpenCV默认的像素坐标系原点在左上角，而有些厂家的文档用的是中心原点。嗯，这种坑踩过一次就记住了。

3.2 畸变模型——现实总是不完美的

针孔模型是理想情况。实际相机有透镜，透镜就会带来畸变。最常见的两种：径向畸变和切向畸变。

径向畸变：就是画面边缘的直线变弯了。桶形畸变（广角镜头常见）和枕形畸变（长焦镜头常见）。

切向畸变：透镜和CMOS不平行导致的。这个在手机相机上比较明显，车载相机一般装配精度高，切向畸变相对小一些。

畸变矫正的数学模型：

// 径向畸变（k1, k2, k3 是畸变系数）
// (x, y) 是归一化平面上的坐标
// r^2 = x^2 + y^2

x_distorted = x * (1 + k1*r^2 + k2*r^4 + k3*r^6)
y_distorted = y * (1 + k1*r^2 + k2*r^4 + k3*r^6)

// 切向畸变（p1, p2 是畸变系数）
x_distorted += 2*p1*x*y + p2*(r^2 + 2*x^2)
y_distorted += p1*(r^2 + 2*y^2) + 2*p2*x*y

我曾经调试过一个项目，车道线在图像边缘总是检测不准。排查了半天，发现是畸变矫正参数没更新——换了镜头模组，但标定文件还是老的。你想想看，畸变参数不对，边缘的像素位置偏差能有几十个像素，车道线检测能准才怪。

3.3 外参标定——把相机装到车上去

内参搞定了，接下来就是外参。外参解决的是：相机坐标系和车体坐标系之间的变换关系。

说白了，就是知道相机在车上的确切位置和朝向。装在挡风玻璃后面，还是装在车顶？俯仰角是多少？偏航角是多少？这些就是外参。

外参用数学表达就是：

// 外参矩阵 [R|t]
// R: 3x3 旋转矩阵（描述朝向）
// t: 3x1 平移向量（描述位置）

// 从车体坐标系到相机坐标系的变换：
// X_cam = R * X_vehicle + t

外参标定常用的方法：

• 手动测量法：用尺子量位置，用量角器量角度。简单粗暴，但精度有限。
• 标定板法：在车前方放标定板，通过已知的标定板角点位置反算外参。这是最常用的方法。
• 自然特征法：利用车道线、路沿等自然特征进行在线标定。这个比较高级，适合量产车的在线校准。

3.4 标定流程——手把手教你做

我建议的标定流程是这样的：

1. 内参标定：用棋盘格拍20-30张不同角度的照片，用OpenCV或MATLAB标定工具箱跑一遍。得到fx、fy、cx、cy和畸变系数。
2. 外参初值：根据相机安装位置，手动测量一个大概的R和t。比如相机装在挡风玻璃后面，高度1.2米，俯仰角向下5度。
3. 外参精调：在车辆前方放置标定板，采集图像。利用已知的标定板3D点和图像上的2D点，用PnP算法求解精确的外参。
4. 验证：把标定好的内外参代入，看车道线投影是否准确。一般要求在近处（5米内）误差小于1个像素，远处（30米）误差小于3个像素。

好了，这一节的内容就这些。内参和外参是车道线检测定位融合的基础，后面讲车道线检测算法时，你会频繁用到这些概念。特别是把检测到的车道线像素坐标转换到车体坐标系时，内参和外参一个都不能少。

记住一句话：标定做得有多准，定位就能做多稳。