【LCC系列】自动驾驶中激光雷达和相机的自动在线标定（2013）

本文介绍一个自动驾驶中激光雷达和相机 自动在线标定方法。该论文基于 点云深度不连续与图像边缘对齐的假设，提出的方法可以在 自然场景中自动 检测标定误差，并实时连续 纠正传感器漂移导致的标定误差。

论文：Automatic Online Calibration of Cameras and Lasers （2013）
作者：Jesse Levinson, Sebastian Thrun （Stanford Artificial Intelligence Laboratory）
官网：Stanford Autonomous Driving Team

论文出自鼎鼎大名斯坦福大学的自动驾驶团队的，该团队曾在DARPA无人车竞赛中夺冠。Sebastian Thrun是谷歌无人车之父，也是Udacity的创始人。 一作Jesse Levinson则是其得意门生，现在是ZOOX的联合创始人和CTO。两人都是自动驾驶领域的风云人物，来一起看看大佬曾经的杰作！

1 动机

为了利用激光雷达和相机的互补特性，需要标定两种传感器，求取其外参，以便将两种模态的数据转换到统一坐标系下，进行融合定位建图或感知。一般做法是先标定好传感器，再进行使用。但是，在机器人或无人车的运行过程中，很可能发生传感器位姿漂移，从而使标定参数产生误差，融合的数据也就不再可靠，从而引发安全问题。如何实时在线地判断传感器标定是否正确，如何纠正可能的传感器漂移导致的外参误差呢？

本文针对以上两个问题，做出两大贡献：1）提出一种概率检测算法，可以实时，高可靠地判断传感器是否准确标定，2）提出一种连续标定算法，针对缓慢的外参漂移，在线更新外参。

2 方法

该论文的目标是输入图像和点云序列，可以依靠自然场景进行实时在线地检测和纠正标定误差。该方法之所以能摆脱标定板等特定目标的限制，主要依赖于以下假设：相比标定错误，在标定正确时，激光雷达点云中的深度不连续更可能投影到相机图像的边缘位置。这个假设还是比较直观的，而且点云中的深度不连续和图像中的边缘都是非常常见的特征，基于此假设的方法也是比较容易实现的。

如何去使用这个假设涉及到以下问题：图像边缘的提取，点云深度不连续的提取，量化“更可能投影到…位置”，基于概率的量化指标判断标定误差，纠正标定误差。

2.1 提取图像边缘

分为两步：1）先图像灰度化，用边缘检测算法检测图像边缘，得到边缘图$E$。本文采用该像素与周围8像素的最大绝对值衡量该像素对应的边缘度。2）使用图像逆距离变换赋予非边缘像素以边缘度，得到$D$。
$$D_{i,j}=\alpha \cdot E_{i,j}+(1-\alpha )\cdot \underset{x,y}{\max }{E}_{x,y}\cdot {\gamma }^{max(|x-i|,|y-j|)}$$

即，每个位置$(i,j)$的边缘度是该位置边缘度$E_{i,j}$和其他位置对该位置的最大辐射量的加权求和。辐射量由其他位置边缘度和距离有关。

如上图，每个像素都有一个数值，描述其边缘程度。

2.2 提取点云深度不连续

每条扫描线单独考虑，寻找相比两个临近点更近的点。给定点云$P^i$，计算点云$X^i$，对于$X^i$中每个点$p$，则其深度不连续数值如下：
$$X_p^i=\max (P_{p-1}^i.r-P_p^i\cdot .r,P_{p+1}^i.r-P_p^i\cdot .r,0{)}^{\gamma }$$

下缀$.r$指每个点的距离测量。使用$\gamma$为0.5。过滤所有深度不连续小于30cm的点。下图（上）绿点即为深度不连续点，投影到图像上。下图（下）显示对应图像边缘。

2.3 目标函数

给定标定外参$C$，将$X_i$中的所有点投影到边缘图$D_i$上，结合最后w帧数据，目标函数如下：$$J_C=\sum _{f=n-w}^n\sum _{p=1}^{|X_f|}{X}_p^f\cdot D_{i,j}^f$$

这个目标函数计算了X中深度不连续性与边缘图中边缘度乘积的和，描述了当前外参下，点云深度不连续和图像边缘的对齐程度。一般来说，值越大，对应的外参越好。

以上目标函数不是外参的函数，无法优化求解外参。理论上，可以通过全局搜索找最佳外参，但是6维参数空间太大，难以实时搜索最优值。但可以用来确定给定C是否正确。

2.4 检测标定误差

检测标定误差基于以下假设：Jc是否是J的局部最优，是C是否准确的重要指标。而对C进行局部格网搜索得到c’，如果计算对应的Jc‘都小于Jc，就可以判断当前Jc是局部最优，C正确。

如果搜索半径为1，则6维参数对应$3^6=729$个不同J (包含Jc)。令$F_c$为728个J的扰动值比Jc更小的比例。 则$F_c$越接近1，Jc是局部最优的概率越大，则标定正确可能性越大。

在一段数据序列上，分别计算正确标定和错误标定的$F_c$，并用高斯分布拟合。

左侧为错误标定的分布，右侧为正确标定的分布。基于两个高斯分布，就可计算正确标定的概率 （Fc = x）：

计算得到概率，通过与设定阈值比较，就可判定标定是否正确。

注意此处P不是概率分布，而是统计测试

上面使用直方图进行可视化，用线画图可视化如下。可得两个结论：1）正确标定下，（% of worse calibrations） 数值接近1，而错误标定下，在0.5上下跳动。2）相比single-frame window, 使用9-frame window的结果噪声更小，更准确。

2.5 自动在线标定

除了判断一个标定是否正确，还可以利用全局最优Jc附近的目标函数J的局部凸性假设，来跟踪C随时间的小变化。

方法：在每一此迭代中，如果C的所有扰动导致J下降，C保持不变；否则，在格网搜索的所有C’中，选择最大目标函数J对应的标定C’。

相比单帧，9帧效果更好

3 实验

激光雷达：Velodyne HDL-64E S2 LIDAR, 64线，水平360度，垂直-22到+2度, 10Hz, 每转100,000点
相机：Point Gray Ladybug3 Panoramic unit (包含5个相机，只用其一)， 10Hz,

C++实现, 实时运行在笔记本CPU。

3.1 在线检测标定误差

随机添加旋转平移误差 （up to 20cm, 2 degrees），进行测试。

• 使用9帧窗口，在超过一千帧的数据上，可百分百分辨0.25度或10cm的误差。90%情况下可分辨0.1度的误差。
• 窗口越大，误差检测能力越强，但计算量大，无法实时。

3.2 在线纠正标定误差

旋转漂移更常见，对旋转添加随机扰动。

• 水平方向角度(yaw角)更准确。水平扫描分辨率高，提取垂直方向边缘。
• 对roll, pitch的跟踪平均误差0.12度。对yaw的跟踪平均误差0.06度。
• 整体来说，能达到0.1度的旋转标定精度

4 总结

• 两个技术：标定误差的在线检测和在线纠正。
• 两个假设：1）点云的深度不连续与图像边缘应对齐（前端特征提取）；2）目标函数J在全局最优附近是凸的（后端优化）。
• 主要结论：1）yaw角标定更准确；2）整体达到0.1度精度；3）使用数据窗口更鲁邦。
• 可能的改进：更大的格网半径 (larger grid radii),使用 Monte Carlo 采样方法 （比如使用粒子滤波，而非本文的贪婪方法）。