0. 简介

在20年DARPA地下挑战赛中CoSTAR队伍提出了LOCUS这个深度学习模块，在两年后LOCUS2.0出世，LOCUS 2.0包括一种新的基于法线的广义迭代最近点（GICP）公式，该公式减少了点云对齐的计算时间，一种自适应体素网格滤波器，无论环境的几何结构如何，都能保持所需的计算负荷，以及一种滑动窗口建图方法，该方法限制了内存消耗。发布了LOCUS 2.0作为一个开放源代码库，具体在Github中可以找到。与主流的算法相比，表现优异。

1. 主要贡献

激光雷达里程计算法旨在利用扫描匹配来恢复机器人在连续激光雷达获取之间的运动。通过对固定环境特征的重复观测，机器人可以同时估计其运动、构建未知环境的地图并利用该地图跟踪其在其中的位置。虽然许多激光雷达里程计算法可以实现令人瞩目的准确性，但它们的计算成本仍可能对计算受限平台不利，从而减少了它们在异构机器人系统中的适用范围，其中一些机器人的计算资源可能非常有限。此外，许多现有方法为了定位目的而在内存中维护全局地图，使它们不适用于内存中地图大小显著增加的大规模探索。

本文的先前工作[1]介绍了LOCUS 1.0，这是一种多传感器激光雷达中心的高精度里程计和实时三维地图方案，具备一个多阶段的扫描匹配模块，配备了健康感知传感器集成，以松散耦合方案融合其他感知模态。虽然在感知退化环境中实现了令人瞩目的准确性和鲁棒性，但LOCUS 1.0的先前版本：

  1. 具有更大的计算负载

  2. 在内存中维护全局地图

  3. 对更广泛的传感器故障不够鲁棒，例如一个激光雷达传感器出现故障。

LOCUS 2.0提出了算法和系统级改进，以减少计算负载和内存需求，使系统能够在挑战性的感知条件下，在严格的计算和内存约束下，实现大规模探索的准确和实时的自我运动估计。本文的新特点和贡献包括：

  1. 基于法向量的GICP：一种新的广义迭代最近点（GICP）公式，利用点云法向量来近似点协方差计算，从而提高计算效率
  2. 自适应体素格滤波器，确保确定性和近似恒定的运行时间，独立于周围环境和激光雷达
  3. 两种滑动窗口地图存储数据结构的改进和评估：多线程八叉树、ikd树[2]

2. 系统概述

LOCUS 2.0提供了一种准确的基于广义迭代最近点（GICP）算法[17]的多阶段扫描匹配单元和一个健康感知的传感器集成模块，以松耦合的方式鲁棒地融合其他感应方式。如图2所示，该架构包含三个主要组件：i）点云预处理器，ii）扫描匹配单元，iii）传感器集成模块。点云预处理器负责管理多个输入激光雷达流以产生统一的3D数据产品，该产品可以被扫描匹配单元有效地处理。预处理模块包括点云的运动畸变校正（MDC），该模块使用IMU测量校正了由机器人运动引起的扫描期间传感器旋转的点云畸变。

接下来，点云合并器通过将来自机器人框架的不同激光雷达传感器的点云组合起来，扩大了机器人的视野。为了使多个激光雷达数据可靠地合并，我们引入了一个基于超时的健康监视器，动态更新哪些激光雷达应在点云合并器中进行组合（即如果其消息太延迟，则忽略激光雷达）。接下来，滤波器将会删除属于机器人的三维点。并通过自适应体素网格滤波器维护一定数量的体素化点，以管理CPU负载并确保确定性行为。与LOCUS 1.0相比，自适应体素网格滤波器将点云降采样策略从固定栅格叶大小和随机滤波这种盲目的体素化策略改为自适应系统。

扫描匹配单元执行GICP来完成scan-to-scan和scan-to-submap的策略，以估计机器人的6-DOF运动。LOCUS 2.0与其前身相比，不会重新计算协方差，而是利用一种新颖的GICP公式来使用法向量，这些法向量只需要计算一次并存储在地图中。在具有多模态感知的机器人中，如果可用，LOCUS 2.0 使用来自传感器集成模块的非激光雷达源的初始估计，来简化scan-to-scan匹配阶段中GICP的收敛，通过使用近似最优的种子来初始化优化，提高精度并减少计算，增强实时性能。

LOCUS 2.0 还包括一种更有效的地图存储技术。该系统采用滑动窗口方法，因为大规模区域无法在内存中维护。例如，在这里展示的其中一个洞穴数据集中，1厘米分辨率的全局地图需要50 GB的内存，远远超过小型移动机器人上通常可用的内存。这种方法需要高效的计算解决方案来进行插入、删除和搜索。

3. 基于法线的GICP（将点的协方差用法线表达，比较重要的创新点，提速手段）

LOCUS 2.0使用GICP进行scan-to-scan和scan-to-submap的匹配GICP通过使用概率模型来解决配准问题[17]，从而推广了点对点和点对平面ICP配准。为此，GICP需要每个点的协方差可用于对齐点云。协方差通常是基于给定点周围相邻点的分布计算的。Segal等人[17]提出了平面到平面的应用，假设现实世界的表面至少在局部上是平面的。在这种表述中，曲表面上的点由协方差矩阵局部表示，其中已知该点属于具有高置信度的平面，但其在该平面中的确切位置则具有较高的不确定性。

在这里，我们展示了面对面协方差计算如何等同于从预先计算的法线计算协方差，仅需要法线这一事实scan-to-submap对准尤其重要，因为否则地图需要重新计算点协方差，这是一个昂贵的操作，涉及创建kd-tree和最近邻搜索。相反，通过使用法线，协方差计算仅执行一次（因为它不受额外点的影响），并且结果可以被存储和重复使用。

在GICP的最常见实现中，需要计算两个扫描中每个点$i$的协方差$C^B_i$和$C^A_i$ [18]，[19]。每当需要注册两个扫描时，GICP的协方差计算将作为预处理步骤进行计算。下面我们将介绍如何获得协方差，而不必每次收集扫描时重新计算它们。任何定义良好的协方差矩阵$C$都可以进行特征值分解和特征向量[20]，$C = λ_1 u_1 · u_1^T + λ_2 u_2 · u_2^T + λ_3 u_3 · u_3^T$，其中$λ_1$，$λ_2$，$λ_3$是矩阵C的特征值，$u_1$，$u_2$，$u_3$是矩阵$C$的特征向量。具有相同值的两个特征值可以被解释和可视化为等分布的特征向量，其平坦表面在每个方向上具有相同的分布。这使得可以将协方差计算问题视为几何问题。

在平面对平面度量中，对于来自扫描$A$的点$a_i$，我们非常确切地知道其在法向量方向上的位置，但是我们对其在平面上的位置不太确定。为了表示这一点，我们设置$u_1 = n$，并将$λ_1 = ε$分配为法向量方向的方差，其中$ε$是一个小常数。然后，我们可以选择另外的特征向量为正交于$n$的平面上的任意向量，并分配相对较大的特征值$λ2 = λ3 = 1$，表示我们对平面上的位置不确定。

让我们取一个位于平面上且垂直于法线的任意向量。该向量需要满足平面方程（它垂直于法向量）并穿过原点：$n_xx + n_yy + n_zz = 0$。那么$z = − \frac{n_x·x+n_y·y}{n_z}$ ，因此平面上的向量族为$u_2 = \frac{(x,y,−(n_x·x+n_y·y)/n_z)}{||(x,y,−(n_x·x+n_y·y)/n_z|}$ ，其中$n_z$和$n_x$分别对应法向量$n$的$z$和$x$分量，$n_z = 0$表示水平向量。第三个向量$u_3$需要同时垂直于$u_1$和$u_2$，因为特征向量需要覆盖整个3D空间。因此，$u3 = n×u_2$。如果我们知道矩阵$C$的特征向量和特征值，我们有$C = \epsilon u_1·u_1^T +1.0 u_2·u_2^T +1.0 u_3·u_3^T$ 。从上面代入，我们得到：


那么，如果我们任意取$x = 1$和$y = 0$，则$z = − n_x n_z$，协方差简化为：

结果意味着协方差可以通过预先计算的每个点的法线来纯粹表达。

4. 自适应体素网格滤波器

为了管理激光雷达里程计的计算负载，无论环境和激光雷达配置如何都不至于让激光里程计给系统的负载过高（就激光雷达数量和类型而言），我们提出了一种自适应体素网格滤波器。在这种方法中，目标是将体素化点数保持在固定水平，而不是指定体素栅格叶大小并使系统暴露于输入点变化，这些变化来自不同传感器配置和环境横截面几何形状。这个设计目标来自这样一个事实，即配准阶段中的几乎所有计算都依赖于点云的数量$N$。因此，想法是将3D点的体素化数量固定，以便每次扫描的计算时间大致固定。方法如下：取任意初始体素大小$d_{init}$并设置$d_{leaf} = d_{init}$，其中$d_{leaf}$是当前时间戳中体素叶大小。我们提出以下控制方案：$d_{leaf_{t+1}} = d_{leaf_t} \frac{N_{scan}}{N_{desired}}$ 。公式描述了当前体素大小$d_{leaf_{t+1}}$应该根据当前输入扫描中点数$N_{scan}$与给定机器人所需计算点数$N_{desired}$之比来改变多少，与当前大小$d_{leaf}$相比。这种简单技术保持了固定水平上的点数，同时避免了任何大幅跳跃、太少点（例如故障扫描）或太多点。结果是提高了效率并减少了系统的计算负载。

5. 滑动窗口地图

LOCUS 1.0使用八叉树数据结构将全局地图存储在内存中。原生的八叉树实现没有有效的方式来剪枝数据。虽然一种可能的解决方法是过滤机器人周围的点并相应地重建八叉树，但这可能计算开销大并导致长时间的刷新时间。为了解决这些挑战，并在内存限制下实现大规模探测，LOCUS 2.0提供了两种地图滑动窗口方法：i）多线程八叉树，ii）增量k-d tree（ikd-tree）。其中，多线程八叉树方法只维护环境的机器人中心子图。两个并行线程（线程a和线程b）分别在专用的数据结构（$map_a/octree_a$和$map_b/octree_b$）上工作，负责通过框滤器动态过滤当前机器人位置周围的点云地图，并相应地使用更新后的地图重建八叉树，同时考虑到并行工作进程之间的机器人运动。增量k-dtree是一种二叉搜索树，通过合并新的扫描来动态存储三维点。增量k-dtree不仅在叶节点中维护三维点：它们也在内部节点中保留点。这种结构允许动态插入和删除，并依赖于整个数据结构的懒惰标签存储。增量k-dtree的初始构建类似于kd-tree，空间在最长的维度上递归地分裂中位点。移出ikd-tree数据结构边界的点不会立即删除，而是被标记为已删除并保留信息，直到触发重新平衡程序。

图3：我们滑动地图方法的插图。所有点都保留，直到机器人到达原始窗口的边界（步骤3）。然后，设置一个新窗口，并删除该窗口外的点。

6. 参考链接

https://blog.csdn.net/CV_Autobot/article/details/128196065