DLO：直接激光雷达里程计

---

前言

代码：github
原文：原文

摘要—在感知挑战环境中，场地机器人需要快速且准确的状态估计，但现代激光雷达（LiDAR）传感器迅速超出了现有里程计算法的处理能力。为此，本文提出了一种轻量级前端激光雷达里程计解决方案，在计算能力受限的机器人平台上实现了一致且准确的定位。我们的直接激光雷达里程计（DLO）方法包括几个关键的算法创新，优先考虑计算效率，并能够使用密集、最小预处理的点云来实时提供精确的位姿估计。这通过一种新颖的关键帧系统实现，该系统高效管理历史地图信息，此外还采用了一种定制的迭代最近点（ICP）求解器，用于快速点云配准并实现数据结构的回收。与当前最先进的方法相比，我们的方法具有更高的准确性和更低的计算开销，并已在多种感知挑战环境中进行广泛评估，评估对象包括NASA JPL CoSTAR团队在DARPA地下挑战中的研究和开发工作，测试平台包括空中和四足机器人。

图1. 快速轻量的激光雷达里程计。Team CoSTAR的两种计算资源有限的机器人平台。(A) 我们定制的四旋翼平台，上方配有Ouster OS1激光雷达传感器。(B) 一台波士顿动力的Spot机器人，配备定制载荷和带保护罩的Velodyne VLP-16激光雷达。© 使用我们的轻量里程计方法在这些机器人上进行测试和集成时，绘制的石灰岩矿井的俯视图，作为DARPA地下挑战的一部分。

一、介绍

在大规模、感知挑战环境中的准确状态估计和地图构建，已成为自主移动机器人关键的能力。然而，典型的视觉SLAM方法在尘土、雾霾或低光照条件下往往表现不佳，而基于激光雷达（LiDAR）的方法由于直接深度测量的优越范围和精度，可以提供更可靠的定位[1]。然而，近期关于激光雷达里程计（LO）的研究揭示了处理商业激光雷达传感器在实时高频率状态估计中产生的大量深度返回数据的挑战[2][3]。本文提出了几个算法创新，使得在密集激光雷达扫描的情况下实现实时定位成为可能，同时与现有方法相比，在精度和计算复杂度上表现出我们的算法的优越性。

目前的LO算法分为两个阶段来估计机器人自运动：首先，通过在相邻的激光雷达帧之间执行“扫描到扫描”的对齐，以恢复一个初步的运动估计；然后，通过将当前扫描与过去的环境信息进行“扫描到地图”的配准，以提高全局位姿一致性。不幸的是，现代激光雷达每次扫描产生的大量数据点迅速超出计算能力有限的处理器的承载能力，在对齐过程中形成性能瓶颈，可能导致帧丢失并最终导致较差的位姿估计。更具体地说，扫描到扫描的对齐需要在两云之间进行对应点的配准，但这一过程通常涉及到一个最近邻搜索，而随着每次扫描的点数增多，这一过程的计算量呈指数增长。基于特征的方法[2][4]–[6]尝试通过只使用最显著的点来缓解这一问题，但这些方法通常采用计算量大的特征提取步骤，并且可能意外地丢弃本可以改善后续配准质量的数据。此外，在扫描到地图对齐过程中，键控的环境历史（包括所有或部分过去的点）会随着新扫描的获取和存储在内存中而迅速增长。尽管通过与子地图对齐（而不是完整的扫描历史）可以提高计算效率，但点的持续增加仍然显著扩大了典型子地图提取方法的最近邻搜索空间。基于树的数据结构已被证明可以显著减少最近邻搜索的成本[7]，但即使仅仅经过几个关键帧，提取局部子地图仍然涉及过多的点，因此无法在长期导航中保持一致的性能。

在本文中，我们提出了直接激光雷达里程计（DLO）算法，这是一种高速、计算高效的前端定位解决方案，允许直接使用密集的点云扫描数据，而无需进行大量预处理。本文的主要贡献是一个定制的“速度优先”管道，通过最小预处理的激光雷达扫描数据和可选的IMU，在消费级处理器上实时精确解决机器人自运动问题。我们工作的一个关键见解是算法的速度和精度之间的联系，我们的方法包括三个核心创新。首先，采用一种自适应关键帧系统，通过一种新颖的空间度量方法高效地捕捉重要的环境信息。其次，采用一种快速的基于关键帧的子地图方法，通过凸优化生成可接受的局部子地图，用于全局位姿精化。第三，NanoGICP，一种定制的迭代最近点求解器，用于轻量级点云扫描匹配，并通过数据结构回收消除冗余计算。我们的方法已在多个挑战性环境中进行了广泛评估，评估对象包括计算能力有限的机器人平台，作为CoSTAR团队为DARPA地下挑战的研究和开发的一部分，同时我们也已将代码开源，供社区使用。

二、相关工作

基于激光雷达（LiDAR）的里程计通常被视为一个非线性优化问题，用于计算最佳拟合的齐次变换，以最小化两个点云之间对应点和/或平面之间的误差。由于对应关系在事先无法得知，因此诸如迭代最近点（ICP）算法[8]或其变体，如广义ICP（GICP）[9]，已成为对齐两个点云的标准方法；然而，遍历所有数据点可能会导致计算开销较大。基于特征的方法试图在扫描匹配之前提取并仅使用最显著的点以减少计算量。此类特征可以通过手动调节的方法[10]或学习网络[11]来获得，可能包括平面[5]、线条和边缘[4][6]，或地面点[2]。这些工作旨在将从视觉里程计（VO）技术中获得的见解转化为3D领域。然而，加入此步骤会增加计算开销，并且可能会丢失本可以帮助提高对应匹配精度的数据点。另一种方法是直接方法，它尝试对密集的点云进行对齐，但必须进行大量降采样以实现计算可行性[12][13]。最近，提出了一种递归滤波框架，例如卡尔曼滤波器[14][15]，以实现实时性能，但可能会牺牲估计的准确性。

在扫描对齐后紧随其后的第二阶段已被证明能通过增加位姿估计与先前扫描的一致性来减少全局漂移[3][13]。在扫描到地图的阶段，通过将当前点云与现有的内存中地图对齐，进一步精化扫描到扫描的变换；该子地图通常通过检索机器人当前位置附近一定半径内的地图点来获得。然而，这种在“点空间”中的搜索，由于检索最近邻数据点所需的大量操作，可能会迅速增加计算开销。虽然存在一些技术可以缓解这一问题，例如仅在键控位置逐步存储地图数据[6]，但此搜索仍然涉及数千次计算，可能会增加处理器负载，从而导致丢帧。

为了解决这些问题，我们的DLO算法围绕“速度优先”的理念构建，允许使用最小预处理的点云，并为计算资源有限的机器人提供准确的位姿估计（见图2）。我们工作的关键贡献在于我们如何高效地为全局精化推导子地图以进行扫描到地图的匹配。即，与大多数工作通过提取位于机器人当前位置附近的点来构建子地图不同，DLO通过将扫描点集与其对应的关键帧位置关联，改为在关键帧空间中进行搜索。然后，子地图通过将来自附近关键帧的子集以及构成凸包的关键帧的点云连接起来构建，从而为当前扫描提供了附近和远处的点进行锚定。此外，我们定制的GICP求解器使得跨多个求解器实例的数据显示结构得以广泛重用，从而消除在两阶段过程中冗余的计算。我们的系统还可选地接受来自IMU的初始化先验信息，并以松耦合方式进一步提高在剧烈旋转运动中的精度。通过在多个具有计算限制的机器人平台和多个挑战性环境中进行广泛测试，验证了我们方法的可靠性。本工作是CoSTAR团队为DARPA地下挑战进行的研究和开发的一部分，支持NASA喷气推进实验室的网络信念感知自主（NeBula）框架[16]，其中DLO是我们自主空中车辆队伍的主要状态估计组件（见图1(a)）。

图2. 激光雷达里程计架构。我们的系统首先通过扫描到扫描（S2S）匹配，利用RANSAC异常值剔除和可选的IMU旋转先验，获取时间为k和k−1的两个相邻扫描之间的相对变换。这个初步估计值随后被传播到世界坐标系，并作为初始化点用于我们的二级GICP模块进行扫描到地图优化（S2M），该模块将当前点云Pk与由附近和边界关键帧的扫描构成的子地图Sk进行扫描匹配。该过程的输出是一个全局一致的位姿估计，随后会根据几个度量标准检查是否应将当前位姿存储为新的关键帧。

三、方法

A. 符号表示

点云P由一组点p ∈ P组成，这些点具有笛卡尔坐标pi ∈ R^3。我们用L表示激光雷达的坐标系，B表示位于IMU框架的机器人坐标系，W表示世界坐标系，它在初始位置与B重合。请注意，在这项工作中我们假设L和B参考框架是重合的。子图、协方差和kd树结构分别表示为S、C和T。我们采用标准约定，即x轴向前，y轴向左，z轴向上，我们的工作试图解决以下问题：给定在时间k的相邻点云扫描Pk和Pk-1，估计机器人当前的姿态$X_k\in SE(3)$和在W中的地图$M_k$。

B. 预处理

我们的系统假设输入为通过360°激光雷达（如Ouster OS1（20 Hz）或Velodyne VLP-16（10 Hz））收集的3D点云数据。为了最小化原始传感器数据中的信息损失，预处理过程中仅使用两个滤波器：首先，我们通过一个大小为1 m³的盒式滤波器去除可能来自机器人本身的所有点返回。这一点尤其重要，特别是在空中机器人（图1(a)）的螺旋桨或保护罩（图1(b)）处于激光雷达视野范围内时。得到的点云随后通过一个分辨率为0.25 m的3D体素网格滤波器进行轻度降采样，以便后续任务使用，同时保持周围环境中的主要结构。请注意，在本工作中，我们并未进行运动畸变校正，因为非刚性变换可能会带来计算负担，并且我们直接使用密集的点云，而不是像大多数工作那样提取特征。预处理后，平均每个点云包含约10,000个点。

C. 通过广义ICP进行扫描匹配

基于激光雷达的里程计可以被视为通过比较连续的点云和内存中的点云来解决机器人自我运动的过程，以恢复SE(3)变换，这转化为连续激光雷达采集之间机器人的6自由度运动。这个过程通常分为两个阶段进行，首先提供一个最佳的即时猜测，然后进一步细化以与先前的关键帧位置更加全局一致。

1. 扫描对扫描：在第一阶段，扫描对扫描匹配的目标是计算源点云${\mathcal{P}}_k^s$和目标点云${\mathcal{P}}_k^t$（其中${\mathcal{P}}_k^t={\mathcal{P}}_{k-1}^s$）之间的相对变换${\hat{X}}_k^{\mathcal{L}}$，它们在$\mathcal{L}$中被捕获，其中

$${\hat{X}}_k^{\mathcal{L}}=\underset{X_k^{\mathcal{L}}}{\mathrm{argmin}}\mathcal{E}\left(X_k^{\mathcal{L}}{\mathcal{P}}_k^s,{\mathcal{P}}_k^t\right).\text{\hspace{1em}(1)}$$

GICP的残差误差$\mathcal{E}$定义为

$$\mathcal{E}\left(X_k^{\mathcal{L}}{\mathcal{P}}_k^s,{\mathcal{P}}_k^t\right)=\sum _i^N{d}_i^{\top }{\left({\mathcal{C}}_{k,i}^t+X_k^{\mathcal{L}}{\mathcal{C}}_{k,i}^s{X}_k^{{\mathcal{L}}^{\top }}\right)}^{-1}{d}_i,\text{\hspace{1em}(2)}$$

使得这个阶段的总体目标是

$${\hat{X}}_k^{\mathcal{L}}=\underset{X_k^{\mathcal{L}}}{\mathrm{argmin}}\sum _i^N{d}_i^{\top }{\left({\mathcal{C}}_{k,i}^t+X_k^{\mathcal{L}}{\mathcal{C}}_{k,i}^s{X}_k^{{\mathcal{L}}^{\top }}\right)}^{-1}{d}_i,\text{\hspace{1em}(3)}$$

对于点云${\mathcal{P}}_k^s$和${\mathcal{P}}_k^t$之间的N个对应点，其中$d_i=p_i^t-X_k^{\mathcal{L}}{p}_i^s$，$p_i^s\in {\mathcal{P}}_k^s$，$p_i^t\in {\mathcal{P}}_k^t$，对于所有的i，${\mathcal{C}}_{k,i}^s$和${\mathcal{C}}_{k,i}^t$是对应的估计协方差矩阵。与源点云或目标点云中的每个点i分别相关联。正如第II-D节将进一步讨论的，我们可以使用外部传感器提供的先验来初始化上述目标函数，以尝试将收敛推向全局最小值。也就是说，对于(3)，如果通过IMU预积分可以得到先验${\tilde{X}}_k^B$，我们可以设置初始猜测${\tilde{X}}_k^{\mathcal{L}}={\tilde{X}}_k^{\mathcal{B}}$来创建一个松耦合系统。然而，如果没有先验可用，系统则回归到纯激光雷达里程计，其中${\tilde{X}}_k^{\mathcal{L}}=I$，并且仅依赖于点云对应匹配来完成这一步。

2. 扫描对地图：在恢复初始机器人运动估计后，执行次级阶段的扫描对地图匹配，并遵循与扫描对扫描相似的程序。然而，这里的目标不是计算两个即时点云之间的相对变换，而是通过与局部子图匹配来进一步细化前一步骤中的运动估计，使其更加全局一致。换句话说，这里的任务是计算一个最优变换${\hat{X}}_k^{\mathcal{W}}$，介于当前源点云${\mathcal{P}}_k^s$和某个派生子图${\mathcal{S}}_k$之间，使得

$${\hat{X}}_k^{\mathcal{W}}=\underset{X_k^{\mathcal{W}}}{\mathrm{argmin}}\mathcal{E}\left(X_k^{\mathcal{W}}{\mathcal{P}}_k^s,{\mathcal{S}}_k\right).\text{\hspace{1em}(4)}$$

在类似地定义了GICP的残差误差$\mathcal{E}$之后，如(2)中所述，扫描对地图的整体目标函数为

$${\hat{X}}_k^{\mathcal{W}}=\underset{X_k^{\mathcal{W}}}{\mathrm{argmin}}\sum _j^M{d}_j^{\top }{\left({\mathcal{C}}_{k,j}^{\mathcal{S}}+X_k^{\mathcal{W}}{\mathcal{C}}_{k,j}^s{X}_k^{{\mathcal{W}}^{\top }}\right)}^{-1}{d}_j,\text{\hspace{1em}(5)}$$
对于点云${\mathcal{P}}_k^s$和子图${\mathcal{S}}_k$之间的M个对应点，其中${\mathcal{C}}_{k,j}^{\mathcal{S}}$是子图中点j的相应扫描拼接协方差矩阵，如第II-F节中稍后定义的。(5)使用前一节从$\mathcal{L}$到$\mathcal{W}$的扫描对扫描传播结果进行初始化，即${\tilde{X}}_k^{\mathcal{W}}={\hat{X}}_{k-1}^{\mathcal{W}}{\hat{X}}_k^{\mathcal{L}}$，以便将这种先前的运动与历史地图数据进行比较，以实现全局一致性。这一阶段的输出${\hat{X}}_k^{\mathcal{W}}$是用于下游模块的最终估计机器人姿态。我们在这里指出，这项工作的一个关键创新是如何派生和管理这一阶段的子图。与之前通过查询存储地图中每个单独点的局部性来创建子图的工作不同，我们将扫描与关键帧关联，并在关键帧空间中搜索以拼接点云并创建$S_k$。其含义包括更快、更一致的局部子图生成，与基于半径的搜索相比，这种方法更加宽容，将在第II-E节中进一步讨论。

图3. 基于关键帧的子地图构建。不同子地图方法的比较，显示当前扫描（白色）、派生的子地图（红色）和完整地图（蓝色）。(A) 一种常见的基于半径的子地图方法，半径r = 20 m，在点云空间中进行检索。(B) 我们的基于关键帧的子地图方法，它通过连接一部分已键控的扫描，在扫描到地图阶段帮助锚定当前扫描中的最远点（绿色框）。

D. 优化先验

方程(3)描述了扫描对扫描的非线性优化问题，可以通过先验进行初始化，以减少收敛到次优局部最小值的可能性。这种先验代表了两个激光雷达帧之间相对运动的初始猜测，可以来自惯性测量单元(IMU)的角速度测量值的积分。更具体地说，角速度测量值${\hat{\omega }}_k$定义为${\hat{\omega }}_k={\omega }_k+b_k^{\omega }+n_k^{\omega }$，在$\mathcal{B}$中测量，具有静态偏差$b_k^{\omega }$和零白噪声$n_k^{\omega }$以方便起见。在校准偏差后，机器人本体在两个连续LiDAR采集之间的相对旋转运动， 通过陀螺仪传播的四元数运动学，可以计算两个连续激光雷达帧之间的相对旋转运动：$q_{k+1}=q_k+(\frac{1}{2}{q}_k\otimes {\omega }_k)\Delta t$。这里，$q_k$在积分前初始化为单位四元数，$\Delta t$是IMU测量之间的时间差，单位为秒，并且只使用当前激光雷达扫描和前一次扫描之间发现的陀螺仪测量值。请注意，我们只关心IMU预积分期间的旋转先验，并通过加速度计检索平移先验的工作留待将来。这种传播得到的四元数被转换为具有零平移分量的SE(3)矩阵，用作${\tilde{X}}_k^{\mathcal{B}}$，即扫描对扫描的先验。

E. 快速基于关键帧的子图映射

这项工作的一个关键创新在于我们的系统如何管理地图信息以及如何在扫描对子图匹配中派生局部子图以进行全局自我运动的精细化。我们不是直接使用点云并将点存储到典型的八叉树数据结构中，而是保持关键帧的历史记录以在其中搜索，其中每个关键帧都与其对应的点云扫描以键值对的形式链接。然后，用于扫描对子图匹配的局部子图是通过连接关键帧子集中的相应点云生成的，而不是直接检索机器人当前位置周围某个半径内的局部点。

这种设计选择的含义是双重的：首先，通过在“关键帧空间”而不是“点云空间”中搜索，获得了一个计算上更可行的问题。在累积点云地图中进行基于半径的搜索可能需要对数十万个点进行距离计算——即使使用增量八叉树数据结构，这个过程也很快变得不可行。然而，即使在长时间遍历后，对关键帧的搜索通常只涉及几百个点，并提供更一致的计算性能，减少了丢帧的可能性。

图4. 关键帧选择和自适应阈值。(A) 我们方法的子地图（红色）是通过连接一部分关键帧（绿色球体）中的扫描生成的，这些关键帧包括K个最近邻关键帧和构成关键帧集合凸包的关键帧。(B) 自适应关键帧的示意图。在这种场景中，当机器人沿狭窄坡道行进时，阈值减小，以更好地捕捉小尺度细节。

此外，基于关键帧的方法与基于范围的方法相比，构建了一个更为宽容的子图。也就是说，由于从关键帧点云派生的子图的大小仅依赖于激光雷达传感器的范围而不是预定的距离，派生的子图可以与当前扫描有更多的重叠；这在图3中有所说明。在这个例子中，固定半径$r=20m$的子图与当前扫描的重叠不足，并且可能因为只包含空间附近的点而随时间引入漂移；然而，基于关键帧的方法覆盖了当前扫描的大部分，这有助于更好的扫描对地图的对齐。扩大半径大小可能有助于增加基于半径方法的这种重叠，但这样做会显著减慢后续任务，如GICP协方差计算。

1. 通过k最近邻和凸包选择关键帧：为了构建子图$S_k$，我们从选定的环境关键帧的子集中连接相应的点云。设${\mathcal{K}}_k$为所有关键帧点云的集合，使得${\mathcal{S}}_k\subseteq {\mathcal{K}}_k$。我们定义子图$S_k$为K个最近邻关键帧扫描${\mathcal{Q}}_k$和L个最近邻凸包扫描${\mathcal{H}}_k$的连接，使得${\mathcal{S}}_k={\mathcal{Q}}_k\oplus {\mathcal{H}}_k$，其中指定凸包的索引由构成包含组成${\mathcal{K}}_k$的关键帧的所有凸集的交集的关键帧集合定义。

这个结果在图4(a)中有所说明，其中用绿色突出显示的关键帧组成了提取的子图，用红色表示。直观地说，提取最近邻关键帧旨在帮助当前扫描中附近点的重叠，而来自凸包的（包含边界地图点）则增加了与扫描中更远点的重叠。这种组合通过最大化扫描对地图的重叠来减少整体轨迹漂移，并为系统提供了多种环境特征的尺度以供对齐。请注意，被分类为既是最近邻又是凸包索引的关键帧在子图中只使用一次。

2) 自适应关键帧选择：关键帧的位置影响派生的子图，进而影响里程计的准确性和鲁棒性。通常使用固定阈值（例如，每1米或10度的平移或旋转变化）[4]、[6]、[13]来丢弃关键帧节点，但最优位置可能高度依赖于周围环境的结构。更具体地说，在大规模环境中，点云扫描捕获的特征更加突出，可以依赖更长时间。相反，在狭窄或小规模环境中，需要更小的阈值以持续捕获子图中的小规模特征（例如，紧角落）以实现更好的定位。因此，我们选择根据瞬时点云扫描中的“宽敞度”来缩放新关键帧的平移阈值，定义为$m_k=\alpha m_{k-1}+\beta M_k$，其中$M_k$是预处理点云中每个点到原点的中值欧几里得距离，$\alpha =0.95$，$\beta =0.05$，$m_k$是用于缩放时间k处关键帧阈值$t{h}_k$的平滑信号，使得

$$\begin{array}{rl}& t{h}_k=\{\begin{array}{ll}10m& \text{if }{m}_k>20m\\ 5m& \text{if }{m}_k>10m\&m_k\le 20m\\ 1m& \text{if }{m}_k>5m\&m_k\le 10m\\ 0.5m& \text{if }{m}_k\le 5m\end{array}\end{array}\text{\hspace{1em}(6)}$$

旋转阈值固定在$30^{\circ }$。图4(b)展示了这种自适应阈值的效果，这有助于对环境尺寸变化的鲁棒性。

图5. Alpha地图。使用我们的DLO算法在Urban Alpha数据集上生成的稠密3D点云地图的不同视角和角度。每个时间戳的估计位置被用来将提供的扫描转换到世界坐标系中；这一过程对数据集中的所有扫描进行了处理，并通过拼接/体素滤波生成了上述图像。

图6. 错误比较。绘制了一个1200秒运动窗口内的绝对位姿误差，展示了半径和关键帧子地图方案之间的差异。基于关键帧的方法没有半径子地图方案固有的范围限制，这直接导致了里程计误差的降低，因为它能够提供更为感知的子地图。请注意，自适应关键帧主要有助于提高可靠性和对环境尺寸变化的鲁棒性（见图9）

F. 算法实现

1. 扫描拼接子图法线：广义ICP涉及最小化两个云之间的平面到平面距离，其中这些平面由扫描中每个点计算出的协方差来建模。我们不是在每次迭代中计算子图中每个点的法线（这对于实时操作来说可能是不可行的），而是假设子图协方差集${\mathcal{C}}_k^{\mathcal{S}}$可以通过连接填充子图的N个关键帧的法线${\mathcal{C}}_{k,n}^{\mathcal{S}}$来近似，使得${\mathcal{C}}_k^{\mathcal{S}}\approx {\sum }_n^N{\mathcal{C}}_{k,n}^{\mathcal{S}}$。因此，每个子图的法线集合不需要显式计算，而只需通过拼接先前计算过的法线来重建。

2. 数据结构回收利用：在上述基础上，当前激光雷达里程计流程中的几个算法步骤可以受益于数据结构共享和重用，通过消除不必要的和冗余的操作，大幅降低整体系统开销。如表I所总结，系统需要总共八个元素来成功执行扫描对扫描和扫描对地图匹配。这包括用于搜索点对应关系的kd树${\mathcal{T}}_k$和协方差矩阵${\mathcal{C}}_k$。在每次扫描匹配过程中，对于源点云和目标点云进行GICP对齐。
   在四个所需的kd树数据结构中，只需要显式构建两个。也就是说，源点云（输入）${\mathcal{T}}_k^{\text{source}}$的树只需要在每次扫描获取时构建一次，并且在两个模块之间共享（因为相同的扫描被用作两个源点云）。对于扫描对扫描的目标树${\mathcal{T}}_k^{\text{target}}$，这仅仅是前一次迭代的扫描对扫描源树${\mathcal{T}}_{k-1}^{\text{source}}$，因此可以传播。扫描对地图的目标树需要显式构建，但由于子图是从一组关键帧派生出来的，这个构建只需要在我们通过k最近邻和凸包策略选择的关键帧集合从一次迭代变化到下一次时执行，即当${\mathcal{S}}_k\ne {\mathcal{S}}_{k-1}$时。否则，数据结构可以再次被重用，以节省额外的计算资源。另一方面，GICP所需的点协方差$C_k$只需要在每次扫描获取时计算一次，其数据可以直接在其他三个实例中共享。

3. 双NanoGICP：为了促进扫描匹配模块之间的交流，我们开发了NanoGICP，这是一个定制的迭代最近点求解器，它结合了FastGICP[17]和NanoFLANN[18]开源包，并根据前面描述的数据结构共享进行了额外的修改。特别是，NanoGICP使用NanoFLANN高效构建

特别地，NanoGICP使用NanoFLANN高效地构建kd树数据结构，这些数据结构随后被FastGICP用于点云对应匹配。在实际应用中，数据结构共享是在两个具有不同超参数的独立NanoGICP实例之间进行的——每个实例针对一个扫描匹配问题——并按照算法1中详细描述的程序进行。

图7. 平均收敛时间。对每种算法（包括我们的NanoGICP求解器和另外两个开源GICP包）在100个基准对齐测试中的平均收敛时间进行比较。

图8. 数据回收方案的消融研究。四种不同数据回收方案的处理时间和CPU使用率的箱线图，涵盖了从不重用数据结构到部分重用和完全重用的不同情况。

三、结果

图9. 极端环境。顶部：使用我们的定制无人机在肯塔基州列克星敦的地下矿井一部分进行自主映射，同时运行DLO。该环境包含了一些具有挑战性的条件，如：(A) 低光照、(B) 物体障碍物和© 湿滑泥泞的地面。底部：通过DLO使用Velodyne VLP-16在四足机器人上对位于加利福尼亚州洛杉矶市中心的一个废弃地铁的三个楼层进行映射。此过程中的飞行任务是我们手动遥控四足机器人上下楼层，并在每层楼内移动，总共覆盖了856米的距离。

图10. MegaCavern。使用我们的DLO算法映射的肯塔基州路易斯维尔MegaCavern的不同视角，总估计轨迹为9057.66米。数据由Team Explorer提供。