SLAM中姿态估计的图优化方法比较（g2o/Ceres/GTSAM/SE-Sync）

 编辑 | 深蓝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等)可以得到问题最优解。主要想法是用上式在初始值附近的一阶泰勒展开来近似误差函数。一般来说，包括四个主要步骤：

1. 固定一个初始值；

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

3. 对步骤2问题求解，并更新初始值；

4. 重复步骤2-3直到收敛。

Pose-SLAM的优势：求解问题存在稀疏性，可以加快求解速度，并且对于初始状态的变化较为鲁棒。

Pose-SLAM的缺点：对外点(outliers，亲切一点就是错的离谱的结果)不具有鲁棒性，另外，优化估计Rotation的过程是一个非凸优化问题，容易造成导致局部最优问题，且不保证全局最优问题。

◐

4.优化框架介绍

g2o

g2o是一个开源的通用框架，用于优化可以定义为图形的非线性函数，它的优点是易于扩展，高效，适用于广泛的问题。主要特点有：

1. 可以与其他最先进的算法相媲美，同时具有高度的通用性和可扩展性；

2. 通过利用稀疏连通性、图的特殊结构以及现代处理器的特点来实现高效计算；

3. 该框架包含了3种不同的PGO算法：GaussNewton 、Levenberg-Marquardt、Powell’s Dogleg。

应用：ORB-SLAM的后端处理、SVO的visual odometry部分

Ceres

Ceres Solver是一个开源的c++库，用于建模和解决大型、复杂的优化问题。它主要致力于解决非线性最小二乘问题(束平差和SLAM)，但也可以解决一般的无约束优化问题。

1. 该框架易于使用、可移植、广泛优化、低计算时间；

2. Ceres的设计允许用户定义和修改目标函数和优化求解器；

3. 实现的求解算法包括trust region solvers(Levenberg-Marquardt、Powell’s Dogleg)和line search solvers。

应用：OKVIS、VINS中的图优化问题

GTSAM

GTSAM是一个开源c++库，它实现了机器人和计算机视觉应用的传感器融合。它可用于SLAM、视觉测程和SFM中的优化问题。

1. GTSAM使用因子图来模拟复杂的估计问题，并利用其稀疏性来提高计算效率；

2. 实现了Levenberg- Marquardt、Gauss-Newton的优化器。

应用：GTSAM与各种传感器前端一起使用，SVO的变体使用GTSAM作为里程计的后端。

SE-Sync

SE-Sync的目标给定节点之间的相对变换的噪声测量的条件下，估计一组未知的姿态的值

1. 主要应用于二维和三维几何估计；

2. 改进了以往的方法，它利用了特殊的欧氏同步问题的一个新的(凸)半定松弛来直接搜索全局最优解；

3. 采用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方法汇总 | 自动驾驶和移动机器人领域