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：所提出的分层束调整的金字塔结构，其中层数l=3，步长s=3，窗口大小w=6。第一层、第二层和第三层的因子将同一层中的相邻节点连接起来。红色虚线连接应该是相同的节点，例如x^1_{9i}，即从较低层的局部窗口到从该局部窗口构建的节点的较高层，例如x^2_{3i}和x^3_i
图2：系统概述。浅黄色区域表示自下而上的过程，浅蓝色区域表示自上而下的过程
3. 自下而上的分层BA(重点内容)
我们用\mathbb{F}^i_j表示第i层的第j个LiDAR帧，用x^i_j \triangleq T^i_j = (R, t) ∈SE(3)表示其对应的姿态。我们用T^i_{j,k}表示T^i_j和T^i_k之间的相对姿态，即T^i_{j,k} =(T^i_j)^{−1}· T^i_k。需要注意的是，\mathbb{F}^i_j中的点是以LiDAR局部坐标系表示的，而T^i_j是以全局坐标系表示的。我们用w表示局部窗口大小，s表示在LiDAR关键帧的自下而上构建过程中的步长大小，如图1所示。假设我们有N_i个来自第i层的LiDAR帧。在自下而上的过程中，使用提供的初始姿态轨迹在每个局部窗口中进行局部BA，并优化每个帧与该窗口中的第一帧之间的相对姿态。每个局部窗口中的BA得到的Hessian矩阵H也被记录下来，并用作后续自上而下的姿态图构建中的信息矩阵。给定第i层中一个包含w个LiDAR帧的局部窗口{\mathbb{F}^i_{sj+k}| j = 0, · · · , \lfloor \frac{N_i−w}{s} \rfloor; k= 0, · · · , w − 1}及其优化后的相对姿态T^{i^∗}_{j,k}，我们将这些帧聚合成 (i+1) 层的关键帧。该关键帧中的点都被转换到局部窗口的第一帧中，并且关键帧的姿态，表示为T^{i+1}_j，被设置为在前面的局部窗口中优化的
这个过程可以从下层重复执行到上层，直到达到最优的层数 l。值得注意的是，新关键帧（局部BA）的构建不依赖于局部窗口外的帧，因此适合在同一层中使用多个局部窗口进行并行处理。假设我们有总共N个LiDAR帧，即N_1 = N，并且每次我们选择将下层的w帧聚合成一帧传递到上层，步长为s。假设n是可以用于并行处理的线程数。由于BA的计算时间是O(M^3)，其中M是涉及的姿态数量，我们可以推导出第l层金字塔的总时间消耗O(T_l)。
第l层金字塔的总时间消耗包括每层局部BA消耗的时间和顶层全局BA消耗的时间。对于第l层金字塔，第i层（i < l）中的局部窗口数量为\frac{N}{s^i}，每个局部窗口消耗O(w^3)的时间。使用n个并行线程，局部BA的总时间消耗等于每层局部BA的总和，即w^3 _ (Σ^{l-1}_{i=1} \frac{N}{s^i} _ \frac{1}{n})，而第l层的全局BA消耗O((\frac{N}{s^{l-1}})^3)的时间。总之，O(T_l)可以表示为
我们将T_l视为l的函数，并通过令T_l的导数等于零来计算最优的l^∗。这将导致
图3展示了在不同帧数N下，计算时间T_l与层号l的关系的一个例子。可以看出，随着层号从l=1（原始BA）增加到l^∗，计算时间大大减少，表明了所提出的分层BA的有效性。当l > l^∗时，计算时间不会显著增加，保持几乎恒定，表明任何大于l^∗的层号都能同样有效地工作。
图3：以层号l和姿态数N为自变量，以时间消耗T_l为因变量的示例图。其中，w=10，s=5，n=8。从图中可以看出，相比于原始的BA（l=1），我们提出的分层BA（l=3或l=4）能够显著降低总体时间消耗。
在自底向上的分层BA中，为了进行特征提取和关联，我们使用了一种自适应体素化方法，该方法在[5]中提出，可以提取适用于不同结构环境的不同尺寸的平面特征。为了提取这些不同尺寸的平面特征，整个点云在转换为相同的全局坐标系后，被分割成大小为V的多个体素，每个体素通过检查包含点的最小和最大特征值比是否小于一个阈值（即\frac{λ_1}{λ_3} < θ）来进行平面测试。如果平面测试通过，则体素中的点被视为位于同一个平面上，并用于BA。否则，体素将被递归地分割，直到包含的点形成一个平面。
上述自适应体素化过程在点的数量非常大时耗时。为了缓解这个问题，我们注意到在较低层次中不被视为平面特征的点在较高层次中也不会形成平面。因此，在自底向上的过程中，我们仅使用每个体素中的平面特征点在局部BA中构建上层的关键帧。这个过程进一步节省了下一层自适应体素地图构建的时间，并提高了局部BA中的计算精度。
4. 自顶向下的姿态图优化(另一处贡献)
自顶向下的姿态图优化过程旨在减少自底向上的分层BA过程中的姿态估计误差，该过程仅考虑在同一局部窗口中可见的特征，而忽略了在不同局部窗口中观察到的特征。如图1所示，姿态图在金字塔结构中以自顶向下的方式构建。在金字塔的每一层中，因子是相邻帧之间的相对姿态。具体而言，第i层因子中节点x^i_j和x^i_{j+1}之间的成本项被定义为
其中，Ω^i_{j,j+1}是姿态x^i_j和x^i_{j+1}之间的相关矩阵，其中i ∈ L，j ∈ F^i，并且通过从自底向上的分层BA过程中获得的Hessian矩阵H的求逆来计算。L = {1, · · · , l}表示l层的集合，而F^i = {0, · · · , N_{i − 1}}表示N_i个数字的集合。由于节点x^{i+1}_j和x^i_{s·j}本质上是相同的，我们可以将它们视为同一个节点。
因此，(4)中的代价减少了
其中i ∈ L，j ∈ F^i，并且图1中的原始位姿图简化为图4。值得注意的是，当s < w时，连续局部窗口重叠区域内出现的帧可能会贡献多个代价项（每个局部窗口贡献一个）。要最小化的目标函数是：
其中 \mathbb{F} = {\mathbb{F}^1_i| i ∈ ^1} 是所有第一层帧的集合。然后使用Levenberg-Marquardt方法解决这个因子图，使用GTSAM进行优化。
图4：我们提出的方法的最终因子图，层数为l=3，步长为s=3，窗口大小为w=6
评论（0）
您还未登录，请登录后发表或查看评论