# 论文阅读 A Comparison of Graph Optimization Approaches for Pose Estimation in SLAM

论文阅读 A Comparison of Graph Optimization Approaches for Pose Estimation in SLAM

Abstract—Simultaneous localization and mapping (SLAM) is an important tool that enables autonomous navigation of mobile robots through unknown environments. As the name SLAM suggests, it is important to obtain a correct representation of the environment and estimate a correct trajectory of the robot poses in the map. Dominant state-of-the-art approaches solve the pose estimation problem using graph optimization techniques based on the least squares minimization method. Among the most popular approaches are libraries such as g2o, Ceres, GTSAM and SE-Sync. The aim of this paper is to describe these approaches in a unified manner and to evaluate them on an array of publicly available synthetic and real-world pose graph datasets. In the evaluation experiments, the computation time and the value of the objective function of the four optimization libraries are analyzed.
Index Terms—pose-graph, optimization, trajectory estimation, SLAM, g2o, GTSAM, Ceres, SE-Sync I.

Abstract

同步定位与映射(SLAM)是实现移动机器人在未知环境中自主导航的重要工具。SLAM顾名思义，在地图上获得正确的环境表示和估计机器人姿态的正确轨迹是很重要的。最先进的方法是利用基于最小二乘最小化方法的图优化技术来解决位姿估计问题。其中最流行的方法是库，如**g2o、Ceres、GTSAM和SE-Sync。**本文的目的是用一种统一的方式描述这些方法，并在一组公开可用的合成和真实世界姿态图数据集上评估它们。在评价实验中，分析了四种优化库的计算时间和目标函数值。姿态图，优化，轨迹估计，SLAM, g2o, GTSAM, Ceres, SE-Sync I。

I. INTRODUCTION

经过多年在 SLAM 场景中的主导地位，基于滤波的方法越来越多地被基于优化的方法所取代。位姿图优化 (PGO) 最初是在 [1] 中引入的，但由于计算效率低下而不太流行。今天，随着计算能力的增加，PGO 方法已经成为最先进的方法，能够快速准确地解决 SLAM 优化和估计问题。
基于优化的 SLAM 方法通常由两部分组成。第一部分使用基于传感器数据的对应关系确定新观测和地图之间的约束。第二部分计算机器人位姿和给定约束的地图。 可分为图法和平滑法。当前最先进的基于优化的方法的一个例子是 g2o [2]。它是非线性最小二乘问题的通用优化框架。[3] 中介绍了第一种平滑方法 $\sqrt{SAM}$。[4] 中介绍了对这种方法的改进，即增量平滑和建图 (iSAM)。iSAM 扩展了 $\sqrt{SAM}$，通过更新稀疏平滑信息矩阵的分解，为全 SLAM 问题提供有效的解决方案。iSAM 的升级，iSAM2在[5] 中介绍。这些平滑方法在 GTSAM [6] 中实现，这是另一个最先进的优化库。

SLAM 问题的一个经典示例是所谓的位姿 SLAM，它避免了构建环境的显式地图。位姿 SLAM 的目标是在给定闭环和测距约束的情况下估计机器人的轨迹。这些相对位姿测量通常是使用自运动估计、扫描匹配、迭代最近点 (ICP) 或某种形式的最小化视觉重投影误差从 IMU、激光传感器、相机或车轮里程计获得的。值得注意的是，[7] 和 [8] 中的方法提出了基于滤波器的位姿 SLAM 算法，但我们在当前论文中的重点是实现基于优化的位姿 SLAM 的方法。

其中最流行的方法是 g2o [2]、Ceres Solver [9]、GTSAM（“Georgia Tech Smoothing and Mapping”）[6] 和 SESync（“Synchronization over Special Euclidean group SE( n)") [10]。文献中很少有比较这些方法的作品。例如，在 [11] 中，作者提供了视觉 SLAM 的概述，并将 g2o、GTSAM 和 HOG-Man [12] 作为后端进行了比较。
作者在[13]中讨论了旋转估计在位姿图估计中的重要性，并在基准数据集上比较了g2o和GTSAM的不同旋转估计技术。在[14]中，作者比较了g2o框架下的不同优化算法。在[15]中提出了一个统一的SLAM框架GLSLAM，它提供了多种SLAM算法实现，还可以对不同的SLAM方法进行基准测试。但据我们所知，目前还没有将g2o、Ceres、GTSAM和SE-Sync进行统一的比较。本文的目的是描述这些方法以统一的方式,评估他们的数组公开合成数据集和真实的图(图1中可视化)。在未来,我们想用这个比较方便PGO的选择方法。

本文组织如下。第二节介绍了非线性图优化的一般情况和这四种方法的每一种。实验是本文的主要部分，在第三节中进行了描述。在本节的第一部分中，描述了硬件、实验设置和基准测试数据集。最后对实验结果进行了讨论。最后，第四部分对全文进行了总结。

II. NONLINEAR POSE-GRAPH OPTIMIZATION APPROACHES

图2:SLAM问题的位姿图表示。(a)每个节点对应一个机器人姿态。相邻的位姿用边连接。该边缘模型模拟了两个位姿之间的空间约束。相邻位姿之间的边表示里程计，其余边表示对环境相同部分的重复观察(循环闭包)。(b)每条边定义其误差函数 $e_{ij}$ 和信息矩阵 $\Omega ij$。误差函数是实际测量值 $z_{ij}$ 与近似约束 ${\hat{z}}_{ij}$ 之间的差值。

每个位姿图由节点和边组成。位姿图中的节点对应于机器人在环境中的位姿，边缘表示节点之间的空间约束。相邻节点之间的边为里程约束，其余边为循环闭合约束。如图2a所示。位姿图优化的目标是找到在位姿图中所有约束条件下最小二乘误差的节点构型。一般来说，非线性最小二乘优化问题可以定义为:
$${\mathbf{x}}^{\ast }=\underset{\mathbf{x}}{\mathrm{argmin}}\mathbf{F}(\mathbf{x})$$
其中 $F(x)$ 为图中所有约束条件的误差之和:
$$\mathbf{F}(\mathbf{x})=\sum _{⟨i,j⟩\in \mathcal{C}}{\mathbf{e}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{e}}_{ij}$$
其中，$C$ 表示连通节点之间的指标对集合，${\Omega }_{ij}$ 表示节点 $i$ 和 $j$ 之间的信息矩阵，$e_{ij}$ 是建模位姿 $x_i$ 和 $x_j$ 满足测量 $z_{ij} 所施加约束的程度的非线性误差函数。最后,每个约束用信息矩阵 $\Omega ij$ 和误差函数 $e_{ij}$ 建模。如图2b所示。
传统上，式(1) 的解是通过它的优化技术(例如高斯-牛顿或利文伯格-马夸特)获得的。他们的想法是用误差函数的一阶泰勒展开来逼近当前的初始猜想。一般来说，它们包含4个主要步骤:
1)修正最初的猜测。
2)将问题近似为一个凸问题。
3)解出2)并将其设为一个新的初始猜想。
4)重复2)直到收敛。
姿态SLAM更容易解决，因为它不需要构建环境地图。图论问题具有稀疏结构，计算速度快。另一个优点是，它对错误的初始猜测是稳健的。位姿SLAM的缺点是对异常值一般不鲁棒，且在存在大量假环闭包时不收敛。此外，旋转估计使其成为一个难以解决的非凸优化问题，因此凸松弛会导致局部最小值问题，且不能保证全局最优。在本节中，我们将简要描述优化框架。

A. G2O

G2o[2]是一个开源的通用框架，用于优化可定义为图的非线性函数。它的优点是易于扩展、高效和适用于广泛的问题。作者在[2]中表示，他们的系统可以与其他最先进的算法相媲美，同时具有高度的通用性和可扩展性。它们利用稀疏连通性和图的特殊结构，利用先进的方法求解稀疏线性系统，并利用现代处理器的特点来实现效率。该框架包含三种求解PGO的方法:Gauss- Newton, Levenberg-Marquardt and Powell’s Dogleg.。它主要用于解决机器人技术中的SLAM问题和计算机视觉中的束调整问题。ORB-SLAM([16]，[17])使用g2o作为摄像机姿态优化的后端，SVO[18]使用它进行视觉里程测量。

B. Ceres

Ceres Solver[9]是一个开源c++库，用于建模和解决大型、复杂的优化问题。它主要致力于解决非线性最小二乘问题(束平差和SLAM)，但也可以解决一般的无约束优化问题。该框架易于使用、可移植，并经过广泛优化，以低计算时间提供高质量的解决方案。Ceres的设计允许用户定义和修改目标函数和优化求解器。所实现的求解器包括信赖域求解器(Levenberg-Marquardt, Powell s Dogleg)和直线搜索求解器。由于它有许多优点，Ceres被用于许多不同的应用和领域。OKVIS([19]，[20])和VINS[21]使用Ceres来优化定义为图的非线性问题。

C. GTSAM

GTSAM[6]是另一个开源c++库，它实现了机器人和计算机视觉应用的传感器融合。它可用于解决SLAM、视觉里程计和结构自运动(SfM)中的优化问题。GTSAM使用因子图[22]来建模复杂的估计问题，并利用它们的稀疏性来提高计算效率。它实现了Levenberg- Marquardt和Gauss-Newton风格的优化器，共轭梯度优化器，Dogleg和iSAM:在平滑和映射。GTSAM被用于学术界和工业界的各种传感器前端。例如，有一个SVO[23]的变体，它使用GTSAM作为视觉里程计的后端。

D. SE-Sync

SE-Sync[10]是一个在特殊欧几里德群上执行同步的公认正确算法。它的目标是估计一组未知的姿态(在欧几里德空间中的位置和方向)的值，给出了节点之间相对变换的噪声测量。它们主要应用于二维和三维几何估计。例如，姿势图SLAM(机器人技术)、摄像机运动估计(计算机视觉)和传感器网络定位(分布式传感)。作者在[10]中指出，SE- Sync通过利用特殊欧氏同步问题的一种新颖的(凸)半定松弛来直接搜索全局最优解，并能够为所找到的解生成一个计算正确性的证书，从而改进了以前的方法。他们使用truncated-Newton Riemannian Trust-Region method [24]来寻找姿态的有效估计。

III. EXPERIMENTS

我们的目标是实验性地评估第二节中描述的优化框架，并对它们进行比较。为此，我们考虑它们在总计算时间和(2)所描述的目标函数的最终值方面的性能。我们使用公开的合成和真实的基准测试数据集。

A. Experimental setup

实验是在一台联想ThinkPad上进行的P50搭载英特尔酷睿i7-6700HQ八核CPU，运行频率为2.60 GHz，内存为16gb。说明计算机运行的是Ubuntu 20.04。g2o、Ceres和GTSAM框架使用相同的求解器Levenberg-Marquardt，而SE - sync使用Rieman- nian信赖域(RTR)方法([24])。每个算法最多可迭代100次。停止准则以达到最大迭代次数和相对误差减小为基础。SE-Sync使用稍微不同的方法，因此必须指定一个基于黎曼梯度范数的附加准则。相对误差减小的容忍度设置为$10^{-5}$，梯度范数设置为$10^{-2}$。参数是根据[10]中的建议选择的。在[13]中研究了定位初始化对全局最优解的影响。受此工作的启发，我们也做了位姿图的初始化之前的优化过程。为了取得更好的结果，我们使用生成树方法[2]来获取初始位姿图。

B. Benchmarking datasets

从[25]中得到6个二维位姿图，2个实字图，4个仿真图。INTEL and MIT pose（英特尔和麻省理工学院的姿势图）是真实世界的数据集，通过处理原始车轮里程计和激光距离传感器的测量数据，收集于西雅图的英特尔研究实验室和麻省理工学院基利安法庭。M3500位姿图是模拟曼哈顿世界[26]。M3500a、M3500b和M3500c数据集是M3500数据集的变体，在相对方向测量中添加了高斯噪声。噪声的标准差分别为0.1、0.2、0.3 rad。此外，还从[13]获得了6个三维数据集。在仿真中创建了姿态图Sphere-a、Torus和Cube。Sphere-a数据集是[27]中发布的一个具有挑战性的问题。其他三个姿势图是真实世界的数据集。在[28]中引入Garage数据集，使用来自乔治亚理工大学Rim中心的三维激光传感器对点云的ICP获取Cubicle和Rim。所有这些位姿图都显示在图1中，并带有测程约束和闭环约束。表I包含每个数据集的节点和边的数量。这些数字决定了优化过程中参数的数量和问题的复杂性。
在这里插入图片描述

C. Results

我们用总计算时间和目标函数(2)的值来总结表II中所有的性能结果。对于每个数据集，我们都说明了算法的终止原因。如果算法在最大迭代极限内完成优化，则算法收敛。我们也给出了SE-Sync的验证时间来证明全局最优。图3、4和5显示了针对基准问题的优化姿态图。


英特尔:英特尔是最容易解决的问题之一。各种方法都成功地解决了这一问题。轨迹如图3a所示，可以看出它们都得到了相似的结果。GTSAM完成优化的时间最长，但实现的目标函数值最低。SE-Sync在本例中是最快的，其值略大于GTSAM。考虑到这一点，SE -Sync似乎是最好的解决方案。
2) MIT: MIT是最小的问题，但只有几个闭环约束。因此，从一个良好的初始猜测开始优化是很重要的。否则，GTSAM、Ceres和g2o将无法通过Levenberg-Marquardt算法收敛到有意义解。所有算法在小于最大迭代次数的情况下收敛，并且实现了几乎相同的目标函数值。优化问题不到半秒就解决了，但Ceres和g2o是最快的。Ceres也实现了最低的目标函数值，似乎是MIT数据集的最佳求解器。最终的轨迹如图3b所示。
3) M3500:这里同时展示了M3500数据集的所有四种变体。GTSAM和Ceres最擅长解决基本的M3500问题，因为它们都达到了最低的目标函数值，但Ceres略快一些。所有方法都成功地解决了该问题，其优化后的位姿图如图3c所示。M3500a是一个更困难的问题，因为噪声添加到相对方向。尽管如此，所有的方法都解决了它，但是从图3d中可以看出，g2o解决方案与其他解决方案有所不同。Ceres和GTSAM趋同到最低值，但ceres还是更快。M3500b和M3500c是非常具有挑战性的问题，因为噪音量很高。在这两种情况下，GTSAM不收敛，而谷神星和g2o陷入局部极小值。SE-Sync是解决这两种问题的最成功的方法。考虑到缺少连接姿态图左右部分的闭环约束，以及测量中的噪声量，SE-Sync已经实现了一个相当好的解决方案。这些位姿图可以在图4a和图4b中看到。
4)Sphere-a:球体也是一个具有挑战性的问题，因为在相对方向测量中添加了大量的噪声。即使有最初的猜测，也只有SE-Sync能够解决它。它在0.15 s内收敛到全球最小值，比g2o快50倍，比Ceres和GTSAM快100倍。通过SE-Sync得到的最优解如图4c所示，其中包含了其他三种方法得到的局部最优解。
5)环面:环面是最简单的3D问题之一。除了g2o之外的所有方法都收敛到全局最优，但SE-Sync在速度上再次胜出。g2o解决了这个问题，但从图5a中可以看出，与其他三种气体相比，它的优化轨迹略有偏移。
6) Cube: Cube数据集由于节点和边的数量较多，比较复杂和耗时，但所有方法都找到了解决方案。SE-Sync是目前为止解决这个问题最快的。它需要8秒，而其他的需要超过一分钟。最终的解决方案如图5b所示。

7) Garage: Garage数据集是最小的、最容易解决的3D数据集，每种方法都有一个解决方案。这如图5c所示。Ceres、GTSAM和SE-Sync收敛到相同的目标值，在这种情况下，GTSAM是最快的。
8) Cubicle和Rim: Cubicle数据集是Rim数据集的一个子集，所以我们一起讨论它们。这两个数据集具有挑战性，因为它们都包含大量的节点和边。此外，大量的边是假循环闭包。在这两种情况下，只有GTSAM和SE- Sync收敛到一个解决方案。优化后的位姿图如图4d和4e所示。ceres和g2o无法找到解决办法。GTSAM解决隔间问题的速度比SE-Sync快两倍，但SE-Sync收敛到全局最优。SE- Sync优化环比GTSAM快5倍，并收敛到全局最优。

IV. CONCLUSION

本文比较了SLAM中用于位姿估计的图优化方法。我们考虑了g2o和GTSAM，这是目前最先进的方法，Ceres，一个用户友好的开源框架，SE-Sync，一个新的和健壮的姿态同步方法。评估过程考虑了经过的优化时间和目标函数的值，结果以12个基准测试数据集的表格形式给出。
与其他三种方法相比，SE-Sync在大多数数据集上实现了最小的总时间。如果需要，它还能够验证全局最优性证书，但要以额外的计算时间为代价。g2o具有最高的总时间，但在简单的2D数据集上表现良好。Ceres易于使用，提供了大量的灵活性，并且相对快速。除了在非常嘈杂的2D数据集上，GTSAM的性能几乎和SE-Sync一样好。配合适当的前端，这些方法可以非常有效地解决SLAM问题。对于较差的数据关联、高噪声和性能较差的前端，最好使用SE-Sync作为后端。使用一种良好的初始化方法，GTSAM似乎也能完成同样好的工作。如果前端是优秀的，数据集相对简单，或者噪声低，这是一个个人喜好的问题来决定这些后端。我们希望这个比较可以帮助其他研究人员为他们的SLAM应用选择一个后端方法。