视觉slam14讲——第11讲后端2

本系列文章是记录学习高翔所著《视觉slam14讲》的内容总结，文中的主要文字和代码、图片都是引用自课本和高翔博士的博客。代码运行效果是在自己电脑上实际运行得出。手动记录主要是为了深入理解
涉及到的主要内容如下

• 理解Pose Graph 优化
• 因子图优化
• 增量式图优化

1 位姿图(Pose Graph)

1.1 Pose Graph的意义

带有相机位姿和空间点的优化称为BA优化，可以求解大规模定位和建图的问题。但是随着机器人运动轨迹变长，地图规模变大，BA的计算效率会降低。
实际上，通过若干次的观测之后，收敛的特征点，空间位置会收敛到一个固定值，而发散的点则观测不到了。
因此，倾向于在优化几次后就把特征点固定住，只把他们看做位姿估计的约束，不再实际优化他们的位置估计。

可以构建一个只有轨迹的图优化，而位姿节点之间的边，由关键帧之间通过特征匹配之后得到的运动估计来给定初始值。不同的是，初始估计完成后就不再优化那些路标点的位置，而只关心所有相机位姿之间的关系。

1.2 Pose Graph优化推导

Pose Graph节点表示相机位姿，用ξ1,...,ξn表达，边是两个位姿节点之间的相对运动估计，这种估计可能来自于特征点法或者直接法，称ξi和ξj之间运动为△ξij。

△ξij=ξ−1i。ξj=ln(exp((−ξi)^)exp(ξ^j))∨

构建误差

eij=ln(exp((−ξij)^)exp((−ξi)^)exp(ξ^j))∨

优化变量有两个ξi和ξj，最终的总体目标函数为

minξ=12∑i,j∈εeTi,j∑i,j−1ei,j

接下来用高斯牛顿法、列文伯格——马夸特方法求解此问题。

2 因子图优化

2.1 贝叶斯网络Bayes Network

从贝叶斯网络的角度来看，SLAM可以表达成一个动态的贝叶斯网络(Dynamic Bayes Network,DBN)。贝叶斯网络是一种概率图，由随机变量节点和表达随机变量条件独立性的边组成，形成一个有向无环图。

通过这种方式，构建一个贝叶斯网络，表达了所有的变量，以及各个方程给出的变量之间的条件概率关系。后端优化的目标就是在这些约束下，通过调整贝叶斯网络中随机变量的值，是的后验概率达到最大。

{x,l}∗=arg max(x0)∏P(xk|xk−1,μk)∏P(zk|xi,lj)

这样一个由条件概率描述的贝叶斯网络，可以使用概率图模型中的算法进行求解。进一步观察发现后验概率由多项因子成绩组成，因此贝叶斯网络又可以转换为一个因子图(Factor Graph)

2.2 因子图

因子图是无向图，由两种节点组成，表示优化变量的节点和表示因子的因子节点。

运动方程和观测方程作为因子存于图中。因为可能表达成概率分布的信息有许多种，于是因子图也可以定义成许多不同的因子。比如轮式编码器的测量、IMU的测量，使之成为一种非常通用的优化方式。

2.3 增量特性

从优化的角度来讲，优化一个因子图和普通的图并没有太大区别——因为最后都是解决一个最小二乘问题，不断寻找梯度，使目标函数下降。因子图优化的稀疏性与凸优化类似，通过稀疏QR分解、Schur补或者Cholesky分解，加速对因子图优化的求解。
Kaess等人提出的iSAM(incremental Smooth and Mapping)中，对因子图进行了更加精细的处理，使得它可以增量式的处理后端优化。
调整目标函数

J(x)=∑keTv,kR−1kev,k+∑k∑jeTy,k,jQ−1k,jey,k,j

中的优化变量，使目标函数下降。无论采用哪种梯度下降策略，最后碰到一个形如

H△x=g

的线性方程求解。