【LCC系列】不用标定目标，实现高分辨率激光雷达和相机的像素级自标定（2020）

本文介绍一个 高分辨率激光雷达(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^C\overline{T}$，投影$\pi$，畸变纠正$f$，得到对应的像素点$p_i$。然后，在图像边缘像素构建的K-d树中，搜索$p_i$的$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$，通过协方差特征值分解计算其边缘法向量$n_i$。 { q i , n i } \{ q_i,n_i\} {qi​,ni​}就是对应图像边缘的参数表示。

另外，也将边缘方向投影到图像平面，验证其与$n_i$的正交性，去除当两个靠近但非平行的线构成的错误匹配。

2.4 外参标定

2.4.1 测量噪声

图像边缘点$q_i$的测量噪声：${}^I{w}_i\in \mathcal{N}(0{,}^I{\Sigma }_i)$
点云边缘点${}^L{P}_i$的测量噪声：${}^L{w}_i\in \mathcal{N}(0{,}^L{\Sigma }_i)$

此处细节请参考论文

2.4.2 外参优化

考虑测量噪声，将点云边缘点${}^L{P}_i$投影到图像平面，与图像边缘 { q i , n i } \{ q_i,n_i\} {qi​,ni​}构成约束公式（利用点在线上约束）：
$$0=n_i^T(f(\pi {(}_L^{C}T{(}^L{P}_i{+}^L{w}_i)))-(q_i{+}^I{w}_i))\text{\hspace{1em}(1)}$$
由(1)式，可知一个边缘点提供一个约束，因此一条边缘线（对应两个独立点）则提供2个约束。上式是非线性的，为了迭代求解，需要线性化，一阶泰勒展开（1）式得到：
$$\begin{array}{rl}0& =n_i^T(f(\pi {(}_L^{C}T{(}^L{P}_i{+}^L{w}_i)))-(q_i{+}^I{w}_i))\\ & \approx {\mathbf{r}}_i+{\mathbf{J}}_{{\mathbf{T}}_i}\delta \mathbf{T}+{\mathbf{J}}_{{\mathbf{w}}_i}{\mathbf{w}}_i\end{array}\text{\hspace{1em}(2)}$$
其中，${\mathbf{r}}_i$是外参初值${}_L^C\overline{T}$对应的公式近似值，${\mathbf{J}}_{{\mathbf{T}}_i}$是对外参的导数，${\mathbf{J}}_{{\mathbf{w}}_i}$是对噪声的导数。

结合（2），组合多个点对应的公式，，得到误差函数
$$\mathbf{v}=-{\mathbf{J}}_{\mathbf{w}}\mathbf{w}=\mathbf{r}+{\mathbf{J}}_{\mathbf{T}}\delta \mathbf{T}\sim \mathcal{N}(0,{\mathbf{J}}_{\mathbf{w}}\Sigma {\mathbf{J}}_{\mathbf{w}}^{\mathbf{T}})\text{\hspace{1em}(3)}$$
(3)对应平差中的$v=Bx-l$,从最大似然估计角度，得到目标函数如下：
$$\underset{\delta T}{\min }(\mathbf{r}+{\mathbf{J}}_{\mathbf{T}}\delta \mathbf{T}{)}^T({\mathbf{J}}_{\mathbf{w}}\Sigma {\mathbf{J}}_{\mathbf{w}}^{\mathbf{T}}{)}^{-1}(\mathbf{r}+{\mathbf{J}}_{\mathbf{T}}\delta \mathbf{T})\text{\hspace{1em}(4)}$$
对应平差中$\min v^{T}Pv$ ,其中$P$为权重，也是协方差的逆。令（4）求导等于0，得到参数$\delta \mathbf{T}$最优估计：
$$\delta {\mathbf{T}}^{\ast }=-({\mathbf{J}}_T^T({\mathbf{J}}_{\mathbf{w}}\Sigma {\mathbf{J}}_{\mathbf{w}}^{\mathbf{T}}{)}^{-1}{\mathbf{J}}_T{)}^{-1}{\mathbf{J}}_T^T({\mathbf{J}}_{\mathbf{w}}\Sigma {\mathbf{J}}_{\mathbf{w}}^{\mathbf{T}}{)}^{-1}\mathbf{r}\text{\hspace{1em}(5)}$$
对应平差中$x=(B^{T}PB{)}^{-1}{B}^{T}Pl$。得到$\delta \mathbf{T}$即可对${}_L^C\overline{T}$更新:
$${}_L^C\overline{\mathbf{T}}{\leftarrow }_L^C\overline{\mathbf{T}}\oplus \delta \mathbf{T}\text{\hspace{1em}(6)}$$

2.4.3 估计的不确定性

可以用$\delta \mathbf{T}$的协方差表示估计的不确定性，协方差越大，不确定性越大。由上一节，可得$\mathbf{w}$的协方差是$\Sigma$, $\mathbf{v}$的协方差是${\mathbf{J}}_{\mathbf{w}}\Sigma {\mathbf{J}}_{\mathbf{w}}^{\mathbf{T}}$,根据（3）式，可推出$\delta \mathbf{T}$的协防差：
$${\mathbf{\Sigma }}_T=({\mathbf{J}}_{\mathbf{T}}^{\mathbf{T}}({\mathbf{J}}_{\mathbf{w}}\mathbf{\Sigma }{\mathbf{J}}_{\mathbf{w}}^{\mathbf{T}}{)}^{-1}{\mathbf{J}}_{\mathbf{T}}{)}^{-1}\text{\hspace{1em}(7)}$$

这里的不确定性，与平差中的权重（协方差的逆）、控制理论中的可观测性是相通的，即判断现有观测数据（及其估计结果）的可靠性。

2.4.4 边缘分布的影响

通过上一节，知道可以用(7)计算协方差${\mathbf{\Sigma }}_{\mathbf{T}}$来快速评估场景数据质量。当边缘分布不佳，${\mathbf{J}}_{{\mathbf{t}}_{\mathbf{i}}}$就会比较小，造成${\mathbf{\Sigma }}_{\mathbf{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]

参考：https://github.com/hku-mars/livox_camera_calib/issues/4