本文介绍一个自动驾驶中激光雷达和相机 自动在线标定方法。该论文基于 点云深度不连续与图像边缘对齐的假设,提出的方法可以在 自然场景中自动 检测标定误差,并实时连续 纠正传感器漂移导致的标定误差。
论文: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
E
E。本文采用该像素与周围8像素的最大绝对值衡量该像素对应的边缘度。2)使用图像逆距离变换赋予非边缘像素以边缘度,得到
D
D
D。
D
i
,
j
=
α
⋅
E
i
,
j
+
(
1
−
α
)
⋅
max
x
,
y
E
x
,
y
⋅
γ
m
a
x
(
∣
x
−
i
∣
,
∣
y
−
j
∣
)
D_{i,j} = \alpha\cdot E_{i,j}+(1-\alpha)\cdot \max_{x,y} E_{x,y}\cdot \gamma^{max(|x-i|,|y-j|)}
Di,j=α⋅Ei,j+(1−α)⋅x,ymaxEx,y⋅γmax(∣x−i∣,∣y−j∣)
即,每个位置
(
i
,
j
)
(i,j)
(i,j)的边缘度是该位置边缘度
E
i
,
j
E_{i,j}
Ei,j和其他位置对该位置的最大辐射量的加权求和。辐射量由其他位置边缘度和距离有关。
如上图,每个像素都有一个数值,描述其边缘程度。
2.2 提取点云深度不连续
每条扫描线单独考虑,寻找相比两个临近点更近的点。给定点云
P
i
P^i
Pi,计算点云
X
i
X^i
Xi,对于
X
i
X^i
Xi中每个点
p
p
p,则其深度不连续数值如下:
X
p
i
=
max
(
P
p
−
1
i
.
r
−
P
p
i
⋅
.
r
,
P
p
+
1
i
.
r
−
P
p
i
⋅
.
r
,
0
)
γ
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}
Xpi=max(Pp−1i.r−Ppi⋅.r,Pp+1i.r−Ppi⋅.r,0)γ
下缀
.
r
.r
.r指每个点的距离测量。使用
γ
\gamma
γ为0.5。过滤所有深度不连续小于30cm的点。下图(上)绿点即为深度不连续点,投影到图像上。下图(下)显示对应图像边缘。
2.3 目标函数
给定标定外参 C C C,将 X i X_i Xi中的所有点投影到边缘图 D i D_i Di上,结合最后w帧数据,目标函数如下: J C = ∑ f = n − w n ∑ p = 1 ∣ X f ∣ X p f ⋅ D i , j f J_C = \sum_{f=n-w}^n \sum_{p=1}^{|X_f|} X_p^f \cdot D_{i,j}^f JC=f=n−w∑np=1∑∣Xf∣Xpf⋅Di,jf
这个目标函数计算了X中深度不连续性与边缘图中边缘度乘积的和,描述了当前外参下,点云深度不连续和图像边缘的对齐程度。一般来说,值越大,对应的外参越好。
以上目标函数不是外参的函数,无法优化求解外参。理论上,可以通过全局搜索找最佳外参,但是6维参数空间太大,难以实时搜索最优值。但可以用来确定给定C是否正确。
2.4 检测标定误差
检测标定误差基于以下假设:Jc是否是J的局部最优,是C是否准确的重要指标。而对C进行局部格网搜索得到c’,如果计算对应的Jc‘都小于Jc,就可以判断当前Jc是局部最优,C正确。
如果搜索半径为1,则6维参数对应 3 6 = 729 3^6=729 36=729个不同J (包含Jc)。令 F c F_c Fc为728个J的扰动值比Jc更小的比例。 则 F c F_c Fc越接近1,Jc是局部最优的概率越大,则标定正确可能性越大。
在一段数据序列上,分别计算正确标定和错误标定的
F
c
F_c
Fc,并用高斯分布拟合。
左侧为错误标定的分布,右侧为正确标定的分布。基于两个高斯分布,就可计算正确标定的概率 (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 采样方法 (比如使用粒子滤波,而非本文的贪婪方法)。