|
|
Date：2021-11-16
作者；chaochaoSEU
文章末尾有【激光SLAM、视觉SLAM】交流群加入方式！
3D视觉精品课程推荐：
1.面向自动驾驶领域的多传感器数据融合技术
2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)
9.从零搭建一套结构光3D重建系统[理论+源码+实践]
10.单目深度估计方法：算法梳理与代码实现
11.相机模型与标定（单目+双目+鱼眼相机） A Comparison of Graph Optimization Approaches for Pose Estimation in SLAM
作者：Andela Juric, Filip Kende, Ivan Markovic, Ivan Petrovic
论文地址：chrome-extension://ikhdkkncnoglghljlkmcimlnlhkeamad/pdf-viewer/web/viewer.html?file=https%3A%2F%2Flamor.fer.hr%2Fimages%2F50036607%2F2021-ajuric-comparison-mipro.pdf#=&zoom=80
典型基于优化的SLAM问题来说：Pose-SLAM的目标是在给定闭环和里程约束条件下估计机器人的轨迹(相对姿态)。这些相对姿态测量通常通过IMU、lidar、camera或GNSS获得，使用ego-motion、scan-registration、ICP等构建损失函数。利用最流行的优化框架g2o、Ceres、GTSAM、SE- Sync等进行求解。但是目前没有论文在同一条件下对这些框架算法进行评估，本文的就是做在相同条件下，测试不同框架对不同问题的性能效果。
摘要： SLAM是一种重要的工具，它使移动机器人能够在未知环境中自主导航。正如名称 SLAM 所暗示的，重要的是获得环境的正确表示并估计地图中机器人位姿的正确轨迹。主流的最新方法使用基于最小二乘最小化方法的图优化技术来解决位姿估计问题。其中最流行的方法是 g2o、Ceres、GTSAM 和 SE-Sync 等库。本文的目的是以统一的方式描述这些方法，并在一系列公开可用的合成和现实世界位姿图数据集上对其进行评估。在评估实验中，分析了四个优化库的计算时间和目标函数值。
I 引言
经过多年在 SLAM 场景中的主导地位，基于滤波的方法越来越多地被基于优化的方法所取代。位姿图优化 (PGO) 最初是在 [1] 中引入的，但由于计算效率低下而不太流行。今天，随着计算能力的增加，PGO 方法已经成为最先进的方法，能够快速准确地解决 SLAM 优化和估计问题。
基于优化的 SLAM 方法通常由两部分组成。第一部分使用基于传感器数据的对应关系确定新观测和地图之间的约束。第二部分计算机器人位姿和给定约束的地图。可分为图法和平滑法。当前最先进的基于优化的方法的一个例子是 g2o [2]。它是非线性最小二乘问题的通用优化框架。[3] 中介绍了第一种平滑方法 pSAM。[4] 中介绍了对这种方法的改进，即增量平滑和建图 (iSAM)。iSAM 扩展了 pSAM，通过更新稀疏平滑信息矩阵的分解，为全 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方法的选择。
本文的结构如下。Section II描述了一般的非线性图优化以及这四种方法中的每一种。实验是本文的主要部分，在Section III中进行了描述。在本节的第一部分，描述了硬件、实验设置和基准数据集。实验结果在本节末尾讨论。最后，Section IV总结了论文。
II.非线性位姿图优化方法
每个位姿图都由节点和边组成。位姿图中的节点对应机器人在环境中的位姿，边代表它们之间的空间约束。相邻节点之间的边是里程约束，其余边表示闭环约束。这在图 2a 中可视化。位姿图优化的目标是找到一种节点配置，使位姿图中所有约束的最小二乘误差最小。一般来说，非线性最小二乘优化问题可以定义如下：
传统上，（1）的解决方案是通过迭代优化技术（例如，G-N或莱L-M）获得的。他们的想法是用围绕当前初始猜测的一阶泰勒展开来近似误差函数。一般来说，它们包括四个主要步骤：
1) 修正初始猜测。
2）将问题近似为凸问题。
3）求解2）并将其设置为新的初始猜测。
4）重复2）直到收敛。
位姿 SLAM 更容易解决，因为它不构建环境地图。图公式化问题具有稀疏结构，因此计算速度更快。另一个优点是它对糟糕的初始猜测具有鲁棒性。位姿SLAM的缺点是一般对异常值不强健，当有很多假闭环时不收敛。此外，旋转估计使它成为一个困难的非凸优化问题，因此凸松弛会导致局部最小值问题，并且无法保证全局最优。在本节中，我们将简要描述基于非线性最小二乘法的优化框架，这些框架以位姿图的形式提供解决方案。
A.g2o
g2o [2] 是一个开源通用框架，用于优化可以定义为图的非线性函数。它的优点是易于扩展、高效且适用于广泛的问题。作者在 [2] 中指出，他们的系统可与其他最先进的算法相媲美，同时具有高度的通用性和可扩展性。他们通过利用稀疏连接和图的特殊结构、使用先进的方法来解决稀疏线性系统以及利用现代处理器的特性来提高效率。该框架包含三种不同的方法来解决 PGO、GaussNewton、Levenberg-Marquardt 和 Powell’s Dogleg。主要用于解决机器人中的SLAM问题和计算机视觉中的BA问题。ORB-SLAM ([16][17]) 使用 g2o 作为后端进行相机位姿优化，SVO [18] 将其用于视觉里程计。
B. Ceres
Ceres Solver [9] 是一个开源 C++ 库，用于建模和解决大型复杂优化问题。它主要致力于解决非线性最小二乘问题（BA和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] 是一种可证明正确的算法，用于在特殊欧几里得群上执行同步。它的目标是在给定节点之间相对变换的噪声测量值的情况下估计一组未知位姿（欧几里德空间中的位置和方向）的值。它们的主要应用是在 2D 和 3D 几何估计的背景下。例如，位姿图 SLAM（在机器人技术中）、相机运动估计（在计算机视觉中）和传感器网络定位（在分布式传感中）。作者在 [10] 中指出，SE-Sync 通过利用特殊欧几里得同步问题的新颖（凸）半定松弛直接搜索全局最优解来改进以前的方法，并且能够为找到的解生成正确性的计算证明。他们应用截断牛顿黎曼信任区域方法 [24] 来找到有效的位姿估计。
III. 实验
我们的目标是通过实验评估第二部分中描述的优化框架并比较它们。为此，我们正在考虑它们在总计算时间和（2）描述的目标函数的最终值方面的性能。我们使用公开可用的合成和现实世界基准数据集。
A. 实验设置
实验是在配备八核 Intel Core i7-6700HQ CPU、运行频率为 2.60 GHz 和 16 GB RAM 的联想 ThinkPad P50 上进行的。计算机正在运行 Ubuntu 20.04。为 g2o、Ceres 和 GTSAM 框架选择了相同的求解器 Levenberg-Marquardt，而 SE-Sync 使用黎曼信任域 (RTR) 方法 ([24])。每个算法最多只能进行 100 次迭代。停止标准基于达到最大迭代次数和相对误差减少。SE-Sync 使用稍微不同的方法，因此必须指定基于黎曼梯度范数的附加标准。相对误差减少的容差设置为 10-5，梯度的范数设置为 10-2。根据[10]中的建议选择参数。[13] 中的作者研究了方向初始化对寻找全局最优值的影响。受这项工作的启发，我们还在优化过程之前进行了位姿图初始化。为了获得更好的结果，我们使用生成树方法获得初始位姿图[2]。
B. 基准数据集
有从[25]获得的六个二维位姿图、两个真实词图和四个在模拟中创建的图。INTEL 和 MIT 位姿图是真实世界的数据集，通过处理在西雅图英特尔研究实验室和 MIT Killian Court 收集的原始车轮里程计和激光测距传感器测量值而创建。M3500 位姿图是一个模拟的曼哈顿世界 [26]。M3500a、M3500b 和 M3500c 数据集是 M3500 数据集的变体，在相对方向测量中添加了高斯噪声。噪声的标准偏差分别为 0.1、0.2 和 0.3 rad。还有从[13]获得的六个三维数据集。位姿图 Sphere-a、Torus 和 Cube 是在模拟中创建的。Sphere-a 数据集是 [27] 中发布的一个具有挑战性的问题。其他三个位姿图是真实世界的数据集。Garage 数据集是在 [28] 中引入的，Cubicle 和 Rim 是使用 ICP 在来自佐治亚理工学院 RIM 中心的 3D 激光传感器的点云上获取的。所有这些位姿图都在图 1 中用它们的里程计和闭环约束进行了可视化。表 I 包含每个数据集的节点数和边数。这些数字决定了优化过程中参数的数量和问题的复杂程度。
C. Results
我们根据总计算时间和目标函数 (2) 的值总结了表 II 中的所有性能结果。对于每个数据集，我们说明算法的终止原因。如果算法在最大迭代限制内完成优化，则算法收敛。我们还为 SE-Sync 提供验证时间以证明全局最优。图 3、4 和 5 显示了基准测试问题的优化位姿图。
表中组织了每个算法和数据集的优化时间和目标值。总时间对应于经过的优化时间，而 F(x) 对应于目标函数的值。给出了每个数据集的终止原因：达到的最大迭代次数（iter）、相对函数减少（conv）、发散（no conv）和黎曼阶梯的最大水平（r max）。SE-Sync 的验证时间是算法检查解决方案最优性所需的时间。最小优化时间标记为红色，最低目标值标记为绿色。蓝色对应于轨迹的视觉表现不佳的其他最小目标值，或者与标记为绿色的值相同。
1) INTEL：INTEL 是最容易解决的问题之一。所有的方法都成功地解决了它。轨迹一起显示在图 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 在这两种情况下都不收敛，Ceres 和 g2o 陷入局部最小值。SE-Sync 是解决这两种问题的最成功方法。考虑到缺乏连接位姿图左右部分的闭环约束，以及测量中的噪声量，SE-Sync 已经实现了相当不错的解决方案。这些位姿图可以在图 4a 和 4b 中看到。
4) Sphere-a：Sphere-a 问题也具有挑战性，因为大量噪声被添加到相对方向测量中。即使有初步的猜测，也只有 SE-Sync 能够解决它。它仅在 0.15 秒内收敛到全局最小值，这比 g2o 快 50 倍，比 Ceres 和 GTSAM 快 100 倍。SE-Sync 的最优解如图 4c 所示，其他三种方法获得的局部最优解。
5) Torus：Torus是最简单的 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 和 SESync 收敛到一个解决方案。优化后的位姿图在图 4d 和 4e 中可视化。Ceres 和 g2o 无法找到解决方案。GTSAM 求解 Cubicle 的速度比 SESync 快两倍，但 SE-Sync 收敛到全局最优。SESync 优化 Rim 的速度比 GTSAM 快五倍，并收敛到全局最优。
IV 总结
在本文中，我们比较了 SLAM 中用于位姿估计的图优化方法。我们考虑了 g2o 和 GTSAM，它们是当前最先进的方法，Ceres，一个用户友好的开源框架，以及 SE-Sync，一种新颖而强大的位姿同步方法。评估过程考虑了经过的优化时间和目标函数的值，结果以十二个基准数据集的表格形式给出。
与其他三种方法相比，SE-Sync 在大多数数据集上实现了最短的总时间。如果需要，它还能够验证全局最优性，但以额外的计算时间为代价。 g2o 的总时间最长，在简单的 2D 数据集上表现良好。Ceres 易于使用，提供了很大的灵活性，而且速度相对较快。 GTSAM 的性能几乎与 SE-Sync 一样好，除了非常嘈杂的 2D 数据集。搭配适当的前端，这些方法在解决 SLAM 问题方面非常强大。对于较差的数据关联、高噪声和性能不佳的前端，似乎最好使用 SE-Sync 作为后端。凭借良好的初始化方法，GTSAM 似乎也做得同样出色。如果前端优秀，数据集相对简单，或者噪音低，在这些后端之间决定是个人喜好的问题。我们希望这种比较可以帮助其他研究人员为其 SLAM 应用程序选择后端方法。
更多干货
欢迎加入【3D视觉工坊】交流群，方向涉及3D视觉、计算机视觉、深度学习、vSLAM、激光SLAM、立体视觉、自动驾驶、点云处理、三维重建、多视图几何、结构光、多传感器融合、VR/AR、学术交流、求职交流等。工坊致力于干货输出，为3D领域贡献自己的力量！欢迎大家一起交流成长~
添加小助手微信：CV_LAB，备注学校/公司+姓名+研究方向即可加入工坊一起学习进步。 |
本帖子中包含更多资源
您需要 登录 才可以下载或查看，没有账号？立即注册
×
|
窥视卡
雷达卡
发表于 2021-11-20 07:46
提升卡
置顶卡
沉默卡
喧嚣卡
变色卡
千斤顶
显身卡