基于分层分段的SLAM算法优化(IROS 2021)

Hierarchical Segment-based Optimization for SLAM

作者：Yuxin Tian, Yujie Wang, Ming Ouyang, Xuesong Shi来源：IROS 2021论文地址：https://arxiv.org/pdf/2111.04228.pdf

作者：chaochaoSEU|来源微信公众号：3D视觉工坊

摘要：

本文提出了一种用于SLAM系统的分层分段优化方法。首先，我们提出了一种可靠的轨迹分割方法，可用于提高后端优化的效率。然后，我们首次提出了一种缓冲机制来提高分割的鲁棒性。在优化过程中，我们利用全局信息对误差较大的帧进行优化，用插值代替优化更新估计好的帧，根据每一帧的误差分层分配计算量。与基准上的对比实验表明，我们的方法大大提高了优化效率，且几乎没有精度下降，大大优于现有的高效优化方法。

1 引言

随着各种相机的广泛使用和三维计算机视觉的发展，SLAM和SfM系统在过去的几十年中得到了广泛的研究。为了提高建图和定位的精度，使用后端算法优化3D位姿信息是必不可少的环节。Bundle Adjustment (BA) 是一种常用于 SfM 和 SLAM 系统的优化方法。同时，位姿图是 SLAM 系统中常用的另一种优化方法。与同时优化位姿和地标（地标表示特征点在世界坐标中的 3D 位置）的 BA 不同，位姿图仅优化位姿。此外，BA 使用重投影误差作为优化目标，而位姿图使用相对位姿误差。然而，传统的BA和位姿图计算量大，迭代次数多，消耗大量计算资源和时间，阻碍了许多实时、大规模、复杂的多机器人应用。为了提高系统的效率，许多论文对BA方法提出了各种改进，而很少考虑对位姿图的改进。此外，与 SfM 相比，旨在提高 SLAM 系统效率的同时保持位姿估计精度的工作相对较少。到目前为止，可以提高BA效率的方法主要有两类，即Incremental BA和Hierarchical BA。Incremental BA 主要遵循增加当前地图的思想，通过增量合并新的测量来减少计算量。但是这种方法主要是为了提高SLAM系统中前端局部优化的效率，由于缺乏全局优化，估计位姿容易漂移。Hierarchical BA 通常采用将整个轨迹分成几段的方法。然后在每个段内和段之间使用不同的优化方法来提高计算效率。许多论文从各个方面研究了分层 BA。然而，目前的方法有一些缺点。一些方法[1][2][3]在不降低计算复杂度的情况下减少了迭代次数，导致效率提升有限。其他方法 [4] [5] [6] [7] [8] 使用每个段内的局部优化方法和不同段之间的对齐来代替全局优化，但由于缺少全局误差分布，系统精度不理想。还有一种方法[7]，主要关注不同段之间连接部分的误差，而忽略了段内的优化。但是这种方法对分割的精度要求很高，而且容易出现段之间的连接不平滑的情况。我们的工作遵循Hierarchical BA 的路线，并将思想扩展到位姿图优化。在本文中，我们提出了一种适用于SLAM的基于分段的优化方法。与传统的以SfM为主的高效优化方法不同，我们充分利用SLAM系统有序丰富的先验知识来提高优化算法的效率。我们使用SLAM系统中跟踪过程中产生的速度信息和重投影误差信息进行分割。这种方法试图为段内的位姿提供更准确的结果，而为段间的位姿提供一个不太理想的结果。这样，在进行全局优化时，会更多地关注精度较差的区域，因此我们可以在提高效率的同时获得更好的优化结果。同时，我们设置了分割时的缓冲机制。我们添加多个单独的帧作为两个相邻段之间的缓冲区。例如，图 1 显示了分割结果的示例。我们在执行全局优化时优化缓冲区内的所有帧。这种方法充分利用全局信息来减少不同段之间的误差，以达到更高的精度结果。此外，缓冲区的使用还可以缓解分割精度的问题。这样，我们不再需要过多关注分裂点的准确位置，而只需关注一个近似位置，可以有效提高鲁棒性。

图1 上图分别为KITTI的00和05序列的分割结果。彩色区域代表段，黑色区域代表缓冲区。在下半部分，我们展示了不同高效优化算法对每个 KITTI 序列（每个点代表一个序列）的准确率（RMSE）和效率（时间）性能。请注意，这些值除以轨迹的长度。

最后，在进行全局优化时，对每个段的前几帧和最后几帧以及缓冲区中的帧进行全局优化。然后将每个段中参与全局优化阶段的那些帧用于插值以更新段内的位姿信息。具体算法在第三节详细描述。我们将该方法应用于开源SLAM数据集，包括KITTI [9]、TUM RGB-D [10]和EuRoC [11]，结果表明我们的方法在效率和准确性方面明显优于现有方法，如图1（C）。总之，本文的贡献包括：• 我们提出了一种混合轨迹分割和缓冲的方法，可用于SLAM 系统的后端优化，以提高计算效率，同时缓解对分割精度要求过高的问题。• 我们率先引入基于插值的方法来替代大量优化以有效计算内段位姿，同时确保几乎没有精度损失。• 我们为 SLAM 中的位姿图和全局 BA 提出了一种基于分段的统一优化方法，我们的系统在效率和准确性方面都显着优于以前的方法。

II．相关工作

有多种方法旨在解决在光束调整等优化阶段由于帧数增加而导致的效率问题，尤其是在大规模环境中。早期的工作[12]利用了BA中Hessian矩阵的稀疏模式，提出先使用Schur补计算相机位姿，然后使用后代替代地标。它显着降低了BA的复杂度，后来被引入到g2o等常见求解器中[ 13]。一项研究 [14] [15] [16] [17] [18] [19] [20] 使用增量 BA 的方法，其中新帧增量合并到现有地图。然而，系统的性能受到初始化差和累积错误的影响，特别是对于长序列。此外，Hierarchical BA有增长趋势，它将优化分为几个阶段，每个阶段只优化子节点组[21] [1] [2] [3] [4] [5] [6] ] [7]。选择优化节点的子组，最常用的方法是将轨迹划分为若干段，其中每段内的误差较小，段间的误差较大。这样，我们可以从解决更容易和更快收敛的小子问题开始，然后设计不同的段合并策略将段绑定在一起。从而可以提高BA的整体效率，优化结果更加稳定。在[1]中，首先基于相似性度量对图像进行聚类，并进行局部BA优化簇内的位姿，然后使用全局BA合并簇。[2] 在概念上是相似的，除了使用成对对齐的绑定片段。[3] [4] 尝试对集群进行并行计算，从而更好地提高效率。[6] [7] 在优化期间以自适应方式在迭代之间改变分区方式，这引入了额外的复杂性成本。但这些方法主要针对解决离线SfM问题。对于段划分标准，co-visibility [5] [6] 经常被用作标准。例如，[5] 为 SLAM 系统设计了一种混合方法，它利用空间和时间的共同可见性来确定当前帧与附近帧的连接性，并相应地分割轨迹。然而，共同可见性能否可靠地反映误差度量是值得怀疑的。例如，在共可见度较大的地方，误差不一定很小，因为我们无法保证地标的 3D 位置的准确性。[7]使用连续帧之间最陡下降方向的角度作为分割轨迹的标准，只考虑局部变化。此外，[7]声称虽然重投影误差可以作为一种解决方案，但它可能会将轨迹分成太多段，因为大误差往往分布在选择分裂点的区域。然而，我们认为这个问题可以通过在段之间引入缓冲区来解决，缓冲区内的所有帧都将被视为需要后续优化的大错误区域。除了重投影误差，我们还使用速度信息来更好地识别分裂点，并且系统能够可靠地运行。要将段合并在一起，自然要在每个段的边缘使用帧进行对齐或优化。例如，[5] 使用局部 BA 来优化段内的位姿，并使用基于运动平均的对齐方法来计算段之间的相对变换并将它们合并在一起。由于仅估计成对变换，因此很容易导致累积误差。因此，在提高 BA 过程的效率的同时，结果的准确性受到显着影响。相反，[7] 提出通过分配单个 7-DoF 相似变换将每个段视为刚体，并应用全局 BA 进行仅优化重叠帧，从而避免漂移误差。段内帧的位姿通过优化前后的相对位姿变化来更新。但是，对齐方式很容易造成段之间的不一致。为了解决这个问题，划分和优化过程是分层进行的，很繁琐。此外，我们认为在段的头部和尾部分配两个 7-DoF 相似变换可以轻松解决不一致问题。段内帧的位姿可以通过类似于[22]的插值方法获得，系统的精度几乎保持不变。

III.提出的方法

在目前的SLAM框架中，后端的优化主要有两部分，一是利用相机位姿信息的位姿图优化，二是利用重投影误差的BA优化。我们提出的分割方法可以有效提高两个优化模块的计算效率。具体来说，我们将我们的方法分为以下五个部分进行详细描述。第 III-A 节展示了我们如何使用 SLAM 系统的先验信息进行分割。第 III-B 节详细描述了提议的缓冲机制。第 III-C 节和第 III-D 节描述了将所提出的方法分别纳入位姿图优化和BA优化模块的方法。最后，第 III-E 节描述了全局优化后段内相机位姿的插值方法。我们方法的流程如图2所示。

图2.所提方法的流程。上半部分展示了现有 SLAM 系统的整体流程，下半部分展示了我们基于分段的方法对每个相应流程的主要思想。

A.分割方法

为了提高系统的效率，全局优化应该主要优化精度较低的部分，而较少关注精度较好的部分。因此，理想的帧分割应该遵循：• 对于每个片段，帧的姿态估计和片段内的地标相对准确。• 段之间连接部分的位姿估计有较大的误差。为了达到目的，我们分析了SLAM系统中位姿图优化（1）和BA优化（2）的数学形式。

图优化中最好的分割点应该在误差较大的边附近。根据（1），图中的分割点应该是位姿图优化中相邻位姿变换估计较差的帧。并且根据（2），优化图中的段点应该是BA整优化中具有较大重投影误差的帧。因此，分裂点应该是重投影误差大或相对位姿误差大的帧。在SLAM系统的跟踪过程中，由于测量噪声不可避免地会出现估计误差，但这些误差一般很小。然而，环境中的一些剧烈变化会导致位姿估计误差的突然增加，例如碰撞、漂移或特征点检测和匹配误差。这些条件在 SLAM 系统中反映为速度的快速变化或增加的重投影误差。分裂点的位置应该反映这些意外的变化。因此，我们以相邻帧的位姿变换速度和每帧的重投影误差作为分割的标准。等式（3）显示了该准则的数学形式。有关缓冲区和等式 (4) 的详细信息将在第 III-B 节中讨论。

与之前主要使用公共视图地标的数量作为分割标准的分割方法[2][5][6]相比，我们的方法与优化方程的关系更紧密，更量化，与最终定位准确性更相关。不同分割方法见表III(a)。

B. 缓冲机制

对于传统的分割方法，两个相邻的段直接相连，如图3（a）所示。因此，分割时需要仔细处理分裂点的具体位置，因为不同的分割会产生不同的输出，这会影响优化器的鲁棒性。另外，在SLAM系统中，通常会看到多个连续帧的位姿估计都不准确。将这些帧视为一个片段是不合理的。为了解决这个问题，我们设计了一个缓冲机制。在划分的两个段之间，放置一个缓冲区，如图3（b）所示。在切分过程中，首先使用A段的方法判断当前段是否要结束。然后将后面的帧放入缓冲区，直到速度变化和重投影误差都恢复到一个相对稳定的水平。最后，创建一个新段并重复上述过程。缓冲区选择结束的判据如(4)所示。

图3 缓冲机制示意图。段由彩色帧组成，缓冲区由黑色帧组成。

同时，为了方便后续的优化，我们将每个段按照segment中帧的顺序分为head、tail、inside三个部分。在实验中，我们使用前两帧作为头部，后两帧作为尾部。最终，SLAM 系统中的每一帧都属于以下四类之一：头部、尾部、内部和缓冲区。分割结果的定义如图 4 所示。

图4. 分割结果的定义。红框、蓝框和绿框分别代表该段的头部、内部和尾部。每个段由这三个部分组成。缓冲区中的帧显示为黑色。

缓冲机制有效缓解了传统分割方法对分割精度的过高要求，提高了优化器的鲁棒性。缓冲机制的设计也可以灵活处理连续不准确的位姿估计。同时，缓冲区的使用几乎不会对系统产生额外的计算成本。与传统的分割方法相比，它牺牲很少的效率来换取鲁棒性和准确性的提高。

C. 位姿图优化

SLAM系统中的位姿图优化主要是利用帧的相对位姿变换来优化。优化图中的节点代表帧在世界坐标系中的位姿，节点之间的边代表相对位姿变换。传统位姿图优化中的优化图如图5(a)所示。

图5 位姿图方法。实线方块代表优化图中的位姿节点。虚线方块代表需要通过插值更新的位姿节点。并且连接线表示在优化过程中用作边缘的相对位姿变换。

在我们的方法中，我们通过分割信息改变优化方程以提高优化效率。我们将整个优化过程分为全局优化和segment内姿态更新两个步骤。在接下来的部分中，我们遵循图4所示的四类segmentation的定义。在全局优化阶段，我们利用分割信息来减少位姿图中的节点和边的数量，从而减少优化器的计算量。我们删除段内部的所有节点及其相关边。但是为了在全局优化时保证位姿图的连通性，我们将段头节点和段尾节点之间的边连接起来，以近似删除的边。最后，需要优化的位姿图的节点和边图如图5（b）所示。由于位姿图中的节点和边发生了变化，优化方程也发生了变化，如（5）所示。通过减少参与优化的节点和边的数量，我们大大提高了优化的速度。

经过位姿图优化后，进入segment内位姿更新阶段。从上一步中，我们只能得到head、tail和buffer中节点的优化结果，因此我们需要使用另一种方法来获取segment内部帧的更新结果。对于每一段，我们固定头部和尾部的帧，并使用更有效的插值而不是优化来更新内部的节点。插值方法将在第 III-E 节中详细描述。这样，SLAM系统中每一帧的位姿就有了最终的优化结果。

D. BA优化

SLAM系统中的BA优化使用了landmarks位置和帧位姿之间的重投影误差来优化。优化图中的节点表示帧的位姿和地标的 3D 坐标，边表示从地标到帧的重投影误差的估计。与位姿图优化方法类似，整个优化过程也分为两个步骤：全局位姿优化和段内位姿更新。在全局优化阶段，首先简化节点和边。与位姿图方法类似，我们保留位于头部、尾部和缓冲区中的帧节点。此外，为了保证连通性，我们选择了最少数量的互连帧作为每个段的头部和尾部之间的连接帧。在实际操作中，我们从与头部区域最后一帧的共视度大于30的帧中，按时间顺序选取最后一帧加入到连接帧中。新的连接框架。重复上述过程，直到最后选择的帧与尾部区域的第一帧的covisibility超过30。这样，我们选择最少的连接帧来保持用于束调整的帧的连通性。最后，我们执行全局优化的帧节点包括head、tail、buffer和连接帧中的帧。对于地标节点，仅保留将边连接到框架节点的节点。BA 的原点优化图如图 6(a) 所示，我们提出的原点优化图如图 6(b) 所示。随着优化图中节点和边的变化，优化方程也发生变化，如式（6）所示。

图6. BA方法。实线方块代表优化图中的帧节点，虚线方块代表需要通过插值更新的位姿节点。特别是，线段内部的实线方块代表连接框架。绿点代表地标节点，连接线代表重投影误差边。

经过全局BA优化后，进入segment内位姿更新阶段。与位姿图模块类似，对于每个段，我们固定头部、尾部和连接帧中的帧。然后使用插值而不是优化来更新段内部的其他节点，然后对齐相对地标位置。这样，SLAM系统中的每一帧姿态和地标坐标都会得到最终的优化结果。

E. 段内插值

在本节中，我们将介绍插值方法。我们使用这种方法根据全局优化阶段后的优化帧获得未优化帧的近似结果。[22]表明，当需要估计的帧两侧都有通过优化得到的准确位姿结果和帧之间位姿变换关系的准确估计时，我们可以使用插值代替优化来得到近似的优化结果。可以避免繁重的计算。因此，我们使用改进的插值方法代替优化方法来有效地计算结果。在位姿图优化模块中，段的头部和尾部的帧将参与全局优化并获得更准确的位姿结果。此外，根据A段的分割策略，该段内帧间位姿变换的精度也比较高。我们使用头部和尾部的帧位姿作为参考。而对于segment内部不参与全局优化的每一帧，我们利用当前帧与参考结果的变换关系来完成插值。插值算法的数学形式如(7)所示，其中R、t和s分别代表旋转矩阵、平移向量和尺度。

我们使用相对速度作为插值因子。然后我们使用相对位姿变换分别根据头部和尾部的结果计算当前帧的旋转矩阵。最后，使用球面线性插值获得最终的旋转结果。并且平移的插值结果也是用类似的方法计算出来的。对于BA模块，连接帧和段的头尾部分的帧都将参与全局优化。因此，用于插值的参考帧除了包含片段头部和尾部的帧之外，还将包括连接的帧。其余的与位姿图模块中的相同。完成帧位姿的插值后，未参与BA的地标将根据其与参考帧的位置关系进行更新。由于全局优化的估计精度高，使用插值代替优化可以获得精度损失很小的结果，但可以节省大量时间。同时，与[7]等直接不对段内误差进行优化的方法相比，插值可以有效地消除误差。我们的方法可以在效率和准确性之间取得更好的平衡。

IV. 评估

在本节中，我们对不同的优化算法进行了实验，主要比较了方法的效率和准确性。本文提出的分割方法主要适用于SLAM系统。因此，我们的实验也在 SLAM 数据集中进行，包括 KITTI [9]、TUM RGB-D [10] 和 EuRoC [11]。我们已经基于开源 SLAM 系统 OpenVSLAM 实现了我们的优化算法。因此，首先我们将我们的优化方法与 OpenVSLAM [23] 中使用的传统方法进行比较，以验证我们的改进。此外，我们还将我们的算法与其他高效优化算法进行了比较，以反映我们方法在效率和准确性方面的性能。最后，我们进行ablation实验以验证我们方法中每个模块的效果。

A. 基线比较

在与基线的对比实验中，我们选择 SLAM 数据集中有回环的序列进行实验。我们分别记录了SLAM系统在每个序列中检测回环时位姿图优化和BA的优化时间，以比较效率。此外，我们记录每一帧的最终位姿估计结果，并计算轨迹估计RMSE作为精度的比较。最终的实验结果如表I所示。与传统的OpenVSLAM优化算法相比，我们的方法在精度方面几乎没有损失，但效率大大提高。我们观察到我们的方法在某些序列中的准确性有意外的轻微提高。当优化结果陷入局部最优时，可能会发生这种情况，因为优化中涉及的帧过多。需要对根本原因进行更多调查。

B. 高效优化实验

在高效优化算法的对比实验中，使用的数据集是KITTI数据集的00-10序列。我们删除了 01 序列，因为视觉 SLAM 系统会在跟踪阶段丢失。在实验中，我们的方法与传统的SLAM算法ORBSLAM2[24]，以及两种最好的层次优化算法ENFT SfM[7]和Hybrid BA[5]进行了比较。由于有适合SfM的优化算法，为了保证实验的公平性，我们对SLAM系统中的后端优化做了自适应修改。SLAM系统中所有帧的跟踪完成后，我们将再次执行全局BA。我们记录最终的全局BA时间作为效率的评估。同时，将最终优化结果的绝对轨迹误差（ATE）作为准确度的评价。实验结果如表II所示。结果表明，我们的方法在基于段的优化算法中在准确性和效率方面都达到了最佳性能。

C. Ablation实验

为了验证我们提出的方法中每个模块的效果，我们进行了Ablation实验。实验方法类似于第 IV-B 节中的方法。在 KITTI 的 00-10 序列上执行比较实验。记录最终全局BA的效率和准确性以供比较。结果示于表III中。

Ablation实验分为三个部分，分别用于验证我们的分割方法、缓冲机制和插值方法的效果。在第一个实验中，为了验证我们的分割方法的优越性，我们将所提出的分割方法与几种传统的分割方法进行了比较，包括共同视点的数量、重投影误差、速度和定长分割。经验证，我们的分割方法在优化的准确性和效率方面都具有更好的性能。然后为了验证缓冲区的效果，我们去掉了缓冲区，改为段间直接连接，其他改进保留。实验结果表明，删除缓冲区后，优化的准确率和效率都有所下降。最后，我们将我们的插值方法与使用局部 BA 和线性插值的方法进行比较，以验证我们的插值方法的有效性。结果还表明我们的插值方法表现最好。

V 结论

在本文中，我们提出了一种基于分层分段的 SLAM 系统优化方法。我们提出了一种可靠的轨迹分割方法，可用于提高后端优化的效率。然后，我们首次提出了一种缓冲机制来提高分割的鲁棒性。此外，我们使用全局信息来优化估计不佳的帧，并使用插值代替优化来更新估计好的帧。这种分层优化方法可以在保证精度几乎没有损失的同时，大大提高效率，优于以往的高效优化方法。对比实验也验证了我们方法的有效性。然而，分层分割优化仍有一定的改进空间，包括使用更多的理论方法来代替当前的分割方法或提出分割和优化的紧耦合方法以获得更好的结果。

备注：作者也是我们「3D视觉从入门到精通」特邀嘉宾：一个超干货的3D视觉学习社区本文仅做学术分享，如有侵权，请联系删文。

3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进

4.国内首个面向工业级实战的点云处理课程

5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解

6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦

7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化

8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现