0. 简介
重建准确一致的大规模LiDAR点云地图对于机器人应用至关重要。
现有的解决方案，即位姿图优化，虽然时间效率高，但并未直接优化地图的一致性。最近提出了LiDAR捆绑调整（BA）来解决这个问题，然而在大规模地图上它耗时过长。为了缓解这个问题，本文《Large-Scale LiDAR Consistent Mapping using Hierarchical LiDAR Bundle Adjustment》提出了一种适用于大规模地图的全局一致且高效的映射方法。我们的方法包括自下而上的分层BA和自上而下的位姿图优化，结合了两种方法的优点。通过分层设计，我们使用比原始BA小得多的Hessian矩阵尺寸解决了多个BA问题；通过位姿图优化，我们平滑高效地更新LiDAR的位姿。目前相应的代码也已经在Github开源了。
1. 主要贡献
通过分层束调整设计，我们可以直接优化点云中平面表面的一致性，并避免解决具有大维度的代价函数。通过姿态图优化，我们可以以快速可靠的方式正确更新整个激光雷达姿态，使其收敛。为了保持每两个相邻关键帧之间的平滑性，我们通过将步幅大小设置小于窗口大小来保持它们之间的重叠区域。为了进一步提高优化速度，我们应用了一个滤波器来去除异常点，并在构建金字塔时实现了基于CPU的并行处理。总之，我们的贡献如下：
- 我们提出了一种分层束调整方法，以全局优化激光雷达地图的一致性和里程计精度。我们的方法在给定良好的初始姿态轨迹（例如来自姿态图优化）时改善了地图质量，甚至在初始姿态轨迹存在较大漂移时也能填补差距。
- 我们的工作的有效性已在多个公共机械旋转激光雷达数据集和我们自己收集的固态激光雷达数据集中在结构化和非结构化场景中得到验证。
2. 概述
我们提出的方法的系统工作流程如图2所示。输入是每个LiDAR扫描的原始或校正后的点云以及它们在全局坐标系中的初始估计姿态，这些姿态可以从通用的LiDAR里程计或同时定位和建图（SLAM）算法中获得。该方法包括两个过程，自下而上（见第3节）和自上而下（见第4节），直到收敛为止。在自下而上的过程中，对较小的局部窗口内的LiDAR帧进行局部BA，从第一层到第二层构建关键帧（见图1）。这个过程按层次进行，直到满足最佳层数，并对顶层关键帧进行全局BA。然后，使用来自每个优化层和相邻层之间的因素构建姿态图（见图1）。如图1所示，术语“第一层”，在上下文中也称为底层，描述了初始LiDAR帧和姿态的集合。类似地，“第二层”表示使用局部BA从第一层创建的LiDAR关键帧和姿态的集合。术语“顶层”表示剩余的最后一层LiDAR关键帧的集合（在图1中，顶层指的是第三层）。从底层到顶层逐层创建LiDAR关键帧的过程称为自下而上的过程。通过姿态图优化更新底层LiDAR姿态的过程称为自上而下的过程。
图1：所提出的分层束调整的金字塔结构，其中层数𝑙=3l=3，步长𝑠=3s=3，窗口大小𝑤=6w=6。第一层、第二层和第三层的因子将同一层中的相邻节点连接起来。红色虚线连接应该是相同的节点，例如𝑥9𝑖1x9i1，即从较低层的局部窗口到从该局部窗口构建的节点的较高层，例如𝑥3𝑖2x3i2和𝑥𝑖3xi3
图2：系统概述。浅黄色区域表示自下而上的过程，浅蓝色区域表示自上而下的过程
3. 自下而上的分层BA(重点内容)
我们用𝐹𝑗𝑖Fji表示第𝑖i层的第𝑗j个LiDAR帧，用𝑥𝑗𝑖≜𝑇𝑗𝑖=(𝑅,𝑡)∈𝑆𝐸(3)xji≜Tji=(R,t)∈SE(3)表示其对应的姿态。我们用𝑇𝑗,𝑘𝑖Tj,ki表示𝑇𝑗𝑖Tji和𝑇𝑘𝑖Tki之间的相对姿态，即𝑇𝑗,𝑘𝑖=(𝑇𝑗𝑖)−1⋅𝑇𝑘𝑖Tj,ki=(Tji)−1⋅Tki。需要注意的是，𝐹𝑗𝑖Fji中的点是以LiDAR局部坐标系表示的，而𝑇𝑗𝑖Tji是以全局坐标系表示的。我们用𝑤w表示局部窗口大小，𝑠s表示在LiDAR关键帧的自下而上构建过程中的步长大小，如图1所示。假设我们有𝑁𝑖Ni个来自第𝑖i层的LiDAR帧。在自下而上的过程中，使用提供的初始姿态轨迹在每个局部窗口中进行局部BA，并优化每个帧与该窗口中的第一帧之间的相对姿态。每个局部窗口中的BA得到的Hessian矩阵𝐻H也被记录下来，并用作后续自上而下的姿态图构建中的信息矩阵。给定第i层中一个包含𝑤w个LiDAR帧的局部窗口𝐹𝑠𝑗+𝑘𝑖∣𝑗=0,⋅⋅⋅,⌊𝑁𝑖−𝑤𝑠⌋;𝑘=0,⋅⋅⋅,𝑤−1Fsj+ki∣j=0,⋅⋅⋅,⌊sNi−w⌋;k=0,⋅⋅⋅,w−1及其优化后的相对姿态𝑇𝑗,𝑘𝑖∗Tj,ki∗，我们将这些帧聚合成 (𝑖+1)(i+1) 层的关键帧。该关键帧中的点都被转换到局部窗口的第一帧中，并且关键帧的姿态，表示为𝑇𝑗𝑖+1Tji+1，被设置为在前面的局部窗口中优化的
这个过程可以从下层重复执行到上层，直到达到最优的层数 𝑙l。值得注意的是，新关键帧（局部BA）的构建不依赖于局部窗口外的帧，因此适合在同一层中使用多个局部窗口进行并行处理。假设我们有总共𝑁N个LiDAR帧，即𝑁1=𝑁N1=N，并且每次我们选择将下层的𝑤w帧聚合成一帧传递到上层，步长为𝑠s。假设𝑛n是可以用于并行处理的线程数。由于BA的计算时间是𝑂(𝑀3)O(M3)，其中M是涉及的姿态数量，我们可以推导出第𝑙l层金字塔的总时间消耗𝑂(𝑇𝑙)O(Tl)。
第𝑙l层金字塔的总时间消耗包括每层局部BA消耗的时间和顶层全局BA消耗的时间。对于第𝑙l层金字塔，第𝑖i层（𝑖<𝑙i<l）中的局部窗口数量为𝑁𝑠𝑖siN，每个局部窗口消耗𝑂(𝑤3)O(w3)的时间。使用𝑛n个并行线程，局部BA的总时间消耗等于每层局部BA的总和，即𝑤(3Σ𝑖=1𝑙−1𝑁𝑠𝑖1𝑛)w(3Σi=1l−1siNn1)，而第𝑙l层的全局BA消耗𝑂((𝑁𝑠𝑙−1)3)O((sl−1N)3)的时间。总之，𝑂(𝑇𝑙)O(Tl)可以表示为
我们将𝑇𝑙Tl视为𝑙l的函数，并通过令𝑇𝑙Tl的导数等于零来计算最优的𝑙∗l∗。这将导致
图3展示了在不同帧数𝑁N下，计算时间𝑇𝑙Tl与层号𝑙l的关系的一个例子。可以看出，随着层号从𝑙=1l=1（原始BA）增加到𝑙∗l∗，计算时间大大减少，表明了所提出的分层BA的有效性。当𝑙>𝑙∗l>l∗时，计算时间不会显著增加，保持几乎恒定，表明任何大于𝑙∗l∗的层号都能同样有效地工作。
图3：以层号𝑙l和姿态数𝑁N为自变量，以时间消耗𝑇𝑙Tl为因变量的示例图。其中，𝑤=10w=10，𝑠=5s=5，𝑛=8n=8。从图中可以看出，相比于原始的BA（𝑙=1l=1），我们提出的分层BA（𝑙=3l=3或𝑙=4l=4）能够显著降低总体时间消耗。
点击经典文献阅读之--HBA(大规模LiDAR一致性建图BA)——古月居 kechakanquanw