Large-Scale LiDAR Consistent Mapping usingHierarchical LiDAR Bundle Adjustment

1. 代码地址

GitHub - hku-mars/HBA: [RAL 2023] A globally consistent LiDAR map optimization module

2. 摘要

重建精确一致的大规模激光雷达点云地图对于机器人应用至关重要。现有的基于位姿图优化的解决方案，尽管它在时间方面是有效的，但不能直接优化建图的一致性。激光雷达集束调整(BA)最近被提出来解决这个问题；但是在大尺度规模建图上太费时间了。为了解决这一问题，本文提出了一种适用于大尺度地图的全局一致的高效建图方法。我们提出的工作由自底向上的分层BA和自顶向下的位姿图优化组成，结合了两种方法的优点。通过分层设计，我们用比原始BA小得多的Hessian矩阵来解决多个BA问题；通过位姿图优化，我们可以平滑高效地更新激光雷达姿态。我们提出的方法的有效性和鲁棒性已经在多个空间和时间上的大规模公共旋转激光雷达数据集上得到验证（KITTI，MulRan和Newer College），以及结构化和非结构化场景下的自行收集的固态激光雷达数据集。通过适当的设置，我们证明了我们的工作可以在只用大约12%的序列时间生成一个全局一致的地图。

3. 主要贡献

我们提出了一种分层集束调整方法，以全局优化激光雷达建图一致性和里程计精度。我们提出的方法在给定良好的初始姿态轨迹（例如，来自位姿图优化）的情况下提高了建图质量，并且甚至在初始姿态轨迹存在大漂移的情况下闭合了间隙。
我们提出的工作的有效性已经在结构化和非结构化场景中的多个公共机械旋转激光雷达数据集和我们自己收集的固态激光雷达数据集上得到验证。

4. 算法解析

4.1. 系统概述

图1，提出的分层集体束调整的金字塔结构，层数l = 3，步长s = 3，窗口大小w = 6。第一、第二和第三层因子连接同一层内的每两个相邻节点。红色虚线连接的节点应该是相同的，例如 $x_{9i}^1$ ，从下层的局部窗口的第一个节点到从上层的这个局部窗口构建的节点，例如 $x_{3i}^2$ 和 $x_{i}^3$ 。

我们提出的方法的系统工作流程如图2所示。输入是来自每次激光雷达扫描的原始点或去畸变后的点，以及它们在全局坐标系中的相应位姿的初始估计，这可以从通用激光雷达里程计或SLAM算法获得。该方法由自下而上和自上而下两个过程组成，迭代直到收敛。自下而上的过程中，在较小的局部窗口内对激光雷达帧执行局部BA，以构建从第一层到第二层的关键帧。该过程分层执行，直到满足最佳层数，并且在顶层关键帧上执行全局BA。然后，使用来自每个优化层和相邻层之间的因子来构建位姿图。如图1所示，第一层，也称为底层，描述了初始LiDAR帧和姿态的集合。同样，第二层表示使用局部BA从第一层创建的激光雷达关键帧和姿态的集合。顶层意味着最后剩余的激光雷达关键帧的集合（在图1中，顶层指的是第三层）。从底层到顶层分层创建激光雷达关键帧的过程称为自下而上过程。通过位姿图优化更新底层激光雷达位姿的过程称为自顶向下过程。

4.2. 自下而上的分层BA

我们定义 $F^i_j$ 为第i层的第j个雷达帧， $X^i_j \triangleq T^i_j = (R, t)$ 表示对应的位姿。 $T^i_{j,k}$ 表示 $T^i_{j}$ 和 $T^i_{k}$ 之间的相对位姿。需要注意的是， $F^i_j$ 中的点是在激光雷达的局部坐标系， $T^i_{j}$ 是在全局坐标系中。w是窗口的大小，s为激光雷达关键帧自下而上构建过程中的步幅大小。

假设在第i层一共有 $N_i$ 个雷达帧。在自下而上的过程中，每个局部窗口使用给定的初始位姿执行局部BA，优化得到帧间相对位姿和第一帧的位姿。从每个局部窗口中的BA得到的Hessian矩阵H也被记录，并用作后面的自上而下的位姿图构造中的信息矩阵。给定第i层一个局部窗口的w个激光帧 $\left\{F_{s j+k}^i \mid j=0, \cdots,\left\lfloor\frac{N_i-w}{s}\right\rfloor ; k=0, \cdots, w-1\right\}$ 和优化得到的相对位姿，我们将这些帧聚合成第i+1层的关键帧。将窗口内的点云都转到第一帧坐标系，聚合后的关键帧位姿表示为 $T^{i+1}_{j}$ ，被设置为在前一个局部窗口优化中得到的第一帧的位姿。

$\begin{aligned} & \mathbb{F}_j^{i+1} \triangleq \bigcup_{k=0}^{w-1}\left(\mathbf{T}_{s \cdot j, s \cdot j+k}^{i *} \cdot \mathbb{F}_{s \cdot j+k}^i\right) \\ & \mathbf{T}_j^{i+1}=\mathbf{T}_{s \cdot j}^{i *}=\prod_{k=1}^j \mathbf{T}_{s \cdot k-s, s \cdot k}^{i *} \end{aligned}$

图3，使用w = 10, s = 5和n = 8，层数N，位姿数l
可以看出，从原始BA到分层BA时间消耗显著减少。

对于自底向上的分层BA的每一层中的特征提取和关联，我们使用BALM中提出的自适应体素化方法，该方法提取适合于不同结构的环境的不同大小的平面特征。然而当点数非常大时，自适应体素化过程是耗时的。为了缓解这一问题，我们注意到，在较低层中不被视为平面要素的点也不会在较高层中形成平面。因此，在自下而上的过程中，我们仅使用来自局部BA中每个体素的平面特征点来构建上层的关键帧。该过程进一步节省了下一层自适应体素地图构建的时间，并提高了局部BA中的计算精度。

4.3. 自上而下的位姿图优化

自上而下的姿态图优化过程旨在减少自下而上的分层BA过程中的位姿估计误差，因为自下而上的分层BA仅考虑了同一局部窗口中的共视特征，而忽略了在不同局部窗口中共视的那些特征。

位姿图在金字塔结构中以自顶向下的方式构建。在金字塔的每一层中，因子是相邻帧之间的相对位姿。具体来说，节点 $X^i_{j}$ 和 $X^i_{j + 1}$ 之间的第i层因子中的代价函数定义为：

其中， $i\in{\mathcal{L}},\ j\in{\mathcal{L}},\ Z=\{1,\cdot\cdot\cdot,l\},\ {\mathcal{F}}^{i}=\{0,\cdot\cdot\cdot,N_{i}-1\}$

由于节点 $X^{i+1}_{j}$ 和 $X^{i}_{sj}$ 本质是同一个，所以有:

因此，代价函数可以简化为：

最终的位姿图

整体的目标函数：

其中F是第一层所有关键帧的集合，最终使用GTSAM中的LM求解该图优化问题。

5. 实验

5.1. 精度分析

数据集：KITTI、MulRan、New College、Self Collected Dataset

对比方案：MULLS、LIO-SAM、FAST-LIO2、CT-ICP、GICP Matching Factor

带回环的初始位姿

图5 在KITTI数据集序列07上，（A）MULLS建图结果；（B）HBA优化后的结果；A白色区域中的点云分层现象在B中明显消失。

在MulRan数据集DCC03上的建图效果。（A）LIO-SAM带回环的建图结果；（B）HBA建图结果；

白色区域底部中放大的部分显示了A中存在点云分层现象，B中点云地图一致性更好。

不带回环的初始位姿

图7 (A) 是在KITTI数据集00序列上，MULLS不带回环的建图结果；（B）是HBA建图结果；（A）中白色区域显示了点云重影误差，在B中明显消失；

图8 （A）是FAST-LIO建图结果；（B）是HBA优化后的结果；(B)中白色区域点云重影消失。

图9（A）是FAST-LIO建图结果；（B）是HBA优化后的结果；(B)中侧视图点云地面高度一致，俯视图中重影消失。

由于没有真值，使用MME评估建图质量。

5.2. 消融实验

位姿图优化 VS 直接指定

为了更新底层姿态，一种简单的方法是直接将优化的上层姿态分配给下层姿态，例如，使用上层关键帧的优化姿态在相应的局部窗口内更新下层的前s个姿态。

图10 (A)直接指定结果；（B）从上到下的优化；从黄色虚线区域可以看出，我们提出的位姿图优化在位姿估计精度和建图一致性方面都优于直接分配优化结果。

分层BA VS 简化BA

表8记录了原始BA、简化BA、分层BA在MulRan Dataset的耗时和精度结果。

5.3. 算力消耗

6. 总结

本文提出一种基于分层BA和位姿图优化的方法，对大规模激光雷达点云的位姿估计精度和建图一致性进行优化。利用自下向上的分层BA，我们用比原始BA方法小得多的Hessian矩阵并行求解多个BA问题。通过自上而下的位姿图优化，我们可以平稳高效地更新激光雷达位姿。在给定良好的初始姿态轨迹或大漂移的情况下，我们在具有结构化和非结构化场景的空间和时间大规模激光雷达数据集上验证了我们工作的有效性。在多个公共旋转激光雷达和我们自己收集的固态激光雷达数据集上，我们证明了我们提出的工作在位姿估计精度和建图一致性方面优于其他SOTA方法。在我们未来的工作中，我们可以将IMU预积分和激光雷达测量噪声模型结合到我们的分层BA工作中。