0. 简介

多激光雷达与相机外参标定对于机器人和自动驾驶领域来说是非常关键的，特别是对于固态LiDAR而言，其中每个LiDAR单元的视场(Field-of-View，FoV)非常小，通常需要集体使用多个单元。大多数外部标定方法是针对360°机械旋转LiDAR提出的，其中假设视场与其他LiDAR或相机传感器重叠。很少有研究工作专注于小FoV LiDAR和相机的标定，也没有关注标定速度的提高。在《Targetless Extrinsic Calibration of Multiple Small FoV LiDARs and Cameras using Adaptive Voxelization》中，我们考虑了小FoV LiDAR和相机之间的外部标定问题，旨在缩短总标定时间并进一步提高标定精度。我们首先在LiDAR特征点的提取和匹配中实现了自适应体素化技术。这个过程可以避免在LiDAR外部标定中冗余地创建k-d树，并以比现有方法更可靠和更快的方式提取LiDAR特征点。然后，我们将多个LiDAR的外部标定转化为LiDAR束调整(LiDAR Bundle Adjustment，BA)问题。通过推导二阶代价函数，进一步提高了非线性最小二乘问题的求解时间和精度。我们提出的方法已在四个无目标场景和两种具有完全不同扫描模式、密度和FoV的固态LiDAR下的数据上进行了验证。我们的工作的鲁棒性也在八个初始设置下进行了验证，每个设置包含100个独立试验。与最先进的方法相比，我们的工作在LiDAR-LiDAR外部标定方面提高了15倍的标定速度（来自100个独立试验的平均结果），在LiDAR-相机外部标定方面提高了1.5倍的标定速度（来自50个独立试验的平均结果），同时保持准确性。为了造福机器人学界，该文章的相关代码已经在GitHub上开源了。

1. 主要贡献

本文首先通过使用我们最近提出的高效捆绑调整（BA）方法来注册它们的点云，从而校准激光雷达之间的外部参数（并同时估计激光雷达的位姿）[4]。为了减少激光雷达之间特征对应匹配的时间消耗，我们实现了自适应体素化，将点云动态分割成多个体素，以便每个体素中只有一个平面特征（见第III-B节）。然后，我们通过匹配图像和上述重建的点云之间的共视特征来校准相机和激光雷达之间的外部参数。为了进一步加速特征对应匹配，我们继承了上述自适应体素图，提取激光雷达的边缘特征。总之，我们的贡献如下：

• 我们提出了一种适用于多个小视场激光雷达和相机的无需目标的外部参数校准流程，这些传感器之间几乎没有或者没有视场重叠。我们将激光雷达外部参数校准形式化为一个捆绑调整问题，并将自适应体素化技术应用于激光雷达特征提取和匹配过程中。整个流程具有更高的校准精度和计算效率。
• 我们通过在不同测试场景中使用不同扫描模式、视场和点密度的激光雷达收集的数据来验证我们提出的方法。与各种最先进的方法相比，我们提出的方法可以提高多个激光雷达校准的速度15倍，多个激光雷达-相机校准的速度1.5倍。同时，我们提出的方法保持了高精度的校准，平均平移和旋转误差为激光雷达-相机校准为6毫米和0.09度，激光雷达-激光雷达校准为8毫米和0.2度。
• 我们在GitHub上以ROS开源我们的实现，以造福机器人技术社区。

2. 程序运行

对整个工程的介绍,整个工程代码是比较清晰且代码还比较友好。

config文件下有相机的标定参数以及程序中需要用到的阈值

Voxel.down_sample_size: 0.02   下采样的体素的大小
Plane.min_points_size: 60  平面特征的最小数量
Ransac.dis_threshold: 0.03  RANSAC 的最小距离
Ransac.iter_num: 200  迭代次数

include 头文件其中common.h，mypcl.hpp是公共的头文件。

source为主程序cpp文件，实现了三部标定激光雷达外参的代码以及激光雷达和相机外参标定的代码。并提供了将点云转化为dat文件的功能。

scene1，scene2为作者提供的demo数据集，以及需要的初始位姿和外参。

launch文件下保存着为运行标定的launch文件。

激光雷达之间的外参标定步骤

Step 1: 基于激光雷达的点云位姿优化(需要提供初始位姿在 scene-x/original_pose)

roslaunch mlcc pose_refine.launch

Step 2: 激光雷达外参的优化 (需要提供初始的外参在config/init_extrinsic)

roslaunch mlcc extrinsic_refine.launch

3. 概述

令 ^B_A T = (^B_A R, ^B_A t) ∈ SE(3) 表示从A坐标系到B坐标系的刚体变换，其中 ^B_A R ∈ SO(3) 和 ^B_A t ∈ \mathbb{R}^3 分别表示旋转和平移。我们用 L = {L_0 , L_1 , · · · , L_{n−1}} 表示n个激光雷达的集合，其中 L_0 表示基准激光雷达，C = {C_0 , C_1 , · · · , C_h } 表示h个相机的集合，\xi_L = { ^{L_0} _{L_1} T, ^{L_0} _{L_2} T, · · · , ^{L_0} _{L_{n-1}} T} 表示激光雷达与基准激光雷达之间的外参参数集合，\xi_C ={ ^{C_0}_{L_0} T, ^{C_1}_{L_0} T, · · · , ^{C_L}_{L_0} T }表示激光雷达与相机之间的外参参数集合。为了在可能没有视场重叠的多个激光雷达和相机之间创建共视特征，我们将机器人平台旋转到m个姿态，以便所有传感器都扫描相同的感兴趣区域（见图2）。令 T = {t_0 , t_1 , · · · , t_{m−1} } 表示每个姿态的时间，初始时刻基准激光雷达的姿态为全局坐标系，即 ^G_{L_0} T_{t_0} = I_{4×4} 。令 S = { ^G_{L_0} T_{t_1} , ^G_{L_0} T_{t_2} , · · · , ^G_{L_0} T_{t_{m-1}} } 表示基准激光雷达在全局坐标系中的姿态集合。激光雷达 L_i ∈ L 在时间 t_j ∈ T 扫描的点云块被表示为 P_{L_i ,t_j} ，它位于 L_i 的局部坐标系中。这个点云块可以通过变换到全局坐标系中来表示。

在我们提出的多传感器校准方法中，我们按顺序校准 \xi_L 和 \xi_C 。在第一步中，我们利用高效的多视图配准方法（见第5节）同时估计 LiDAR 的外参 \xi_L 和基准 LiDAR 的位姿轨迹 S 。在第二步中，我们通过匹配从图像中提取的深度连续边缘和上述重建的点云来校准 \xi_C（见第 6 节）。在 LiDAR 和相机外参校准的中心是一个自适应地图，它能够高效地找到 LiDAR 和相机测量之间的对应关系（第4节）

图2：由两个相对指向的传感器之间的旋转所创建的视场重叠。最初的两个传感器L_i和L_j/C_k的设置没有视场重叠。引入旋转运动后，同一区域在不同时间被所有传感器扫描到。

4. 自适应体素化

为了找到不同LiDAR扫描之间的对应关系，我们假设初始基准LiDAR轨迹S、LiDAR外参\xi_L和相机外参\xi_C是已知的。初始基准LiDAR轨迹S可以通过在线LiDAR SLAM（例如[3]）获得，初始外参可以通过CAD设计或粗略的手眼标定[14]获得。我们之前的工作[5]从每个LiDAR扫描中提取边缘和平面特征点，并通过k最近邻搜索（k-NN）将它们与地图中附近的边缘和平面点进行匹配。这将在每次迭代中重复构建全局地图的k-d树。在本文中，我们使用[4]中提出的更高效的体素地图来创建所有LiDAR扫描之间的对应关系。
体素地图是通过将点云（使用当前S和EL进行配准）切割成小的体素来构建的，以便体素中的所有点大致位于一个平面上（具有一定的可调容忍度）。固定分辨率体素地图的主要问题是，如果分辨率过高，分割过程将耗时过长，而如果分辨率过低，环境中的多个小平面将落入同一个体素中而无法分割。为了最好地适应环境，我们实现了一种自适应体素化过程。具体而言，首先将整个地图切割成预设大小的体素（通常较大，例如4m）。然后对于每个体素，如果所有LiDAR扫描中包含的点大致形成一个平面（通过检查特征值之间的比率），则将其视为平面体素；否则，将它们分成八个八分体，每个八分体将再次进行检查，直到包含的点大致形成一个平面或体素大小达到预设的最小下限。此外，自适应体素化直接在LiDAR原始点上进行，因此不需要像[5]中那样先进行特征点提取。
图3显示了在复杂的校园环境中自适应体素化过程的典型结果。可以看到，该过程能够分割不同大小的平面，包括地面上的大平面、建筑墙壁上的中等平面和树冠上的小平面。

图3：A）使用自适应体素化方法分割的激光雷达点云。同一体素内的点被着相同的颜色。虚线白色矩形内的点的详细自适应体素化结果可在B）着色点和C）原始点中查看。初始体素化的默认尺寸为4米，最小体素尺寸为0.25米。

5. 多LiDAR外参标定

通过自适应体素化，我们可以获得一组不同大小的体素。每个体素包含大致在一个平面上的点，并为所有在该体素中具有点的LiDAR位姿创建一个平面约束。具体而言，考虑LiDAR扫描的一组点P_l = { ^G p_{L_i ,t_j} } 组成的第l个体素，其中 L_i ∈ L，j ∈ T。我们定义一个点云一致性指标 c_l( ^G_{L_i} T _{t_j})，它在图S和\xi_L上形成一个因子，如图4(a)所示。然后，通过优化因子图来估计基准LiDAR轨迹和外参。一种自然的选择是将一致性指标 c_l (·) 定义为每个 ^G p_{L_i ,t_j} 到待估计平面的欧氏距离之和（见图4(b)）。

图4：(a) 与S和\xi_L相关的第l个因子项，其中L_i ∈ L且t_j ∈ T。 (b) 点^Gp_k到平面π的距离。

考虑到体素图中的所有这些指标，我们可以将问题形式化为

在 ^G p_k ∈ P_l 的情况下，N_l 是 P_l 中点的总数，n_l 是平面的法向量，q_l 是该平面上的一个点。
值得注意的是，优化变量 (n_l , q_l) 在 (2) 中可以通过解析方法求解（见附录 A），而得到的代价函数 (3) 仅涉及到 LiDAR 姿态 ^G_{L_i} T_{t_j}（因此包括基准 LiDAR 轨迹 S 和外参 \xi_L ），具体如下所示。

其中 λ_3 (A_l) 表示矩阵 A_l 的最小特征值，其定义为

为了在（3）中实现高效的优化，我们对优化变量x进行了二阶封闭形式导数的推导（从（3）到（5）的详细推导在附录B中详细阐述）。

其中\bar{J}是雅可比矩阵，\bar{H}是海森矩阵。δx是优化变量x的微小扰动

然后，通过使用LM方法迭代地解决（6）并将δx更新为x，可以确定最优的x。

6. LiDAR-相机外参标定

通过上述计算得到的LiDAR外参参数\xi_L和姿态轨迹S，我们将所有LiDAR点云转换到基准LiDAR坐标系中，得到一个密集的全局点云。然后，通过最小化背投LiDAR边缘特征点与图像边缘特征点之间的距离之和来优化外参\xi_C。从点云中可以提取两种类型的LiDAR边缘点。一种是前景和背景物体之间的深度不连续边缘，另一种是相邻非平行平面之间的深度连续边缘。正如我们之前的工作[28]中所解释的那样，深度不连续边缘存在前景膨胀和出血点现象；因此，我们使用深度连续边缘来匹配点云和图像。
在[28]中，LiDAR点云被分割成具有统一大小的体素，并通过RANSAC算法估计每个体素内的平面。相比之下，我们的方法使用在第4节中获得的自适应体素地图。我们计算每两个相邻体素之间包含平面法线的夹角。如果这个角度超过阈值，则提取这两个平面的交线作为深度连续边缘，如图5所示。我们选择实现Canny算法来检测和提取图像边缘特征。

图5：深度连续的LiDAR边缘特征提取比较。A）真实世界图像。B）该场景的原始点云。C）使用[28]中的方法提取的边缘，黄色圆圈表示错误的估计。D）使用自适应体素化提取的边缘。

假设^Gp_i表示上述提取的LiDAR边缘特征的第i个点在全局坐标系中的坐标。通过针孔相机及其畸变模型，^Gp_i被投影到由相机C_l在t_j时刻拍摄的图像I_{l,j}上。

其中，f(·)是相机畸变模型，π(·)是投影模型。让I_i表示捕捉到点^Gp_i的图像集合，即I_i = {I_{l,j}}。对于每个^{I_{l,j}}p_i，我们在I_{l,j}上搜索κ个最近的图像边缘特征点q_k。由这κ个点形成的边缘的法向量n_{i,l,j}因此是对应于A_{i,l,j}的最小特征值的特征向量。

这个激光雷达相机对应的残差被定义为

收集所有这样的对应关系，外部\xi_C校准问题可以被表述为

检查式（9）中的残差，我们发现 ^{I_l,j} p_i 依赖于 LiDAR 位姿 ^G_{L_0} T_{t_j} 。这是因为 LiDAR 在不同时间可能与相机存在视场重叠（如图2所示）。由于^G_{L_0} T_{t_j} ∈ S 已经在第5节中得到了很好的估计，我们在这一步中将它们固定。此外，n_{i,l,j} 和 q_{i,l,j} 也隐含地依赖于 \xi_C ，因为 n_{i,l,j} 和 q_{i,l,j} 都与最近邻搜索有关。对于变量 \xi_C 来说，(10) 的完整导数会过于复杂。在本文中，为了简化优化问题，我们忽略了相机外参对 n_{i,l,j} 和 q_{i,l,j} 的影响。这种策略在实践中表现良好，详见第IV-B节。
非线性优化问题（10）通过用一阶导数（11）近似残差来使用 LM 方法求解。通过迭代求解（11）并使用 \boxplus 操作[31]更新 δx 到 x，得到最优的 \xi_C^∗ 。

当l = p时，J_p和R_p分别为J_{i,l,j}和r_{i,l,j}的总和。