SLAM后端优化（续）

一、“续”的由来

上一篇文章已经讲了SLAM的后端优化，使用的是Bundle Adjustment方法。而且介绍了如何利用矩阵的稀疏性加速运算。不过现实往往比我们想象的更残酷，即使利用稀疏性加速，随着地图越来越大，计算量仍然会增大到无法承受的地步。

于是，聪明的人们又想出各种方法加速优化，这就是本文将要介绍的内容。

在上一篇文章提到的BA图优化中，是把所有相机位姿和路标点都放在一起优化。从优化目标的数量入手，我们可以想办法做一些简化。比如，只优化相机位姿而不优化路标点，就是所谓的“位姿图（Pose Graph）优化”。只优化局部区域的相机位姿和路标点，并考虑不同位姿的权重，称为“共视图（Covisibility Graph）优化”。将图化简为最小生成树再优化，称为“本质图（Essential Graph）优化”。共视图优化和本质图优化是ORB-SLAM中采用的优化方式，具体内容可以参考相关论文。本文着重介绍一下位姿图优化。

二、位姿图（Pose Graph）的意义

这里所说的图其实就是图优化中图的概念。仍然是用顶点表示优化变量，用边表示误差项。回忆上一篇文章中的BA优化，是把相机位姿和路标点当成优化变量，把路标点的重投影误差当成误差项，只在相机位姿顶点和路标点之间建立边的连接。现在，为了减小运算量，我们只把相机位姿当成优化变量，让相邻的相机位姿之间用边连接起来。此时，边表达的就不是重投影误差了，而是两个相机位姿之间相对运动估计的误差。这个误差是用相机位姿顶点的值计算出的相对运动与用特征点法估计的相机运动之间的偏差，本质上与重投影误差没有区别，都是调整相机位姿使路标点在相机平面上的投影最准确。

定义好优化变量和误差项后，接下来就是求误差项关于优化变量的偏导数。求导过程请脑补，此处省略5000字:-)

三、位姿图优化的效果

下图是一个待优化的相机位姿图，看起来有些扭曲。

优化前的位姿图

优化后变成了下面这个样子。

优化后的位姿图

多么完美的球体啊！

事实上，优化前的位姿图就是在生成的球体上添加了观测噪声后形成的。看来位姿图优化完全去除了噪声，恢复了正确的位姿。

这个例子的完整代码请到高翔博士的GitHub上下载，地址是：https://github.com/gaoxiang12/slambook/tree/master/ch11

四、参考资料

《视觉SLAM十四讲》第11讲 后端2 高翔
有人知道orb-slam中的covisibility graph的定义吗？ 知乎
ORB-SLAM Raúl Mur-Artal, Juan D. Tardós, J. M. M. Montiel