高翔【自动驾驶与机器人中的SLAM技术】学习笔记（十二）拓展图优化库g2o（一）框架

原创

已于 2024-10-29 10:50:53 修改 · 1.3k 阅读

17 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶 #g2o #SLAM

于 2024-10-24 18:16:51 首次发布

【转载】理解图优化，一步步带你看懂g2o框架

文章来源：理解图优化，一步步带你看懂g2o框架

小白：师兄师兄，最近我在看SLAM的优化算法，有种方法叫“图优化”，以前学习算法的时候还有一个优化方法叫“凸优化”，这两个不是一个东西吧？

师兄：哈哈，这个问题有意思，虽然它们中文发音一样，但是意思差别大着呢！我们来看看英文表达吧，图优化的英文是 graph optimization 或者 graph-based optimization，你看，它的“图”其实是数据结构中的graph。而凸优化的英文是 convex optimization，这里的“凸”其实是凸函数的意思，所以单从英文就能区分开它们。

小白：原来是这样，我看SLAM中图优化用的很多啊，我看了一下高博的书，还是迷迷糊糊的，求科普啊师兄

师兄：图优化真的蛮重要的，概念其实不复杂，主要是编程稍微有点复杂。。

小白：不能同意更多。。，那个代码看的我一脸懵逼。

一、图优化有什么优势？

师兄：按照惯例，我还是先说说图优化的背景吧。

SLAM的后端一般分为两种处理方法，
- 一种是以扩展卡尔曼滤波（EKF）为代表的滤波方法，
- 一种是以图优化为代表的非线性优化方法。
不过，目前SLAM研究的主流热点几乎都是基于图优化的。
补充AI回答：

gtsam、Ceres与g2o、SE-sync目前哪个是主流？

根据最新的研究和比较，se-Sync在姿态估计的图优化算法中表现最为突出，成为当前的主流方法之一。se-Sync通过利用特殊欧几里得同步问题的新颖（凸）半定松弛直接搜索全局最优解，相比其他方法在优化时间和性能上具有显著优势。特别是，在处理具有高噪声和复杂数据关联的场景中，se-Sync显示出了极高的效率和可靠性。

尽管G2O和Ceres也是一个非常强大的框架，分别以其易用性和灵活性以及高效的求解器而受到青睐，但在最新的比较中，它们在某些特定任务和数据集上的表现不如se-Sync。gtsam虽然在某些情况下性能与se-Sync相当，但在面对极端噪声和复杂场景时，se-Sync的鲁棒性和效率使其脱颖而出。

因此，如果考虑当前的主流选择，se-Sync是一个值得考虑的强有力选项，特别适用于需要高效和准确姿态估计的应用场景。

小白：据我所知，滤波方法很早就有了，前人的研究也很深。为什么现在图优化变成了主流了？

师兄：你说的没错。滤波方法尤其是EKF方法，在SLAM发展很长的一段历史中一直占据主导地位，早期的大神们研究了各种各样的滤波器来改善滤波效果，那会入门SLAM，EKF是必须要掌握的。顺便总结下滤波方法的优缺点：

优点：在当时计算资源受限、待估计量比较简单的情况下，EKF为代表的滤波方法比较有效，经常用在激光SLAM中。

缺点：它的一个大缺点就是存储量和状态量是平方增长关系，因为存储的是协方差矩阵，因此不适合大型场景。而现在基于视觉的SLAM方案，路标点（特征点）数据很大，滤波方法根本吃不消，所以此时滤波的方法效率非常低。

小白：原来如此。那图优化在视觉SLAM中效率很高吗？

师兄：这个其实说来话长了。很久很久以前，其实就是不到十年前吧(感觉好像很久)，大家还都是用滤波方法，因为在图优化里，Bundle Adjustment（后面简称BA）起到了核心作用。但是那会SLAM的研究者们发现包含大量特征点和相机位姿的BA计算量其实很大，根本没办法实时。

小白：啊？后来发生了什么？（认真听故事ing）

师兄：后来SLAM研究者们发现了其实在视觉SLAM中，虽然包含大量特征点和相机位姿，但其实BA是稀疏的，稀疏的就好办了，就可以加速了啊！比较代表性的就是2009年，几个大神发表了自己的研究成果《SBA：A software package for generic sparse bundle adjustment》，而且计算机硬件发展也很快，因此基于图优化的视觉SLAM也可以实时了！

小白：厉害厉害！向大牛们致敬！

二、图优化是什么？

小白：图优化既然是主流，那我可以跳过滤波方法直接学习图优化吧，反正滤波方法也看不懂。。

师兄：额，图优化确实是主流，以后有需要你可以再去看滤波方法，那我们今天就只讲图优化好啦

小白：好滴，那问题来了，究竟什么是图优化啊？

师兄：图优化里的图就是数据结构里的图，一个图由若干个顶点（vertex），以及连接这些顶点的边（edge）组成，给你举个例子

比如一个机器人在房屋里移动，它在某个时刻 t 的位姿（pose）就是一个顶点，这个也是待优化的变量。而位姿之间的关系就构成了一个边，比如时刻 t 和时刻 t+1 之间的相对位姿变换矩阵就是边，边通常表示误差项。

在SLAM里，图优化一般分解为两个任务：

1、构建图。机器人位姿作为顶点，位姿间关系作为边。

2、优化图。调整机器人的位姿（顶点）来尽量满足边的约束，使得误差最小。

下面就是一个直观的例子。我们根据机器人位姿来作为图的顶点，这个位姿可以来自机器人的编码器，也可以是ICP匹配得到的，图的边就是位姿之间的关系。由于误差的存在，实际上机器人建立的地图是不准的，如下图左。我们通过设置边的约束，使得图优化向着满足边约束的方向优化，最后得到了一个优化后的地图（如下图中所示），它和真正的地图（下图右）非常接近。