本文介绍一个 高分辨率激光雷达(Livox)和相机的外参标定方法(targetless方法),该方法通过 对齐点云和图像中分别提取的自然场景 边缘,实现像素级精度。
论文:Pixel-level Extrinsic Self Calibration of High Resolution LiDAR and Camera in Targetless Environments
作者 : Chongjian Yuan (香港大学)
Github: hku-mars/livox camera calib
可多看看github issue部分的讨论,加深对算法的理解
1 动机
针对以下挑战:
- 机械式激光雷达的重复扫描特性和不可避免的震动,导致点云稀疏且噪声明显,固态LiDAR (比如Livox)非重复扫描,点云密集,能弥补该缺点。
- target-based方法中,一般靠近传感器放置标定目标,外参误差会随距离放大。另外,需要准备标定目标,不方便。
- 有的targetless方法,先投影再提取特征,受遮挡影响;有的方法采用depth-discontinuous点云边缘特征,不够准确可靠。
做出如下贡献:
- 分析边缘特征提供的约束数量,边缘分布对标定结果的影响。
- 基于激光雷达测量原理,分析常用的基于深度不连续(depth-discontinuous)的点云边缘的弊端,提出深度连续(depth-continuous)的边缘提取方法。
- 不同室内外场景验证方法的有效性,堪比target-based方法。
2 方法
2.1 边缘约束
一对边缘特征可以提供2个约束。如下图,蓝色边为点云中3D边缘,红色边为图像中对应的2D边缘。蓝色边缘沿着C,D方向的平移和A,B方向的旋转不会改变投影结果,所以自由度为4,提供2个约束。
2.2 边缘提取
对于点云边缘提取,一种思路先将点云投影到图像平面再提取特征,这种思路会因为遮挡而导致多值(multi-valued)和零值(zero-valued)投影现象。
如下图,A区域相机可见,雷达不可见,故没有投影点,产生零值现象。B区域激光雷达可见,相机不可见,B区域背景点会投影到前景目标(黑点)处,造成多值现象。
另一种思路,是直接在点云上提取边缘。有两种边缘:深度不连续的(depth-discontinuous),深度连续的(depth-continuous)。如下图,深度连续边缘指平面相交处边缘,其深度连续变化。深度不连续边缘则指前景、背景之间深度跃变的边缘。
但是深度不连续的边缘并不可靠。理由如下:由于一个激光脉冲并不是一个理想的点,而是有一定发散角度的光束,当从前景目标扫描到背景目标时,一部分激光脉冲被前景目标反射,一部分被背景目标反射,会产生两个反射脉冲。当前景反射强度大,第一个反射脉冲占主导地位,即使光束中心线偏移了前景目标,这仍会导致超出前景目标边缘的假的目标点。(如下图A处最左侧黄点)。当前景目标与背景比较靠近,两个脉冲会合并,会产生连接前后景目标的点。(称为bleeding points, A处黄点)这两种现象会使前景膨胀,造成边缘提取误差。
因此,本文提取深度连续的点边缘,分三步:
1)点云体素化 ,体素大小是室外1m,室内0.5m,
2)在体素中,使用RANSAC拟合提取平面,
3)保存互相连接,且角度在30-150度的平面对。平面相交提取边缘。
对于图像边缘提取,使用Canny算法。提取的边缘保存在k-D树(k=2)中,用于匹配。
2.3 边缘匹配
首先,将点云边缘点 L P i ^{L}P_i LPi依次进行欧式变换 L C T ‾ ^C_L\overline{T} LCT,投影 π \pi π,畸变纠正 f f f,得到对应的像素点 p i p_i pi。然后,在图像边缘像素构建的K-d树中,搜索 p i p_i pi的 k k k近邻 Q i = { q i j ; j = 1 , ⋅ ⋅ ⋅ , k } Q_i = \{q_i^j;j=1,\cdot\cdot\cdot,k\} Qi={qij;j=1,⋅⋅⋅,k}。求平均计算 Q i Q_i Qi的中心点 q i q_i qi,通过协方差特征值分解计算其边缘法向量 n i n_i ni。 { q i , n i } \{ q_i,n_i\} {qi,ni}就是对应图像边缘的参数表示。
另外,也将边缘方向投影到图像平面,验证其与
n
i
n_i
ni的正交性,去除当两个靠近但非平行的线构成的错误匹配。
2.4 外参标定
2.4.1 测量噪声
图像边缘点
q
i
q_i
qi的测量噪声:
I
w
i
∈
N
(
0
,
I
Σ
i
)
^I{w_i} \in \mathcal N(0,^I\Sigma_i)
Iwi∈N(0,IΣi)
点云边缘点
L
P
i
^LP_i
LPi的测量噪声:
L
w
i
∈
N
(
0
,
L
Σ
i
)
^Lw_i \in \mathcal N(0,^L\Sigma_i)
Lwi∈N(0,LΣi)
此处细节请参考论文
2.4.2 外参优化
考虑测量噪声,将点云边缘点
L
P
i
^LP_i
LPi投影到图像平面,与图像边缘
{
q
i
,
n
i
}
\{ q_i,n_i\}
{qi,ni}构成约束公式
(利用点在线上约束):
0
=
n
i
T
(
f
(
π
(
L
C
T
(
L
P
i
+
L
w
i
)
)
)
−
(
q
i
+
I
w
i
)
)
(1)
0=n_i^T(f(\pi(^C_LT(^LP_i + ^Lw_i)))-(q_i + ^Iw_i)) \tag{1}
0=niT(f(π(LCT(LPi+Lwi)))−(qi+Iwi))(1)
由(1)式,可知一个边缘点提供一个约束,因此一条边缘线(对应两个独立点)则提供2个约束。上式是非线性的,为了迭代求解,需要线性化
,一阶泰勒展开(1)式得到:
0
=
n
i
T
(
f
(
π
(
L
C
T
(
L
P
i
+
L
w
i
)
)
)
−
(
q
i
+
I
w
i
)
)
≈
r
i
+
J
T
i
δ
T
+
J
w
i
w
i
(2)
\begin{aligned} 0 &=n_i^T(f(\pi(^C_LT(^LP_i + ^Lw_i)))-(q_i + ^Iw_i)) \\ & \approx \bold r_i + \bold J_{\bold T_i} \delta \bold T + \bold J_{\bold w_i} \bold w_i \tag{2} \end{aligned}
0=niT(f(π(LCT(LPi+Lwi)))−(qi+Iwi))≈ri+JTiδT+Jwiwi(2)
其中,
r
i
\bold r_i
ri是外参初值
L
C
T
‾
^C_L \overline T
LCT对应的公式近似值,
J
T
i
\bold J_{\bold T_i}
JTi是对外参的导数,
J
w
i
\bold J_{\bold w_i}
Jwi是对噪声的导数。
结合(2),组合多个点对应的公式,,得到误差函数
v
=
−
J
w
w
=
r
+
J
T
δ
T
∼
N
(
0
,
J
w
Σ
J
w
T
)
(3)
\bold v = - \bold {J_w} \bold w = \bold r + \bold J_{\bold T} \delta \bold T \sim \mathcal N(0,\bold {J_w}\Sigma \bold {J_w^T}) \tag{3}
v=−Jww=r+JTδT∼N(0,JwΣJwT)(3)
(3)对应平差中的
v
=
B
x
−
l
v = Bx-l
v=Bx−l,从最大似然估计角度,得到目标函数
如下:
min
δ
T
(
r
+
J
T
δ
T
)
T
(
J
w
Σ
J
w
T
)
−
1
(
r
+
J
T
δ
T
)
(4)
\min_{\delta T} ( \bold r + \bold J_{\bold T} \delta \bold T )^T(\bold {J_w}\Sigma \bold {J_w^T})^{-1} (\bold r + \bold J_{\bold T} \delta \bold T) \tag{4}
δTmin(r+JTδT)T(JwΣJwT)−1(r+JTδT)(4)
对应平差中
min
v
T
P
v
\min v^TPv
minvTPv ,其中
P
P
P为权重,也是协方差的逆。令(4)求导等于0,得到参数
δ
T
\delta \bold T
δT最优估计
:
δ
T
∗
=
−
(
J
T
T
(
J
w
Σ
J
w
T
)
−
1
J
T
)
−
1
J
T
T
(
J
w
Σ
J
w
T
)
−
1
r
(5)
\delta \bold T^* = -(\bold J_T^T(\bold {J_w}\Sigma \bold {J_w^T})^{-1}\bold J_T)^{-1}\bold J_T^T(\bold {J_w}\Sigma \bold {J_w^T})^{-1} \bold r \tag{5}
δT∗=−(JTT(JwΣJwT)−1JT)−1JTT(JwΣJwT)−1r(5)
对应平差中
x
=
(
B
T
P
B
)
−
1
B
T
P
l
x = (B^{T}PB)^{-1}B^{T}Pl
x=(BTPB)−1BTPl。得到
δ
T
\delta \bold T
δT即可对
L
C
T
‾
^C_L\overline T
LCT更新
:
L
C
T
‾
←
L
C
T
‾
⊕
δ
T
(6)
^C_L\overline \bold T \leftarrow ^C_L\overline \bold T \oplus \delta \bold T \tag{6}
LCT←LCT⊕δT(6)
2.4.3 估计的不确定性
可以用
δ
T
\delta \bold T
δT的协方差表示估计的不确定性,协方差越大,不确定性越大。由上一节,可得
w
\bold w
w的协方差是
Σ
\Sigma
Σ,
v
\bold v
v的协方差是
J
w
Σ
J
w
T
\bold {J_w} \Sigma \bold {J_w^T}
JwΣJwT,根据(3)式,可推出
δ
T
\delta \bold T
δT的协防差:
Σ
T
=
(
J
T
T
(
J
w
Σ
J
w
T
)
−
1
J
T
)
−
1
(7)
\bold \Sigma_T = (\bold {J_T^T}(\bold {J_w} \bold \Sigma \bold {J_w^T})^{-1} \bold {J_T})^{-1} \tag{7}
ΣT=(JTT(JwΣJwT)−1JT)−1(7)
这里的不确定性,与平差中的权重(协方差的逆)、控制理论中的可观测性是相通的,即判断现有观测数据(及其估计结果)的可靠性。
2.4.4 边缘分布的影响
通过上一节,知道可以用(7)计算协方差 Σ T \bold {\Sigma_T} ΣT来快速评估场景数据质量。当边缘分布不佳, J t i \bold {J_{t_i}} Jti就会比较小,造成 Σ T \bold {\Sigma_T} ΣT较大。
如何挑选好的场景:
- 边缘在场景中均匀分布
- 因为雷达噪声随距离增加,标定场景深度适中
3 实验
激光雷达采用Livox AVIA, 相机采用Intel Realsense-D435i。
当激光雷达和相机同一朝向放置,其外参变换矩阵初值可设置为:
[0.0 , -1.0, 0.0 , tx
0.0, 0.0, -1.0, ty
1.0 , 0.0, 0.0, tz
0.0 , 0.0, 0.0 , 0]