论文精读 | slam中姿态估计的图优化方法比较

一、摘要

对于位置环境中的自主导航问题，同步定位与建图(Simultaneous localization and mapping, SLAM)是一个非常重要的工具框架。根据SLAM字面含义可以得知，获取正确的环境表征和估计机器人正确的运动轨迹这两点在SLAM问题中至关重要。在解决优化估计问题上表现SOTA方法的主流思路是基于图优化的最小二乘，比较流行的算法框架有g2o、Ceres、GTSAM、SE-Sync等。作者这篇论文描述了这些方法并进行了对比测试。

二、总体介绍

在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等进行求解。但是没有论文在同一条件下对这些框架算法进行评估，本文的目的就是这个，在相同条件下，测试不同框架对不同问题的性能效果。

三、非线性位姿图优化方法

在Pose-SLAM的优化问题中，每个姿态图(pose graph)都由节点(node)和边(edge)组成，其中：

节点(node)：对应于机器人在环境中的位姿，也是优化的目标；
边(edge)：不同节点之间的空间约束。相邻节点之间的边是里程约束，其余边表示闭环约束。

下图展示了一个pose-graph可视化的结果，由于物体的运动在时间上连续，可以看到 $\left \{ X_{1},X_{2},...,X_{t} \right \}$ 代表一些系列的位姿。

实线连接：时间相邻位姿，用于局部里程计(odometry)优化；
虚线连接：存在共视关系，即有些特征点在不同位姿下同时观察到，用于闭环(loop-closures)优化。

下面简述一个非线性位姿图优化的基本思路步骤：

确定优化目标：在位姿图的所有约束条件下找到最小二乘误差的正确节点配置。一般情况下，可以定义为 $x^{*}=\underset{x}{argmin}F\left ( x \right )$
优化求解：通过迭代优化算(Gauss-Newton、Levenberg-Marquardt等)可以得到问题最优解。主要想法是用上式在初始值附近的一阶泰勒展开来近似误差函数。一般来说，包括四个主要步骤：

其中， $F(x)$ 是所有图约束对应误差总和： $F(x)=\underset{(i,j)\in C}{\sum }e\tfrac{T}{ij}\Omega _{ij}e_{ij}$

其中， C表示连接节点之间的索引对集合， $i$ 和 $j$ 表示节点 $i$ 和 $j$ 之间的信息矩阵， $e_{ij}$ 是非线性误差函数，用来模拟姿态 $x_{i}$ 和 $x_{j}$ 如何满足测量 $Z_{ij}$ 施加的约束。采用信息矩阵 $\Omega_{ij}$ 和误差函数 $e_{ij}$ 对每一个约束进行建模。如下图所示

优化求解：通过迭代优化算(Gauss-Newton、Levenberg-Marquardt等)可以得到问题最优解。主要想法是用上式在初始值附近的一阶泰勒展开来近似误差函数。一般来说，包括四个主要步骤：

固定一个初始值；
将问题近似为凸问题；
对步骤2问题求解，并更新初始值；
重复步骤2-3直到收敛。

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

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

四、优化框架介绍

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来寻找有效的姿态估计。

五、实验测试

实验标准：

评价标准：运行时间、损失结果、终止条件
评价数据集：合成数据集、真实数据集

实验环境：

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两个数据集上)

整体测试统计结果

六、结论

本文比较了在SLAM中用于姿态估计的图优化算法。作者考虑了g2o、GTSAM、Ceres、SE-Sync四个主流优化框架。评估过程考虑了优化时间和优化结果，以表的形式给出了测试结果，发现：

与其他三种方法相比，SE-Sync在大多数数据集上优化时间最短；
g2o优化时间最长，但在简单数据集上表现良好；
Ceres易于使用，提供了很大的灵活性，而且相对较快；
除了在噪声干扰严重的数据集上，GTSAM的性能几乎和SE-Sync一样。

并且作者建议：

对于数据关联差、噪声大和性能差的前端，最好使用SE-Sync作为后端。如果初始化较好，GTSAM表现和SE-Sync一样出色。

对于比性能优秀的前端，在数据集相对简单、噪音很低的情况下，后端优化的选择就凭借个人喜好了。

本文是对论文《A Comparison of Graph Optimization Approaches for Pose Estimation in SLAM》的带读~
论文和论文精读文稿PDF版本点击下方就可以获取啦！

https://mp.weixin.qq.com/s/dhDobC7-IyLPar97T2wGBAhttps://mp.weixin.qq.com/s/dhDobC7-IyLPar97T2wGBA