Keypoint-Based LiDAR-Camera Online Calibration With Robust Geometric Network

Abstract

无可否认，激光雷达相机在线校准是一个极其复杂的问题。对于自动驾驶技术的发展，稳定、准确和快速的在线激光雷达相机校准系统变得非常重要。

由于深度学习在许多领域发挥着至关重要的作用，基于学习的方法有望解决这个问题。然而，现有方法没有充分考虑校准过程中的几何约束。在本研究中，我们提出了一种新的网络：RGKCNet，这是一种基于关键点的2-D–3-D姿态估计网络，可用于实时校准相机和激光雷达。使用声明层（declarative layer），外部校准被视为一个双层优化问题，它允许我们在端到端网络中嵌入基于几何约束的优化器，从而实现2-D–3-D数据关联。通过可训练的点加权层，网络可以提取稀疏关键点并给出用于姿态估计的相应权重，这进一步提高了网络的鲁棒性。我们在KITTI数据集上验证了所提出方法的性能，以证明在实际应用中的显著效果。

关键词——深度声明网络（DDN）、深度神经网络、激光雷达相机外部校准、透视n点问题（PnP）[Deep declarative network (DDN), deep neural network, LiDAR-camera extrinsic calibration, perspective-n-point problem (PnP).]。

INTRODUCTION

自动驾驶中的传感器主要有激光雷达、摄像机和IMU(惯性测量单元，加速度计)，雷达和相机的外部参数需要通过线性/非线性专门求解，这种方法不仅繁琐低效，而且难以获得高精度，不适合无人系统的长期运行。

当校准的传感器系统开始工作时，需要经常校正参数，因为传感器的位置可能会因各种原因（例如车辆运行中发生的抖动和颠簸）而略有变化。此时，准确、快速的在线校准技术至关重要。然而，在一些传统的在线校准技术中，该过程依赖于多帧的人工特征或自我运动，这不能适应复杂的环境。

由于深度学习的鲁棒拟合和泛化能力，已经提出了一些基于深度学习的在线校准方法，并取得了具体结果。这些方法使用一些类似的主干网络结构从RGB图像和深度图像中提取高维特征，然后通过多层感知（MLP）直接回归姿态。这种方法在中被称为绝对姿态回归（APR），并证明[20]APR方法类似于图像检索。尽管这些方法是有效的，但面对许多不同的场景和传感器，它们的泛化能力需要进一步加强。在本文的实验部分，我们还将所提出的方法与APR方法在不同情况下进行了比较。

除了直接回归，一些方法将姿态估计分为两部分。网络首先提取特征并检测关键点，然后通过透视n点（PnP）算法求解姿态。虽然这种方法可以获得更好的精度，但传统的PnP算法不能参与反向传播，这导致关键点不准确。

在这项工作中，我们提出了一种端到端的深度神经网络来解决LiDAR相机的在线校准，并将其命名为RGKCNet。该方法可用于多传感器系统的长期运行，例如自动驾驶平台。所提出的方法与现有的基于深度学习的校准方法非常不同。在数据处理部分，该方法直接处理原始图像和点云，而不是深度图，这保护了几何相关的空间信息。在姿态估计部分，充分考虑了2-D–3-D校准过程中的几何约束。通过使用深度声明性网络（DDN）,2-D–3-D数据关联和姿态估计被表述为一个双层优化问题，这允许我们将标准神经层和PnP解算器放在同一个端到端网络中。这样，既可以使用标准神经层的模式识别能力，也可以使用传统的几何解算器。为了提高网络的鲁棒性，引入了可学习的权重层来确定求解器中涉及的关键点，这使得整个网络能够进行端到端的训练。网络结构如图所示:

RELATED WORK

1. 离线方法
   离线方法基于特定标记，如校准板。然后提取一些特征来关联二维和三维数据，如角、线和平面。由于雷达点云的稀疏性，一些方法需要手动确定匹配点。根据这些几何约束来求解非线性最小二乘问题来估计姿态。这种方法缺少实时性，无法实现实时校正，这种方法很难在无人驾驶系统中广泛使用。

2. 传统线上方法

   传统的在线方法可以大致分为三种

   • 基于特征的方法——特征点

   • 基于运动的方法——多视图几何结构

   • 基于互信息的方法——最大化点云反射强度和图像灰度值之间的互信息

3. 基于学习的方法

   主要研究

   • RegNet、CalibNet通过将点云投影到图像平面上，统一点云和图像的数据格式方便进行学习，并通过一系列MLP回归雷达和相机的六自由度外部参数。

   • CalibRank认为基于学习的姿态回归只不过是具有最相关样本的插值，并提出了一种基于学习的排序流水线来提高校准精度。

   • CalibRCNN在网络的连续帧之间添加姿势约束，并通过LSTM网络提取时间特征。

   • RGGNet使用可变自动编码器作为公差调节器，动态生成外部校准的公差，这些方法一定程度上提高了CalibNet的性能，但是基本思想基本没有改变。

PROBLEM FORMULATION

A. 外参校准

文章方法用于自动驾驶的雷达和相机对齐，相机的参考系$C$和雷达的参考系$L$，配准就是去估计一个${\mathbf{T}}_L^C$：
$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0& 1\end{array}\right]\in SE(3)$$
在系统运行的过程中我们要去估计这个4×4的矩阵，假设初始校准参数${\mathbf{T}}_{Linit}^C$已知，我们需要做的就是将每一帧的错误校准${\mathbf{\Delta }\mathbf{T}}_L^C$估计出来：
$${\mathbf{T}}_L^C={\mathbf{\Delta }\mathbf{T}}_L^C\cdot {\mathbf{T}}_{Linit}^C$$
假设两个传感器之间有重叠的FOV，那么每帧的相机图像可以表示为大小为($H,W,3$)的张量${\mathbf{X}}_c$，每帧点云可以表示为大小为($N,4$)的张量${\mathbf{X}}_l$，4维信息包含了三维坐标${\mathbf{p}}_{\mathbf{i}}=(x_i,y_i,z_i)$和对应的反射强度。对应的公式：
$$z_{ci}\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]={\mathbf{KT}}_L^C\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]$$
这里的$\mathbf{K}$是相机内参，假设外参已经在相机校准过程中获得，$(u_i,v_i)$是3D点云$p_i$投影到2D图像上的坐标。如果获得了三维和二维的点之间的关系，雷达和相机的外参校准问题就变成了一个PnP问题，这时问题就转化为BA优化问题得到解决。

Bundle Adjustment是一种用于3D重建和计算机视觉中的优化算法。该算法的目标是优化相机的内部和外部参数以及场景中3D点的位置，以最小化相机观察到的图像中的重投影误差。
在Bundle Adjustment算法中，通常使用的模型是一个由相机参数和3D点组成的非线性模型。该模型中的相机参数通常包括内部参数（如焦距、畸变系数）和外部参数（如旋转矩阵和平移向量），而3D点则是在世界坐标系中的位置。Bundle Adjustment算法的目标是找到一组相机参数和3D点位置，使得这些参数和点的重投影误差最小。
Bundle Adjustment算法通常使用迭代的方式求解，每次迭代中都会更新相机参数和3D点位置，直到达到收敛条件。Bundle Adjustment算法是一种强大的优化算法，可以在多个相机和多个视角下进行3D重建，以及进行相机的姿态估计和相机标定等任务。

我们将上式重写：
$$\mathbf{u}=h(\xi ,\mathbf{p})$$
这里的$h$表示相机投影过程，而$\xi \in SE(3)$定义了六自由度的姿态参数，这个参数可以通过最小化重投影误差来估计：
$$\begin{array}{rl}{\xi }^{\ast }& =\underset{\xi }{\mathrm{argmin}}\frac{1}{2}\sum _{i=1}^m{\omega }_i\Vert {\mathbf{z}}_i-{\mathbf{u}}_i{\Vert }_2^2\\ & =\underset{\xi }{\mathrm{argmin}}\frac{1}{2}\sum _{i=1}^m{\omega }_i\Vert {\mathbf{z}}_i-h(\xi ,{\mathbf{p}}_{\mathbf{i}}){\Vert }_2^2\end{array}$$
这里的m就是2D-3D匹配点对个数，${\omega }_i$ 是${\mathbf{p}}_i$ 的权重，且${\sum }_{i=1}^m{\omega }_i=1$ 。
传统的校准方法会通过人工标记为来确定这种2D-3D对应关系，而且不会有权重的概念。但是，
对于Online的方法，这种信息无法准确获得，所以文中的方法将使用神经网络来估计每个关键点$p_i$对应的${\mathbf{z}}_i$和权重${\omega }_i$:
$$\begin{array}{rl}{\omega }_i& \to f_w({\mathbf{X}}_l,{\mathbf{p}}_i)\\ {\mathbf{z}}_i& \to f_{\mathbf{z}}({\mathbf{X}}_c,{\mathbf{X}}_l,{\mathbf{p}}_i)\end{array}$$
这里$f_{\omega },f_{\mathbf{z}}$分别代表了网络的俩分支，这之后就可以通过解决PnP来估计出姿态了。

B. 二级优化（Bilevel Optimization）

端到端网络的训练是一个将某些损失函数最小化的全局优化过程，文中提到的方法是一种二级优化问题${}^{[1]}$，得益于DDN网络${}^{[2]}$，二级优化问题可以被更好地解决。

[1] Bilevel Optimization（二级优化）是一种优化问题，它涉及到两个嵌套的优化问题，其中一个优化问题嵌套在另一个优化问题中。在Bilevel Optimization问题中，第一级（上级）问题的目标函数或约束条件取决于第二级（下级）问题的解决方案，而第二级问题本身也是一个优化问题。这种优化问题通常用于博弈理论、机器学习、控制论和经济学等领域。
例如，在机器学习中，Bilevel Optimization可以用来解决超参数优化问题。其中，第一级问题涉及在给定数据集上优化学习算法的超参数（如学习率、正则化参数等），第二级问题涉及在每次迭代中使用特定超参数来训练模型的权重参数。在这种情况下，第一级问题的目标是最小化测试误差，而第二级问题的目标是最小化训练误差。
Bilevel Optimization问题由于具有多个嵌套的优化问题，其求解相对复杂，需要使用特定的求解算法，如反向传播算法、次梯度算法、仿射缩放算法等。

[2]DDN(Deep declarative networks)是一种新型的端到端可学习模型，不同于传统的神经网络模型，DDN 中的节点（或网络层）不再是显式地定义前向计算函数，而是以期望的行为作为隐式定义的优化问题的解来表示。也就是说，DDN 将神经网络中的计算过程转化为一个数学优化问题，并将网络层的计算结果作为优化问题的解。这种隐式定义的节点可以被称为 declarative processing nodes。值得注意的是，DDN 模型是可以包含传统的深度学习模型的，因此可以看作是一种更加灵活和泛化能力更强的深度学习模型。同时，作者还介绍了如何在 PyTorch 深度学习库中实现 declarative processing nodes，以及展示了 DDN 模型在图像和点云分类任务中的应用示例。

DDN将网络结构分为两种类型：声明式层(Imperative Layer)和命令式层(Declarative Layer)：

左：声明式层，右：命令式层

对于左边的声明式层，输入$x$将通过一个显式参数化函数转换为$y$，${\theta }_1$和${\theta }_2$就是神经网络层中对应的权重，是训练过程中主要优化的部分，它们每层的梯度都可以定义。
声明式层可以清晰定义输入输出函数，像是基本的卷积层或是池化层。
对于右边的命令式层，它的输出$y$最小化带参数的目标函数，这种目标函数不确定的情况下我们无法直接获得梯度。
命令式层并没有前向通道的明确函数，它可以被表示为一个数学优化问题：
$$y\in \underset{u\in M}{\mathrm{argmin}}f(x,u;\theta )$$
其中$x$是函数输入，$y$是输出，$f()$代表了目标优化函数，$\theta$是该层的权重参数，$M$是约束条件下的所有取值。
在了解到这俩层之后我们可以通过下式表示二级优化问题：
$$\begin{array}{rl}& \min L(x,y)\\ & s.t.y\in \underset{u\in M}{\mathrm{argmin}}f(x,u)\end{array}$$

"s.t."是"subject to"的缩写，表示在约束条件下的限制。通常在一个优化问题中，我们要最小化或最大化一个目标函数，同时还要满足一些约束条件，那么就可以使用"s.t."来表示这些约束条件。

这里的$L()$是网络的全局损失函数，当$y$是关于$x$的函数时，我们需要计算如下的梯度来训练整个网络：
$$DL(x,y)=D_{X}L(x,y)+D_{Y}L(x,y)\cdot Dy(x)$$
因为声明式层的所有梯度都被明确定义，所以需要去计算$Dy(x)$让网络可以进行梯度的反向传播。
在之前求${\xi }^{\ast }$ 的式子中，将一个未知的六自由度姿态对齐到$SE(3{)}^{[1]}$，BA优化过程可以看作一个unconstrained optimization problem${}^{[2]}$，根据DDN的相关论文，我们知道可以通过下式来计算$D(y)$:
$$Dy(x)=-(D_{YY}^{2}f(x,y){)}^{-1}\cdot D_{XY}^{2}f(x,y)$$
其中的$D_{YY}^{2}f(x,y)$是非奇异的。

[1] SE(3)是三维特殊欧氏群（Special Euclidean group），表示在三维欧几里得空间中具有旋转和平移变换的刚体变换的集合。其中，“特殊”指的是变换中的旋转矩阵行列式为1，而“欧氏”指的是这些变换是在欧氏空间中进行的。变换矩阵T必须属于SE(3)中的元素，即变换必须同时满足旋转矩阵的行列式为1、平移向量是三维向量、最后一行为(0, 0, 0, 1)等条件，才能被认为是一个合法的刚体变换。
[2] Unconstrained optimization problem是指一个优化问题，其中没有任何约束条件。也就是说，优化问题的目标是最小化或最大化一个目标函数，而该函数的输入参数没有受到任何限制。这意味着可以在无限制的情况下调整参数，直到找到最优的解决方案。在这种情况下，优化问题的解可以是任何值，而不受限制。与之相反的是有约束的优化问题(constrained optimization problem)，其中目标函数的输入参数受到一些约束条件的限制。在这种情况下，最优解不一定是全局最优解，因为它必须满足所有约束条件。

METHODS

这部分介绍文章中所用网络的结构，总体如下图所示：

A. 特征提取层

网络输入的数据包括了点云和图像的重叠部分，在训练阶段需要雷达-相机外参参数的ground truth。首先网络需要从输入数据中提取特征描述子。特征提取部分被分为两部分——点云特征提取器和图像特征提取器。
点云部分文章中采用了PointNet++ 作为特征提取器，这一方法被证明可以有效解决点云无序的问题，并在各种点云处理网络中被证明是有效的。
图像部分的话，因为网络需要提取2D-3D匹配的特征点，文章中使用了微软的人体特征点提取网络结构。这种网络结构替代了传统的上采样模型，这个模块使用反卷积层替代了传统的上采样模块，并且在关键点提取领域取得了良好的结果。因此，相对于使用传统上采样模块的模型和使用深度学习特征点提取模型的方法，这个模块的优点在于更好的特征提取和更好的上采样方法。
这层的输出分别是大小为$(N,32)$的点云张量以及大小为$(H/4,W/4，32)$的像素特征。

B. 点云加权层

在进行点云特征提取之后，需要一个点云加权层来对提取的点云特征进行处理从而输出每个点的权重，以及top-k权重最大值的Index，这些点云加权层主要功能是：

1. 减少需要处理的点的数目来提升处理过程的效率
2. 提供解决PnP问题时需要的每个点的权重以及增强网络的鲁棒性
   点云权重层的结构如图所示：

层的输入是大小为$(N,32)$的点的局部特征，然后输出是大小为$(N_k,1)$的top-k权重，这部分的网络设计参考了DeepVCP,文章也充分证明了这种模型的有效性。在DeepVCP中，加权层可以在一定程度上消除点云中的动态对象。由于其训练数据包含点云帧的自我运动，动态对象上的点不能满足全局运动约束。

Pointwise local features（点级局部特征）是计算机视觉中一种用于图像处理和分析的技术。它是指对于图像中的每个像素点，提取该点周围一定范围内的局部特征，例如灰度值、颜色、纹理等信息。这些局部特征可以用来描述图像中的局部结构，并作为机器学习算法的输入。
在实际应用中，Pointwise local features通常与其他技术结合使用，例如SIFT（尺度不变特征变换）、SURF（速度加速稳健特征）、ORB（Oriented FAST and Rotated BRIEF）等特征提取算法，用于图像匹配、目标检测、三维重建等领域。

在这项工作中，输入数据是点云和图像的同一帧。当传感器时间同步时动态物体引起的误差几乎可以忽略。
点云加权层可以帮助找到更明显的局部特征来使匹配更加轻松，如道路主干、交通标志和道路边缘。

C. 点云特征Embedding层

和DeepVCP一样，在通过top-k获得了$N_k$个特征点之后，我们接下来需要获得更加细节的局部特征，这就是Point Feature Embedding Layer的功能。这里使用了Pointnet++的特征提取结构。不同之处就在于不再需要最远点采样层，因为这$N_k$个特征点将会被用作新的中心。采用多尺度分组可以保证多尺度特征提取，改善感受野。

如图所示，从点云加权层中选取的$N_k$个关键点的描述符、坐标和反射强度构成一个大小为$(N_k,36)$的张量，输入到点特征嵌入层。
点特征嵌入层使得三维描述符更加关注所选关键点附近的局部信息，有利于后续的对应关系生成。

D. 对应关系生成层

为了估计雷达和相机的外参，需要找到每个3D关键点在图像上对应的2D点，这就是此层的功能。层结构如下图所示：

首先，错位的关键点被投影到图像上，得到像素坐标$(N_k,2)$。根据像素的位置来通过裁剪操作从特征图中提取边长为d的矩形窗口，得到$N_k$个大小为$(32,d,d)$ 的张量[这里应该是图像特征]，它们与关键点的3D描述符结合起来形成$N_k$个大小为$(64,d,d)$的张量，作为对应生成层的输入。和其他关键点估计网络一样，对应关系生成层的输出是是反映图像关键点位置的Heatmaps。不同的是本文不使用Argmax函数来确定关键点坐标，因为该函数不可微。相反，我们使用局部数值坐标和Heatmaps的点积来获取相应的像素位置，这使得Heatmaps的所有像素向后传播梯度。
通过这种方式得到的像素坐标的精度小于一个像素。因此由特征提取层引起的精度损失可以被中和。
通过一系列具有共享权重的卷积层，对应关系生成层输出为大小为$(N_k,d,d)$的Heatmaps以及一组大小为$(N_k,2)$二维的特征点坐标。由于窗口外的点在数据关联中没有作用，该方法限制了像素坐标的偏差不能太大。然而，随着训练的进行，点权重层会给偏差较大的点分配较低的权重，网络倾向于选择偏差较小的点。CNN的强大的泛化能力在这一层起着重要的作用。通过第F节介绍的一系列几何约束，该层实现了2D-3D关键点关联的功能，并在实验中进行了可视化。

E. 姿态层

在获得非优化的2D对应点之后，使用姿态层来优化带权非线性PnP目标函数(${\xi }^{\ast }$ )并获得$T_L^C$。如二级优化相关表述所述，我们将位姿层视为必要层，并使用limited-memory Broyden-Fletcher-Goldfarb-Shanno(L-BFGS)算法来估计6自由度位姿。为了确保算法能够收敛到更好的局部最优解，首先使用随机抽样一致性(RANSAC)来找到初始值。在获得第一个结果后，我们执行两次PnP求解器，并拒绝25%的异常值以进一步提高准确性。除了使用BA，我们还尝试了EPnP和直接线性变换(DLT)，这些方法也可以进行反向传播梯度，并在实验中评估这些方法。
使用了一种基于几何约束的非线性优化方法，该方法可以更加稳健地估计关键点匹配误差，同时使用三种不同的损失函数来训练网络，包括预测损失、全局损失和正则化损失。作者还进行了消融实验来验证每个组件的效果，并得出结论：使用基于BA的非线性求解器比EPnP和DLT更好，$Los{s}_{reg}$可以提高系统的稳定性和收敛速度，点加权层和位姿层中的权重都能够提高系统的准确性，且在实际应用中可以通过多次运行选择结果。最后，作者通过实验结果表明，该方法的误差只与关键点匹配精度有关，与初始标定误差无关，并且误差集中在低值区域。