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等)可以得到问题最优解。主要想法是用上式在初始值附近的一阶泰勒展开来近似误差函数。一般来说,包括四个主要步骤:
固定一个初始值;
将问题近似为凸问题;
对步骤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一样出色 。
对于比性能优秀的前端,在数据集相对简单、噪音很低的情况下,后端优化的选择就凭借个人喜好了。
往期回顾
【自动驾驶之心】全栈技术交流群
自动驾驶之心是首个自动驾驶开发者社区,聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、规划控制、模型部署落地、自动驾驶仿真测试、硬件配置、AI求职交流等方向;
加入我们: 自动驾驶之心技术交流群汇总!
自动驾驶之心【知识星球】
想要了解更多自动驾驶感知(分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测)、自动驾驶定位建图(SLAM、高精地图)、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布,欢迎扫描下方二维码,加入自动驾驶之心知识星球(三天内无条件退款),日常分享论文+代码,这里汇聚行业和学术界大佬,前沿技术方向尽在掌握中,期待交流!
m0_47208263: 这个是哪篇论文请问
金示木: 怎么感觉把 agents 都翻译成了代理,不应该是智能体吗
大袖揽清风。: 21行应该没有-1次方吧?
一只想要努力的小菜鸡: 请问作者的工作开源了吗?
WeisonWeileen: mediapipe的hand模型的输出自带相对深度估计,而depthAny2 的输出也是相对深度估计