编辑 | 深蓝AI
点击下方卡片，关注“自动驾驶之心”公众号
ADAS巨卷干货，即可获取
后台回复【SLAM综述】获取视觉SLAM、激光SLAM、RGBD-SLAM等多篇综述！
本文是对论文《A Comparison of Graph Optimization Approaches for Pose Estimation in SLAM》的带读~
◐
1.摘要
对于位置环境中的自主导航问题，同步定位与建图(Simultaneous localization and mapping, SLAM)是一个非常重要的工具框架。根据SLAM字面含义可以得知，获取正确的环境表征和估计机器人正确的运动轨迹这两点在SLAM问题中至关重要。在解决优化估计问题上表现SOTA方法的主流思路是基于图优化的最小二乘，比较流行的算法框架有g2o、Ceres、GTSAM、SE-Sync等。作者这篇论文描述了这些方法并进行了对比测试。
◐
2.总体介绍
在SLAM领域刚刚起步的时候，基于滤波算法的SLAM霸占了很多年，因为PGO(Pose graph Optimization)存在处理效率的问题，而SLAM对实时性要求又比较高。到了现如今，由于计算机处理效率逐渐提高，PGO方法被提上了日程，越来越多高效的优化算法使得基于PGO的优化SLAM占据主流地位。
基于优化的SLAM方法主要分为两个部分：
· 第一部分就是根据传感数据的测量数据，建立新的观测与地图之间的约束(就是损失函数) ；
· 第二部分就是根据约束处理优化机器人位姿和地图信息。
以典型基于优化的SLAM问题来说：Pose-SLAM，它避免了建立一个明确的环境地图，目标是在给定闭环和里程约束条件下估计机器人的轨迹(相对姿态)。这些相对姿态测量通常通过IMU、lidar、camera或GPS获得，使用ego-motion、scan-registration、ICP等构建最小化视觉重投影误差(就是损失函数)。利用最流行的优化框架g2o、Ceres、GTSAM、SE- Sync等进行求解。但是没有论文在同一条件下对这些框架算法进行评估，本文的目的就是这个，在相同条件下，测试不同框架对不同问题的性能效果。
◐
3.非线性位姿图优化方法
在Pose-SLAM的优化问题中，每个姿态图(pose graph)都由节点(node)和边(edge)组成，其中：
节点(node)：对应于机器人在环境中的位姿，也是优化的目标；
边(edge)：不同节点之间的空间约束。相邻节点之间的边是里程约束，其余边表示闭环约束。
下图展示了一个pose-graph可视化的结果，由于物体的运动在时间上连续，可以看到
代表一些系列的位姿。
实线连接：时间相邻位姿，用于局部里程计(odometry)优化 ；
虚线连接：存在共视关系，即有些特征点在不同位姿下同时观察到，用于闭环(loop-closures)优化。
下面简述一个非线性位姿图优化的基本思路步骤：
确定优化目标：在位姿图的所有约束条件下找到最小二乘误差的正确节点配置。一般情况下，可以定义为
。
其中， F(x)是所有图约束对应误差总和：
其中，C 表示连接节点之间的索引对集合，i 和 j 表示节点 i 和j 之间的信息矩阵，
是非线性误差函数，用来模拟姿态
和
如何满足测量
施加的约束。采用信息矩阵
和误差函数
对每一个约束进行建模。如下图所示
优化求解：通过迭代优化算(Gauss-Newton、Levenberg-Marquardt等)可以得到问题最优解。主要想法是用上式在初始值附近的一阶泰勒展开来近似误差函数。一般来说，包括四个主要步骤：
固定一个初始值；
将问题近似为凸问题；
对步骤2问题求解，并更新初始值；
重复步骤2-3直到收敛。
Pose-SLAM的优势：求解问题存在稀疏性，可以加快求解速度，并且对于初始状态的变化较为鲁棒。
Pose-SLAM的缺点：对外点(outliers，亲切一点就是错的离谱的结果)不具有鲁棒性，另外，优化估计Rotation的过程是一个非凸优化问题，容易造成导致局部最优问题，且不保证全局最优问题。
◐
4.优化框架介绍
g2o
g2o是一个开源的通用框架，用于优化可以定义为图形的非线性函数，它的优点是易于扩展，高效，适用于广泛的问题。主要特点有：
可以与其他最先进的算法相媲美，同时具有高度的通用性和可扩展性；
通过利用稀疏连通性、图的特殊结构以及现代处理器的特点来实现高效计算；
该框架包含了3种不同的PGO算法：GaussNewton 、Levenberg-Marquardt、Powell’s Dogleg。
应用：ORB-SLAM的后端处理、SVO的visual odometry部分
Ceres
Ceres Solver是一个开源的c++库，用于建模和解决大型、复杂的优化问题。它主要致力于解决非线性最小二乘问题(束平差和SLAM)，但也可以解决一般的无约束优化问题。
该框架易于使用、可移植、广泛优化、低计算时间；
Ceres的设计允许用户定义和修改目标函数和优化求解器；
实现的求解算法包括trust region solvers(Levenberg-Marquardt、Powell’s Dogleg)和line search solvers。
应用：OKVIS、VINS中的图优化问题
GTSAM
GTSAM是一个开源c++库，它实现了机器人和计算机视觉应用的传感器融合。它可用于SLAM、视觉测程和SFM中的优化问题。
GTSAM使用因子图来模拟复杂的估计问题，并利用其稀疏性来提高计算效率；
实现了Levenberg- Marquardt、Gauss-Newton的优化器。
应用：GTSAM与各种传感器前端一起使用，SVO的变体使用GTSAM作为里程计的后端。
SE-Sync
SE-Sync的目标给定节点之间的相对变换的噪声测量的条件下，估计一组未知的姿态的值
主要应用于二维和三维几何估计；
改进了以往的方法，它利用了特殊的欧氏同步问题的一个新的(凸)半定松弛来直接搜索全局最优解；
采用truncated-Newton Riemannian Trust-Region method来寻找有效的姿态估计。
◐
5.实验测试
实验标准：
评价标准：运行时间、损失结果、终止条件
评价数据集：合成数据集、真实数据集
实验环境：
Lenovo ThinkPad P50 equipped with an octa-core Intel Core i7-6700HQ CPU operating at 2.60 GHz and 16 GB RAM
Ubuntu20.04
优化配置：
算法：Levenberg- Marquardt；
迭代次数：最多100次；
停止迭代标准：达到最大迭代次数或相对误差阈值；
梯度下降率：0.01 。
测试结果：
部分优化结果(在INTEL、MIT两个数据集上)
整体测试统计结果
◐
6.结论
本文比较了在SLAM中用于姿态估计的图优化算法。作者考虑了g2o、GTSAM、Ceres、SE-Sync四个主流优化框架。评估过程考虑了优化时间和优化结果，以表的形式给出了测试结果，发现：
与其他三种方法相比，SE-Sync在大多数数据集上优化时间最短；
g2o优化时间最长，但在简单数据集上表现良好；
Ceres易于使用，提供了很大的灵活性，而且相对较快；
除了在噪声干扰严重的数据集上，GTSAM的性能几乎和SE-Sync一样 。
并且作者建议：
对于数据关联差、噪声大和性能差的前端，最好使用SE-Sync作为后端。如果初始化较好，GTSAM表现和SE-Sync一样出色 。
对于比性能优秀的前端，在数据集相对简单、噪音很低的情况下，后端优化的选择就凭借个人喜好了。
往期回顾
激光雷达SLAM方法汇总 | 自动驾驶和移动机器人领域
【自动驾驶之心】全栈技术交流群
自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、规划控制、模型部署落地、自动驾驶仿真测试、硬件配置、AI求职交流等方向；
加入我们：自动驾驶之心技术交流群汇总！
自动驾驶之心【知识星球】
想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球（三天内无条件退款），日常分享论文+代码，这里汇聚行业和学术界大佬，前沿技术方向尽在掌握中，期待交流！
