传感器基础与标定：相机模型与内参标定、激光雷达原理与外参标定、相机-激光雷达联合标定

做SLAM这么多年，我越来越觉得一个道理：标定做不好，后面全是白忙活。你想想看，传感器就像机器人的眼睛，如果眼睛本身就有散光、斜视，那后面再怎么优化算法也救不回来。这一章，咱们就把相机和激光雷达的底细摸清楚，顺便把标定这件事彻底搞定。

1. 相机模型：从针孔到畸变

相机模型说白了就是回答一个问题：三维世界里的一个点，是怎么跑到二维图像上的？

最经典的当然是针孔模型。它的数学表达很简单：

// 世界坐标系到像素坐标的映射
// [u, v, 1]^T = K * [R|t] * [X, Y, Z, 1]^T

// 内参矩阵 K：
// [fx,  0, cx]
// [ 0, fy, cy]
// [ 0,  0,  1]

这里fx, fy是焦距（单位是像素），cx, cy是光心偏移。我习惯把内参理解为"相机自己的性格"——每台相机出厂时都不一样，必须通过标定来摸清它的脾气。

但现实世界没有完美的针孔。镜头会引入畸变，主要有两种：

• 径向畸变：镜头边缘的"桶形"或"枕形"变形。说白了就是直线变弯了。
• 切向畸变：镜头和成像平面不平行导致的偏移。

畸变矫正公式长这样：

// 径向畸变
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)

// 切向畸变
x_distorted = x + (2*p1*x*y + p2*(r^2 + 2*x^2))
y_distorted = y + (p1*(r^2 + 2*y^2) + 2*p2*x*y)

2. 内参标定：张正友法的实战要点

说到内参标定，绕不开张正友法。这个方法的核心思路是：拍不同角度的棋盘格照片，然后利用棋盘格的几何约束来求解内参。

具体步骤我总结为四步：

1. 采集图像：至少15-20张，覆盖不同角度和距离
2. 角点检测：找到棋盘格的内角点
3. 求解单应性矩阵：每张图都有一个单应性矩阵
4. 优化内参：用非线性最小二乘（比如Levenberg-Marquardt）迭代优化

我用OpenCV做标定比较多，代码大概是这样：

import cv2
import numpy as np

# 准备棋盘格角点坐标
pattern_size = (9, 6)  # 内角点数量
objp = np.zeros((pattern_size[0]*pattern_size[1], 3), np.float32)
objp[:,:2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1,2)

# 存储所有图像的点
objpoints = []  # 3D点
imgpoints = []  # 2D点

for fname in image_files:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
    
    if ret:
        objpoints.append(objp)
        imgpoints.append(corners)

# 标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
    objpoints, imgpoints, gray.shape[::-1], None, None
)

3. 激光雷达原理：TOF与机械式扫描

激光雷达（LiDAR）的原理其实不复杂：发射激光，接收反射光，算时间差，得距离。目前主流的有两种：

• 机械式LiDAR：通过旋转电机带动激光器旋转扫描。典型代表是Velodyne的16线、32线、64线。优点是视场角大（360°），缺点是机械结构容易坏。
• 固态LiDAR：没有旋转部件，用光学相控阵或MEMS微镜来偏转激光束。优点是体积小、寿命长，但视场角通常有限。

LiDAR输出的数据格式一般是点云，每个点包含(x, y, z, intensity)。其中intensity是反射强度，可以用来区分不同材质——比如车道线反射强，柏油路反射弱。

我习惯把LiDAR数据组织成这样的结构：

// 一帧点云的数据结构
struct Point {
    float x, y, z;      // 三维坐标（米）
    float intensity;    // 反射强度（0-255）
    uint16_t ring;      // 线束编号（0-15 对于16线LiDAR）
    double timestamp;   // 时间戳（秒）
};

4. 外参标定：LiDAR到IMU/车体的变换

外参标定要回答的问题是：激光雷达装在车上的哪个位置？朝向如何？

这个变换矩阵通常是一个4x4的齐次变换矩阵：

T_lidar_to_vehicle = [R | t]
                     [0 | 1]

其中R是旋转矩阵（3x3），t是平移向量（3x1）。

标定方法我推荐两种：

• 手工测量法：用卷尺量安装位置，用角度尺量朝向。简单粗暴，但精度有限（厘米级）。
• 基于特征的方法：在LiDAR前方放置标定物（比如立方体或平面），通过点云拟合来求解外参。精度可以达到毫米级。

我实际项目中用得最多的是平面拟合标定法：

// 伪代码：基于平面约束的外参标定
// 1. 在LiDAR点云中提取地面平面
// 2. 在IMU/车体坐标系中已知地面方程
// 3. 通过平面法向量对齐求解旋转
// 4. 通过平面距离约束求解平移

// 平面方程：ax + by + cz + d = 0
// 法向量 n = [a, b, c]^T
// 距离 d

5. 相机-激光雷达联合标定：让视觉和激光"对齐"

这是融合SLAM的核心环节。说白了就是：找到相机像素和LiDAR点云之间的对应关系。

联合标定的目标是求解一个4x4的变换矩阵T_cam_to_lidar，把LiDAR点投影到相机图像上：

// LiDAR点投影到图像
// 1. 将LiDAR点从LiDAR坐标系变换到相机坐标系
// 2. 用相机内参投影到像素平面

// 数学表达：
// [u, v, 1]^T = K * T_cam_to_lidar * [X_lidar, Y_lidar, Z_lidar, 1]^T

标定方法我推荐基于棋盘格的方法：

1. 在LiDAR和相机共同视野内放置棋盘格
2. 相机检测棋盘格角点
3. LiDAR检测棋盘格平面上的点云
4. 通过点-面对应关系求解外参

具体实现时，我习惯用Autoware的标定工具包，或者自己写一个基于PCL和OpenCV的标定程序：

// 联合标定的核心步骤
// 1. 从LiDAR点云中提取棋盘格平面上的点
// 2. 从相机图像中提取棋盘格角点
// 3. 构建优化问题：最小化重投影误差

// 优化目标函数：
// min Σ || pi - K * T * Pi ||^2
// 其中 pi 是图像角点，Pi 是LiDAR点，T是待求外参

6. 知识体系总览

这一章的内容比较多，我画了一张图帮你理清思路：

这张图把三个模块的关系理清楚了。相机内参和LiDAR外参是联合标定的输入，联合标定又把两者融合到一起。说白了，这就是一个层层递进的过程。

好了，这一章的内容就到这里。传感器标定这件事，说难不难，说简单也不简单。关键是要理解原理、掌握方法、注意细节。下一章我们会进入SLAM的核心——前端里程计，到时候这些标定好的传感器就要真正派上用场了。

---