Pixel-level Extrinsic Self Calibration of High Resolution LiDAR and Camera in Targetless Environments

无目标环境中高分辨率激光雷达和相机的像素级外参估计

摘要

Our approach does not require checkerboards but can achieve pixel-level accuracy by aligning natural edge features in the two sensors. On the theory level, we analyze the constraints imposed by edge features and the sensitivity of calibration accuracy with respect to edge distribution in the scene. On the implementation level, we carefully investigate the physical measuring principles of LiDARs and propose an efficient and accurate LiDAR edge extraction method based on point cloud voxel cutting and plane fitting.

• 优点：无需目标棋盘格，像素级精度，室内外均可用
• 方法：基于高分辨率的雷达和相机的边缘特征，雷达是点云体素分割和平面拟合，相机是canny边缘检测，最小化重投影误差
• 理论：边缘特征的约束，场景边缘特征分布对外参估计的敏感性

引言

需求：dense point cloud mapping, colorization, and accurate and automated 3D surveying.

批判：提前准备标定物，雷达本身的噪点，稀疏，导致代价函数不稳定，远距离的场景导致标定误差变大。

自夸：解决批判的问题，随时随地可以标定，像素级效果。

贡献：

• 我们仔细研究了激光雷达的底层测量原理，发现常用的深度不连续边缘特征对于校准来说既不准确也不可靠。我们提出了一种新颖且可靠的深度连续边缘提取算法，可以产生更准确的校准参数。
• 我们评估我们的方法和在各种室内和室外环境中实施的鲁棒性、一致性和准确性，并将我们的方法与其他最先进的方法进行比较。结果表明，我们的方法对初始条件具有鲁棒性，与标定场景一致，并且在自然环境中实现了像素级标定精度。我们的方法的精度与基于目标的方法相当（有时甚至更好），并且适用于新兴固态和传统旋转激光雷达。
• 开源。

文献综述

基于目标：几何实体，棋盘格，有典型的坐标和特征，需要准备；

• 几何实体[9]-[11]和棋盘格[4]-[6]已广泛应用于基于目标的方法；

[9] J. Kummerle and T. Kuhner. Unified intrinsic and extrinsic camera and lidar calibration under uncertainties. 2020 IEEE International Conference on Robotics and Automation (ICRA), 2020.

[10] X. Gong, Y. Lin, and J. Liu. 3d lidar-camera extrinsic calibration using an arbitrary trihedron. Sensors, 13(2):1902–1918, 2013.

[11] Y. Park, S. Yun, C. Won, K. Cho, K. Um, and S. Sim. Calibration between color camera and 3d lidar instruments with a polygonal planar board. Sensors, 14(3):5333–5353, 2014.

[4] J. Cui, J. Niu, Z. Ouyang, Y. He, and D. Liu. Acsc: Automatic calibration for non-repetitive scanning solid-state lidar and camera systems, 2020.

[5] G. Koo, J. Kang, B. Jang, and N. Doh. Analytic plane covariances construction for precise planarity-based extrinsic calibration of camera and lidar. 2020 IEEE International Conference on Robotics and Automation (ICRA), 2020.

[6] L. Zhou, Z. Li, and M. Kaess. Automatic extrinsic calibration of a camera and a 3d lidar using line and plane correspondences. 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018.

无目标：

• LiDAR 点首先投影到图像平面上，并根据深度和反射率值着色。然后从该颜色图中提取 2D 边缘并与从图像中获得的边缘进行匹配。
• 通过最大化色彩图和图像之间的互信息来优化外在校准。
• 通过激光束深度不连续性从点云中检测并提取 3D 边缘。然后将 3D 边缘反投影到 2D 图像平面上以计算残差。边缘估计的准确性限制了该方法，因为激光点并不严格落在深度不连续边缘上。
• 外在因素是根据传感器的运动估计的，并通过外观信息进行细化。这种基于运动的校准通常需要传感器沿着足够的激励轨迹移动

作者：

• we directly extract 3D edge features in the point cloud, which suffer from no occlusion problem.
• we use depth-continuous edges, which proved to be more accurate and reliable.
• Our method works for a single pair LiDAR scan and achieves calibration accuracy comparable to target-based methods.

方法

A. 概述

雷达坐标系，相机坐标系，像素坐标系之间的关系如下图所示. ${}_L^C\mathbf{T}={(}_L^C\mathbf{R}{,}_L^C\mathbf{t})\in SE(3)$ ，表示雷达到相机之间的坐标转换，也可以表示雷达在相机坐标系下的坐标；基于室内室外的边缘特征实现估计这个${}_L^C\mathbf{T}$

LiDAR位姿相对于相机的以下自由度（DoF）无法区分：

由边缘特征施加的约束。蓝线代表 3D 边缘，其到图像平面（灰色平面）的投影产生相机测量值（红线）。沿 C 轴或 D 轴平移或绕 A 轴（即边缘本身）或 B 轴旋转（边缘穿过相机原点时除外）后，边缘仍保留在同一黄色平面上，因此在像平面上有相同的投影。这意味着，边缘上的这四个pose变换是不可区分的。

B. Edge Extraction and Matching

提取

• 点云投影到图像平面并从投影的点云中提取特征，例如边缘提取[12]和互信息相关性[13]。

  ​ 问题：

  点投影后特征提取的一个主要问题是遮挡引起的多值和零值映射。

  • 如图（a），如果相机位于激光雷达上方，则区域A由于遮挡而被相机观察到，而激光雷达观察不到，导致该区域投影后没有点（零值映射，间隙A区，图3（b））。
  • B区域是激光雷达观测到的，而不是相机观测到的，该区域内的点（图3（a）中的红点）投影后会干扰其前景点（图3（a）中的黑点）的投影。 3(a))。结果，前景和背景的点将对应于相同的图像区域（多值映射，区域B，图3（b））。
  • 这些现象对于低分辨率的 LiDAR 可能并不显着 [13]，但随着 LiDAR 分辨率的增加而变得明显（见图 3（b））。在投影点云上提取特征并将其与图像特征进行匹配（例如[13]）会遇到这些基本问题，并导致边缘提取和校准中出现重大错误。

• 因此直接在点云上提取这些特征，而不是投影之后进行特征提取

  问题：

  基于深度不连续的方法：方便提取但是对于精度较高的雷达，不稳定且不准确

  分析原因：

  前景膨胀：实际的激光脉冲并不是一个理想的点，而是具有一定发散角（即光束发散角）的光束。当从前景物体扫描到背景物体时，部分激光脉冲被前景物体反射，而剩余的激光脉冲被背景反射，产生两个反射脉冲到激光接收器。在前景物体反射率较高的情况下，由第一个脉冲引起的信号将占主导地位，即使光束中心线偏离前景物体，这也会导致前景物体的假点超出实际边缘（最左边的黄点）如图5(a))。

  出血点：在前景物体靠近背景的情况下，两个脉冲产生的信号会结合在一起，集总信号将导致一组连接前景和背景的点（称为出血点，即图 1 中的黄色点）。 5(a))。这两种现象会错误地使前景物体膨胀，并导致边缘提取（图5（b））和校准出现明显错误。

  基于深度连续的方法：

  1. 体素划分：室内0.5m，室内1m；
  2. 对于每一个体素进行RANSAC实现平面的拟合和提取；
  3. 保留连接并形成一定范围内（例如，[30°，150°]）角度的平面对，并求解平面相交线（即深度连续边）。
  4. 这些线在体素内彼此垂直或平行。此外，通过正确选择体素大小，我们甚至可以提取弯曲的边缘。

• 图像边缘检测使用canny实现
  • 边缘的提取放在k-D的二维的

匹配：

将雷达中边缘中采样，每一个点使用初始转换矩阵转换到相机坐标系下；
$$\begin{array}{c}{}^C{\mathbf{P}}_i={}_L^C\mathbf{T}({}^L{\mathbf{P}}_i)\in {\mathbb{R}}^3;{\mathbf{p}}_i=\mathbf{f}{(}^C{\mathbf{p}}_i)\end{array}$$
根据这个pi寻找像素中的2-D树中的最近邻像素，得到k最近邻的点，计算对应的均值和方差
$$\begin{array}{c}{\mathbf{q}}_i=\frac{1}{\kappa }\sum _{j=1}^{\kappa }{\mathbf{q}}_i^j;{\mathbf{S}}_i=\sum _{j=1}^{\kappa }({\mathbf{q}}_i^j-{\mathbf{q}}_i)({\mathbf{q}}_i^j-q_i{)}^T\end{array}$$
这样的话，根据 Q i = { q i j ; j = 1 , 2 , 3... κ } Q_i = \{ q_i^j;j = 1,2,3...{\kappa} \} Qi​={qij​;j=1,2,3...κ} 表示的直线包含参数为线上的点$q_i$和法向量$n_i$来表示。这个法向量对应S_i的最小特征值对应的特征向量；

另外将边缘方向投影到图像平面并验证其相对于ni的正交性。当两条非平行线在图像平面中彼此靠近时，这可以有效地消除错误匹配。

C. 外参校准

相机产生的测量噪声较为简单，认为1.5的标准差；

对于雷达
$$\begin{array}{c}{\omega }_i^{\mathrm{gt}}={\omega }_i{⊞}_{{\mathbb{S}}^2}{\delta }_{{\mathbf{\omega }}_i}\triangleq e^{\lfloor \mathbf{N}({\mathbf{\omega }}_i){\mathbf{\delta }}_{{\mathbf{\omega }}_i}\times \rfloor }{\omega }_i\end{array}$$

$$\begin{array}{c}{d}_i^{\mathrm{gt}}=d_i+{\delta }_{d_i}\end{array}$$

二者结合
$$\begin{array}{c}\begin{array}{rl}{}^L{\mathbf{P}}_i^{\mathrm{gt}}& =d_i^{\mathrm{gt}}{\omega }_i^{\mathrm{gt}}=(d_i+{\delta }_{d_i})\left({\omega }_i{⊞}_{{\mathbb{S}}^2}{\delta }_{{\mathbf{\omega }}_i}\right)\\ & \approx \underset{L_{{\mathbf{P}}_i}}{\underbrace{d_i{\omega }_i}}+\underset{L_{{\mathbf{W}}_i}}{\underbrace{{\omega }_i{\delta }_{d_i}-d_i\lfloor {\omega }_i\times \rfloor \mathbf{N}({\omega }_i){\delta }_{{\omega }_i}}}\end{array}\end{array}$$
得到雷达的测量噪声模型
$$\begin{array}{c}\begin{array}{rl}& {}^L{\mathbf{w}}_i=\underset{{\mathbf{A}}_i}{\underbrace{\left[\begin{array}{cc}{\mathbf{\omega }}_i& -d_i\lfloor {\mathbf{\omega }}_i\times \rfloor \mathbf{N}({\mathbf{\omega }}_i)\end{array}\right]}}\left[\begin{array}{c}{\delta }_{d_i}\\ {\delta }_{{\mathbf{\omega }}_i}\end{array}\right]\sim \mathcal{N}(0,{}^L{\Sigma }_i),\\ & {}^L{\mathbf{\Sigma }}_i={\mathbf{A}}_i\left[\begin{array}{cc}{\Sigma }_{d_i}& 0_{1\times 2}\\ 0_{2\times 1}& {\Sigma }_{{\mathbf{\omega }}_i}\end{array}\right]{\mathbf{A}}_i^T.\end{array}\end{array}$$
校准模型和优化
$$\begin{array}{c}0={\mathbf{n}}_i^T\left(\mathbf{f}\left(\pi {(}_L^C\mathbf{T}{(}^L{\mathbf{P}}_i{+}^L{\mathbf{w}}_i\right)\left)\right)-\left({\mathbf{q}}_i{+}^I{\mathbf{w}}_i\right)\right)\end{array}$$
使用优化的方式进行优化
$$\begin{array}{c}{}_L^C\mathbf{T}{=}_L^C\bar{\mathbf{T}}{⊞}_{SE(3)}\delta \mathbf{T}\triangleq \mathrm{Exp}(\delta \mathbf{T}){\cdot }_L^C\bar{\mathbf{T}}\end{array}$$

$$\begin{array}{c}\delta \mathbf{T}=\left[\begin{array}{c}\delta \mathbf{\theta }\\ \delta \mathbf{t}\end{array}\right]\in {\mathbb{R}}^6;\mathrm{Exp}(\delta \mathbf{T})=\left[\begin{array}{cc}{e}^{\lfloor \delta \mathbf{\theta }\times \rfloor }& \delta \mathbf{t}\\ 0& 1\end{array}\right]\in SE(3)\end{array}$$

将其使用一阶近似，写成残差和雅可比的形式
$$\begin{array}{c}\begin{array}{rl}0& ={\mathbf{n}}_i^T\left(\mathbf{f}\left(\pi {(}_L^{\mathbf{C}}\mathbf{T}{(}^L{\mathbf{P}}_i{+}^L{\mathbf{w}}_i\right)\left)\right)-\left({\mathbf{q}}_i{+}^I{\mathbf{w}}_i\right)\right)\\ & \approx {\mathbf{r}}_i+{\mathbf{J}}_{{\mathbf{T}}_i}\delta \mathbf{T}+{\mathbf{J}}_{{\mathbf{w}}_i}{\mathbf{w}}_i\end{array}\end{array}$$
对应的雅可比
$$\begin{array}{c}\begin{array}{rlrl}& {\mathbf{r}}_i& & ={\mathbf{n}}_i^T\left(\mathbf{f}(\pi {(}_L^C\bar{\mathbf{T}}{(}^L{\mathbf{P}}_i)\left)\right)-{\mathbf{q}}_i\right)\in \mathbb{R}\\ & {\mathbf{J}}_{{\mathbf{T}}_i}& & ={\mathbf{n}}_i^T\frac{\partial \mathbf{f}(\mathbf{p})}{\partial \mathbf{p}}\frac{\partial \pi (\mathbf{P})}{\partial \mathbf{P}}\left[-\lfloor {(}_L^C\bar{\mathbf{T}}{(}^L{\mathbf{P}}_i))\times \rfloor \mathbf{I}\right]\in {\mathbb{R}}^{1\times 6}\\ & {\mathbf{J}}_{{\mathbf{W}}_i}& & =\left[{\mathbf{n}}_i^T\frac{\partial \mathbf{f}(\mathbf{p})}{\partial \mathbf{p}}\frac{\partial \mathbf{\pi }(\mathbf{P})}{\partial \mathbf{P}}\frac{C}{L}\bar{\mathbf{R}}-{\mathbf{n}}_i^T\right]\in {\mathbb{R}}^{1\times 5}\\ & {\mathbf{W}}_i& & =\left[\begin{array}{c}{L}_{{\mathbf{W}}_i}\\ I_{{\mathbf{W}}_i}\end{array}\right]\in \mathcal{N}(0,{\mathbf{\Sigma }}_i),{\mathbf{\Sigma }}_i=\left[\begin{array}{cc}L{\mathbf{\Sigma }}_i& 0\\ \mathbf{\alpha }& I\mathbf{\Sigma }\end{array}\right]\in {\mathbb{R}}^{5\times 5}\end{array}\end{array}$$
进行移动
$$\begin{array}{c}\mathbf{v}\triangleq -{\mathbf{J}}_{\mathbf{w}}\mathbf{w}=\mathbf{r}+{\mathbf{J}}_{\mathbf{T}}\delta \mathbf{T}\sim \mathcal{N}(0,{\mathbf{J}}_{\mathbf{w}}\mathbf{\Sigma }{\mathbf{J}}_{\mathbf{w}}^{\mathbf{T}}).\end{array}$$
那么最大似然估计，
$$\begin{array}{c}\begin{array}{rl}& \underset{\delta \mathbf{T}}{\max }\log p(\mathbf{v};\delta \mathbf{T})=\underset{\delta \mathbf{T}}{\max }\log \frac{e^{-\frac{1}{2}{\mathbf{v}}^T{\left({\mathbf{J}}_{\mathbf{w}}\mathbf{\Sigma }{\mathbf{J}}_{\mathbf{w}}^T\right)}^{-1}\mathbf{v}}}{\sqrt{(2\pi {)}^N\det \left({\mathbf{J}}_{\mathbf{w}}\mathbf{\Sigma }{\mathbf{J}}_{\mathbf{w}}^T\right)}}\\ & =\underset{\delta \mathbf{T}}{\min }(\mathbf{r}+{\mathbf{J}}_{\mathbf{T}}\delta \mathbf{T}{)}^T{\left({\mathbf{J}}_{\mathbf{w}}\mathbf{\Sigma }{\mathbf{J}}_{\mathbf{w}}^T\right)}^{-1}(\mathbf{r}+{\mathbf{J}}_{\mathbf{T}}\delta \mathbf{T})\end{array}\end{array}$$
得到优化的解
$$\begin{array}{c}\delta {\mathbf{T}}^{\ast }=-{\left({\mathbf{J}}_{\mathbf{T}}^T{\left({\mathbf{J}}_{\mathbf{w}}\mathbf{\Sigma }{\mathbf{J}}_{\mathbf{w}}^T\right)}^{-1}{\mathbf{J}}_{\mathbf{T}}\right)}^{-1}{\mathbf{J}}_{\mathbf{T}}^T{\left({\mathbf{J}}_{\mathbf{w}}\mathbf{\Sigma }{\mathbf{J}}_{\mathbf{w}}^T\right)}^{-1}\mathbf{r}\end{array}$$
进而更新这个pose
$$\begin{array}{c}{}_L^C\bar{\mathbf{T}}{\leftarrow }_L^C\bar{\mathbf{T}}{⊞}_{SE(3)}\delta {\mathbf{T}}^{\ast }.\end{array}$$
校准的不确定性

根据10表达式，两边乘以${\mathbf{J}}_{\mathbf{T}}^T{\left({\mathbf{J}}_{\mathbf{w}}\mathbf{\Sigma }{\mathbf{J}}_{\mathbf{w}}^T\right)}^{-1}$ ,然后求解出$\delta \mathbf{T}$得到
$$\begin{array}{c}\begin{array}{rl}{\delta }^{\mathrm{T}}& \approx \underset{\delta {\mathbf{T}}^{\ast }}{\underbrace{-{\left({\mathbf{J}}_{\mathbf{T}}^T{\left({\mathbf{J}}_{\mathbf{w}}\mathbf{\Sigma }{\mathbf{J}}_{\mathbf{w}}^T\right)}^{-1}{\mathbf{J}}_{\mathbf{T}}\right)}^{-1}{\mathbf{J}}_{\mathbf{T}}^T{\left({\mathbf{J}}_{\mathbf{w}}\mathbf{\Sigma }{\mathbf{J}}_{\mathbf{w}}^T\right)}^{-1}\mathbf{r}}}\\ & -{\left({\mathbf{J}}_{\mathrm{T}}^T{\left({\mathbf{J}}_{\mathrm{w}}\mathbf{\Sigma }{\mathbf{J}}_{\mathrm{w}}^T\right)}^{-1}{\mathbf{J}}_{\mathrm{T}}\right)}^{-1}{\mathbf{J}}_{\mathrm{T}}^T{\left({\mathbf{J}}_{\mathrm{w}}\mathbf{\Sigma }{\mathbf{J}}_{\mathrm{w}}^T\right)}^{-1}{\mathbf{J}}_{\mathrm{w}}\mathbf{w}\\ & \sim \mathcal{N}\left(\delta {\mathbf{T}}^{\ast },{\left({\mathbf{J}}_{\mathbf{T}}^T{\left({\mathbf{J}}_{\mathbf{w}}\mathbf{\Sigma }{\mathbf{J}}_{\mathbf{w}}^T\right)}^{-1}{\mathbf{J}}_{\mathbf{T}}\right)}^{-1}\right)\end{array}\end{array}$$
这个协方差可以表示外参校准的不确定性

D. 校准的边缘分布分析

(12)中的雅可比$J_{T_i}$表示残差对于外在变化的敏感性。在边缘特征非常少或分布不良的情况下，$J_{T_i}$ 可能非常小，导致估计不确定性（协方差）很大，如（16）所示。从这个意义上说，数据质量由（16）中的协方差矩阵自动定量编码。在实践中，在数据收集之前对校准场景进行快速、粗略的评估通常很有用。这可以通过分析推导雅可比 $J_{T_i}$来实现。忽略畸变模型，将针孔投影模型π(·)代入式(11)，得到
$$\begin{array}{c}{\mathbf{J}}_{T_i}={\mathbf{n}}_i^T\left[\begin{array}{cccccc}\frac{-f_x{X}_i{Y}_i}{Z_i^2}& f_x+\frac{f_x{X}_i^2}{Z_i^2}& \frac{-f_x{Y}_i}{Z_i}& \frac{f_x}{Z_i}& 0& -\frac{f_x{X}_i}{Z_i^2}\\ -f_y-\frac{f_y{Y}_i^2}{Z_i^2}& \frac{f_y{X}_i{Y}_i}{Z_i^2}& \frac{f_y{X}_i}{Z_i}& 0& \frac{f_y}{Z_i}& \frac{-f_y{Y}_i}{Z_i^2}\end{array}\right]\end{array}$$
说明，图像中的边缘特征要分布均匀一些，同时深度适中使得这个矩阵大一些，使得不确定性小一些；

E. 初始化和粗校准

需要给出一个初值最大化下面的值即可，每次给定范围内的旋转0.5°和平移2cm
$$\begin{array}{c}P.C.=\frac{N_{match}}{N_{sum}}\end{array}$$

实验和分析

雷达：Avia 和D435i,大约20s即可累积足够的点

这些初始值是在从 CAD 模型获得的值的邻域（旋转 ±5° 和 ±10cm）中随机采样的。耗时不到60s

稳健性验证，准确性验证，交叉验证，比较实验

不好的场景

• 当场景包含所有圆柱形对象时。由于边缘提取是基于平面拟合的，圆形物体会导致边缘提取不准确。此外，圆柱形物体也会产生视差问题，从而降低校准精度。
• 其次是场景中边缘分布不均匀。例如，大部分边缘分布在图像的上部，这形成了较差的约束，很容易受到测量噪声的影响。
• 当场景仅包含沿一个方向（例如垂直）的边缘时，其中约束不足以唯一地确定外部参数。

通用性

除了 Livox Avia 之外，我们的方法还可以应用于传统的多线旋转激光雷达，由于重复扫描，其在静止时具有较低的分辨率。

• 为了提高扫描分辨率，可以稍微移动激光雷达（例如，小的俯仰运动），以便填充扫描线之间的间隙。
• LiDAR（惯性）里程计可用于跟踪 LiDAR 运动并将所有点注册到其初始姿态，从而实现更高分辨率的扫描，从而能够使用我们的方法。

实际操作流程

• 雷达的是否是稠密的，如果不是需要累积点云地图，累积的方式可以是FASTLIO或者直接提取；
• 选择合适的场景，不能太近，也不能太远，特征分布要均匀在整个图像中，使得在x，y，z三个方向都有约束，场景不要太杂乱，不要有圆柱场景；
• 最好是几个箱子，正对着雷达和相机；
• 相机标定可以使用matlab或者calib开源库，注意畸变参数书写方式为$[k1,k2,p1,p2,k3]$（opencv格式）；
• 这些初始值是在从 CAD 模型获得的值的邻域（旋转 ±5° 和 ±10cm）中随机采样的，或者采取默认值；

文章参考

1. https://arxiv.org/abs/2103.01627