基于PGO方法实现快速且精确地求解SLAM优化和估计问题

同时定位和建图（SLAM）是一种重要的工具，它使移动机器人能够在未知环境中自主地导航。正如SLAM这名称所示，其核心为获取环境的正确表示以及估计地图中机器人位姿的正确轨迹。主流的最先进的方法使用基于最小二乘法的图优化技术来求解位姿估计问题，其中最流行的方法为g2o、Ceres、GTSAM和SE-Sync等库。本文的目的是以统一的方式来描述这些方法，并且在一系列公开可用的合成位姿图数据集和真实世界的位姿图数据集上对它们进行评估。在评估实验中，对四个优化库的计算时间和目标函数值进行分析。

介绍

基于滤波的方法已经在SLAM场景中占据主导地位多年，其越来越多地被基于优化的方法所取代。位姿图优化（PGO）首次在Lu等人的工作（Globally Consistent Range Scan Alignment for Environment Mapping）中被引入，但是由于计算效率低的原因，它并不是非常流行。如今，随着计算能力的提高，PGO方法已经成为最先进的方法，并且能够快速且精确地求解SLAM优化和估计问题。

基于优化的SLAM方法通常由两部分组成。第一部分通过基于传感器数据的对应关系来识别新观测数据和地图之间的约束。第二部分计算给定约束情况下的机器人位姿和地图，这能够被分为图和平滑方法。当前最先进的基于优化方法的一个例子为g2o（A general framework for graph optimization），它是一种针对非线性最小二乘问题的通用优化框架。最初的平滑方法之一为，它由Dellaert等人提出（Square root SAM： Simultaneous localization and mapping via square root information smoothing）。一种对该方法的改进为增量式的平滑和建图（iSAM），它由Kaess等人引入（iSAM： Incremental smoothing and mapping）。iSAM对进行扩展以通过更新稀疏平滑信息矩阵的因子分解为整个SLAM问题提供一种有效的求解方式。iSAM的升级版本iSAM2由Kaess等人提出（ISAM2： Incremental smoothing and mapping using the Bayes tree）。这些平滑方法在GTSAM（https://gtsam. org）中实现，它是另一个最先进的优化库。

SLAM问题的一个很好的例子是所谓的位姿SLAM，它避免了构建环境的显式地图。位姿SLAM的目标是要在给定回环和里程计约束的情况下估计机器人的轨迹。这些相对的位姿测量通常使用自身运动估计、扫描匹配、迭代最近点（ICP）或者一些最小化视觉重投影误差的形式，从IMU、激光传感器、相机或者轮式里程计中获取。值得注意的是，Eustice等人和Lenac等人提出的方法为基于滤波的位姿SLAM算法，但是本文的关注点为实现基于优化的位姿SLAM的方法。

最流行的优化方法有g2o、Ceres Solver（http://ceres-solver.org）、GTSAM和SE-Sync（特殊欧式群SE（n）上的同步）（SE-Sync： A certifiably correct algorithm for synchronization over the special Euclidean group）。文献中很少有比较这些方法的工作。例如，Latif等人提供了视觉SLAM的概述，并且将g2o、GTSAM和HOG-Man作为后端进行比较。Carlone等人讨论位姿图估计中旋转估计的重要性，并且在基准数据集上将g2o和GTSAM与不同的旋转估计技术进行比较。Li等人在g2o框架下比较不同的优化算法。Zhao等人提出统一的SLAM框架——GLSLAM，它提供了各种SLAM算法的实现，还能够对不同的SLAM方法进行基准测试。然而，据我们所知，目前没有一种统一的方式来比较g2o、Ceres、GTSAM和SE-Sync。本文的目的为以统一的方式来描述这些方法，并且在一系列公开可用的合成位姿图数据集和真实世界位姿图数据集上对它们进行评估，如下图所示。

在未来，我们想要通过这种比较来促进PGO方法的选择。

本文组织如下。第二节描述通用的非线性图优化以及这四种方法中的每一种。实验是本文的主要部分，在第三节中进行描述，在该节的第一部分，描述了硬件、实验设备和基准数据集。在该节末尾讨论了实验的结果。最后，第四节对本文进行总结。

非线性位姿图优化方法

每个位姿图由节点和边组成。位姿图中的节点对应于环境中机器人的位姿，以及边表示它们之间的空间约束。相邻节点间的边为里程计约束，以及其余边表示回环约束。这在下图图（a）中可视化。

位姿图优化的目标是要寻找一种节点的配置，使得位姿图中所有约束的最小二乘误差最小。一般而言，非线性最小二乘优化问题能够被定义如下：

其中，为图中所有约束上的误差总和：

这里，表示连接节点间索引对的集合，表示节点和节点之间的信息矩阵，为非线性误差函数，它建模位姿和满足由测量施加的约束的程度。最后，每个约束用信息矩阵和误差函数进行建模，如下图图（b）所示。

传统上，公式（1）的解通过迭代优化技术（例如，高斯-牛顿或者莱文贝格－马夸特方法）获取。他们的思想是用误差函数在当前初始估计附近的一阶泰勒展开来近似该误差函数。一般而言，它们由四个主要步骤组成：

1）固定一个初始估计值；

2）将问题近似为凸问题；

3）求解步骤2）并且把求解结果作为新的初始估计值；

4）重复步骤2）直到收敛。

位姿SLAM更容易求解，因为它没有构建环境的地图。由图构建的问题具有稀疏的结构，所以计算速度更快，另一个优势在于它对糟糕的初始估计值具有鲁棒性。位姿SLAM的缺陷在于它通常对异常值不鲁棒，并且当存在很多错误回环时无法收敛。此外，旋转估计使得它成为一个困难的非凸优化问题，因此凸松弛导致问题具有局部极小解，并且无法保证全局最优。在本节中，我们简要地描述基于非线性最小二乘方法的优化框架，这些框架以位姿图的形式提供解决方案。

A.g2o

g2o是一个开源的通用框架，用于优化能够被定义为图的非线性函数。它的优势在于易于扩展、高效并且适用于广泛的问题。作者在他们的工作中指出，他们的系统与其它最先进的SLAM算法相比，同时具有高度通用性和可扩展性。他们通过利用图的稀疏连接和特殊结构、使用改进的方法来求解稀疏线性系统并且利用现代处理器的特性，从而实现高效性。该框架包含三种不同的方法来求解PGO：Gauss-Newton、Levenberg-Marquardt和Powell的Dogleg方法。框架主要用于求解机器人中的SLAM问题和计算机视觉中的bundle adjustment问题。ORB-SLAM使用g2o作为相机位姿优化的后端，SVO将它用于视觉里程计。

B.Ceres

Ceres Solver是一个开源的C++库，用于建模并且求解大规模、复杂的优化问题。它主要致力于求解非线性最小二乘问题（bundle adjustment和SLAM），但是也能够求解通用的无约束优化问题。该框架容易使用、移植并且被广泛优化以提供具有低计算时间的求解质量。Ceres被设计用于支持使用者定义并修改目标函数和优化求解器。实现的求解器包括置信域求解器（Levenberg-Marquardt，Powell的Dogleg）和线搜索求解器。由于Ceres具有很多优势，因此它被用于很多应用和领域。OKVIS和VINS使用Ceres来优化定义为图的非线性问题。

C.GTSAM

GTSAM是另一个开源的C++库，它实现了针对机器人和计算机视觉应用的传感器融合。GTSAM能够被用于求解SLAM、视觉里程计和由运动恢复结构（SfM）中的优化问题，它使用因子图来建模复杂的估计问题，并且利用它们的稀疏性来提高计算效率。GTSAM实现Levenberg-Marquardt和Gauss-Newton风格的优化器、共轭梯度优化器、Dogleg和iSAM（增量式平滑和建图）。GTSAM与学术界和工业界中各种传感器前端一起使用。例如，SVO的一种变体（On-Manifold Preintegration for Real-Time Visual-Inertial Odometry）使用GTSAM作为视觉里程计的后端。

D.SE-Sync

SE-Sync是一种被证实正确的算法，用于在特殊欧式群上执行同步。SE-Sync的目标是在给定节点间相对变化的噪声测量情况下估计一组未知的位姿（欧氏空间中位置和姿态）值，它的主要应用是在2D和3D几何估计，例如位姿图SLAM（机器人中）、相机运动估计（计算机视觉中）和传感器网络定位（分布式感知中）。作者在其工作中指出，SE-Sync通过利用一种新型的特殊欧式同步问题的（凸）半正定松弛来直接搜索全局最优的解以改进之前的方法，并且能够对于找到的解生成正确性的计算证明。他们应用截断的牛顿黎曼置信域方法来寻找位姿的高效估计。

实验

本文的目标是要通过实验评估上一节中描述的优化框架，并且比较它们。出于这个目的，本文在总计算时间和目标函数的最终值（由公式（2）描述）方面考虑它们的性能。本文使用公开可用的合成基准数据集和真实世界的基准数据集。

A.实验设备

实验是在配备有八核Intel Core i7-6700HQ CPU、2.60GHz运行频率和16GB内存的Lenovo ThinkPad P50上进行的。该计算机运行Ubuntu 20.04系统。g2o、Ceres和GTSAM框架选择相同的求解器Levenberg-Marquardt，而SE-Sync使用黎曼置信域（RTR）方法。每种算法限制最大迭代100次。停止条件基于达到最大迭代次数和相对误差减少。SE-Sync使用一种稍微不同的方法，所以必须指定一种额外的基于黎曼梯度范数的条件。相对误差减少的公差范围被设置为，以及梯度的范数被设置为。根据SE-Sync工作中的建议来选择参数。Carlone等人研究姿态初始化对于寻找全局最优的影响（Initialization techniques for 3D SLAM： A survey on rotation estimation and its use in pose graph optimization）。受到该工作的启发，我们还在优化过程之前进行位姿图初始化。为了取得更好的结果，我们使用生成树方法来获取初始的位姿图。

B.基准数据集

Carlone等人获取六个两维的位姿图，包括两个真实世界的位姿图和四个在仿真中创建的位姿图。INTEL和MIT位姿图是真实世界的数据集，它们通过处理在西雅图Intel Research Lab和MIT Killian Court采集的原始轮式里程计和激光雷达传感器测量数据来创建。M3500位姿图是一个仿真的曼哈顿世界（Fast iterative alignment of pose graphs with poor initial estimates）。M3500a、M3500b和M3500c数据集是M3500数据集的变体，它们在相对姿态测量数据中加入高斯噪声。噪声的标准偏差分别为0.1、0.2和0.3弧度。Carlone等人还获取六个三维的数据集。位姿图Sphere-a、Torus和Cube在仿真中创建，其中Sphere-a数据集是由Stachniss等人发布的一个有挑战性的问题（http://www.openslam.org/）。其它三个位姿图数据集是真实世界的数据集。Kummerle等人引入Garage数据集（Autonomous driving in a multi-level parking structure），Cubicle和Rim数据集是通过在来自佐治亚理工学院RIM中心的3D激光传感器的点云上使用ICP方法来获取的。所有这些位姿图在本文第一张图中可视化，图中显示它们的里程计和回环约束。表格I中包含每个数据集的节点和边的数量。这些数量确定了优化过程中参数的数量和问题的复杂度。

C.结果

本文对于总计算时间和目标函数值（公式（2））方面在表格II中总结了所有的性能结果。对于每一个数据集，我们指出算法的终止原因。如果算法在最大迭代次数的限制内完成优化，则该算法收敛。

我们还给出SE-Sync的验证时间以证明全局最优。下面三张图展示针对基准测试问题的优化后的位姿图。

1）INTEL：INTEL是最易求解的问题之一。所有方法均已经成功对其进行求解。所有轨迹显示在下图图（a）中，并且由图可知它们均获得了相似的结果。GTSAM耗费最长的时间来完成优化，但是取得最低的目标函数值。在这种情况下，SE-Sync速度最快，并且其目标函数值略大于GTSAM。考虑到这一点，SE-Sync似乎是最好的求解方案。

2）MIT：MIT是最小的问题，但是仅具有一些回环约束。因此，从一个好的初始估计值开始优化是非常重要的。否则，GTSAM、Ceres和g2o无法通过Levenberg-Marquardt算法收敛到一个有意义的解。所有算法在最大迭代次数内收敛，并且它们获得几乎相同的目标函数值。优化问题在半秒内求解，并且Ceres和g2o速度最快。Ceres还获得最低的目标函数值，因此似乎是数据集MIT的最佳求解器。最终的轨迹如下图图（b）所示。

3）M3500：M3500的所有四种变体均在这里展示。GTSAM和Ceres在求解基础的M3500问题中表现最佳，因为它们均取得最低的目标函数值，但是Ceres速度稍微更快。所有方法都能成功求解该问题，并且它们优化后的位姿图如下图图（c）所示。M3500a是一个更为困难的问题，因为它将噪声加入到相对姿态中。尽管如此，所有方法均能求解它，但是如下图图（d）所示，g2o解决方案偏离了其它的解决方案。Ceres和GTSAM收敛到最低的目标函数值，但是Ceres再次速度更快。

M3500b和M3500c是非常有挑战性的问题，因为其噪声量很高。GTSAM在这两种情况下没有收敛，并且Ceres和g2o陷入局部极小值。SE-Sync是求解这个问题两种变体的最为成功的方法。考虑到缺少连接位姿图左边部分和右边部分的回环约束以及测量中的噪声量，SE-Sync已经实现相当好的解决方案。这些位姿图如下图图（a）和下图图（b）所示。

4）Sphere-a：Sphere-a问题也具有挑战性，因为大量噪声被加入到相对姿态测量数据中。即使给出一个初始估计值，仅SE-Sync能够求解它，它仅在0.15s内收敛到全局最优，比g2o速度快50倍并且比Ceres和GTSAM速度快100倍。由SE-Sync获得的最优解如上图图（c）所示，而其它三种方法获得局部最优解。

5）Cubicle和Rim：Cubicle数据集是Rim数据集的一个子集，所以本文一起讨论它们。这两个数据集具有挑战性，因为它们都包含大量的节点和边。此外，大量的边为错误的回环。仅GTSAM和SE-Sync在这种情况下收敛到一个解。优化后的位姿图如上图图（d）和上图图（e）所示。Ceres和g2o无法找到一个解。GTSAM求解Cubicle的速度比SE-Sync快两倍，但是SE-Sync收敛到全局最优。SE-Sync优化Rim的速度比GTSAM快五倍，并且收敛到全局最优。

6）Torus：Torus是最容易的3D问题之一。除了g2o之外的所有方法均收敛到全局最优，但是SE-Sync再次在速度方面获胜。虽然g2o解决了该问题，但是由下图图（a）所示，可以看出它优化后的轨迹与其它三种方法相比略有漂移。

7）Cube：Cube数据集更为复杂并且耗时，这是由于它包含大量的节点和边，但是所有方法均找到一个解。SE-Sync是迄今为止求解该问题速度最快的方法，它耗费8秒，而其它的方法耗费超过一分钟。最终的解如下图图（b）所示。

8）Garage：Garage数据集是最小且最容易求解的3D数据集，每种方法均能获得一个解，如下图图（c）所示。Ceres、GTSAM和SE-Sync收敛到相同的目标函数值，GTSAM在这种情况下速度最快。

总结

在本文中，我们比较用于SLAM中位姿估计的图优化方法。我们考虑g2o和GTSAM（它们是当前最先进的方法）、一个用户友好的开源框架Ceres和一个用于位姿同步的新型且鲁棒的方法SE-Sync。评估过程考虑了耗费的优化时间和目标函数值，12个基准数据集的结果以表格的形式给出。

当与其它三种方法比较时，SE-Sync在大多数数据集上取得最少的总耗时。如果需要，它还能够验证全局最优，但是以额外的计算时间为代价。g2o具有最高的总耗时，但是在简单的2D数据集上表现良好。Ceres易于使用，提供很多的灵活性，并且速度相对较快。GTSAM表现与SE-Sync几乎一样好，除了在噪声非常多的2D数据集上。搭配一个正确的前端，这些方法在解决SLAM问题中能够非常强大。对于较差的数据关联、高噪声和一个表现较差的前端，似乎最好使用SE-Sync作为后端。GTSAM具有好的初始化方法，因此它似乎也表现相当不错。如果前端非常出色、数据集相对简单或者噪声较低，那么在这些后端中做出选择是个人偏好的问题。我们希望这种比较能够帮助其他研究者为他们的SLAM应用选择一种后端方法。

审核编辑：郭婷