math: 图优化理论基础

图优化是视觉slam中的主流优化方法，所谓的图优化是把常规的优化问题以图的形式来表述。

图(graph)由顶点(Vertex)和边(Edge)组成,在常见的slam问题中，机器人的位姿是一个顶点(Vertex)，不同时刻位姿之间的关系构成边(Edge)，通过不断累积而成的顶点和边构成图(graph)结构，图优化的目标就是通过调整顶点的位姿最大可能的满足边(Edge)之间的约束。其中通过传感器累计信息构建图的过程在slam中称为前端，调整位姿满足约束的优化过程成为后端。

1、举例说明

举个例子以帮助理解：

如图所示，机器人起始位置在x0处，并观察到前方2m有一个路标l0，然后机器人向前移动，通过编码器检测到它向 前移动了1m，然后观测到路标l0在前方0.8m处，如何通过图优化计算出机器人位姿和路标位置的最优状态？

根据机器人的运动过程构建图如上图所示，边的约束关系如下：

即:

残差平方和:

求偏导数:

最后整理计算:

得到机器人位姿和路标位置：

在构建图的过程中，还需要考虑到不同传感器的精度差别，也就是我们对每个传感器的信任程度是不同，对于不同的信任程度我们赋予不同的权重，这就是SLAM中信息矩阵的概念。

2.理论推导

假设一个带有n条边的图，其目标函数可以写成:

说明：

1、其中e作为优化变量xk与zk的符合程度的度量，越大表示两者越不符。用e的平方形式来表示目标函数：

为了表示我们对误差各分量重视程度的不一样，还使用一个信息矩阵 Ω 来表示各分量的不一致性。

2、信息矩阵 Ω 是协方差矩阵的逆，是一个对称矩阵。它的每个元素Ωi,j作为eiej的系数，可以看成我们对ei,ej这个误差项相关性的一个预计。最简单的是把Ω设成对角矩阵，对角阵元素的大小表明我们对此项误差的重视程度。

3、这里的xk可以指一个顶点、两个顶点或多个顶点，取决于边的实际类型。所以，更严谨的方式是把它写成ek(zk,xk1,xk2,…)，但是那样写法实在是太繁琐，我们就简单地写成现在的样子。由于zk是已知的，为了数学上的简洁，我们再把它写成ek(xk)的形式。

于是总体优化问题变为n条边加和的形式：

现在，我们有了一个很多个节点和边的图，构成了一个庞大的优化问题。我们并不想展开它的数学形式，只关心它的优化解。那么，为了求解优化，需要知道两样东西：一个初始点和一个迭代方向。为了数学上的方便，先考虑第k条边ek(xk)吧。

我们假设它的初始点为xk，并且给它一个Δx的增量，那么边的估计值就变为Fk(xk+Δx)，而误差值则从 ek(x) 变为 ek(xk+Δx)。首先对误差项进行一阶展开：

这是的Jk是ek关于xk的导数，矩阵形式下为雅可比阵。我们在估计点附近作了一次线性假设，认为函数值是能够用一阶导数来逼近的，当然这在Δx很大时候就不成立了。

　于是，对于第k条边的目标函数项，有：

　进一步展开：

最后一个式子是个定义整理式，我们把和Δx无关的整理成常数项 Ck ，把一次项系数写成 2bk ，二次项则为 Hk（注意到二次项系数其实是Hessian矩阵）。

　请注意 Ck 实际就是该边变化前的取值。所以在xk发生增量后，目标函数Fk项改变的值即为

我们的目标是找到Δx，使得这个增量变为极小值。所以直接令它对于Δx的导数为零，有：

　所以归根结底，我们求解一个线性方程组:

如果把所有边放到一起考虑进去，那就可以去掉下标，直接说我们要求解

原来算了半天它只是个线性的！线性的谁不会解啊！

　读者当然会有这种感觉，因为线性规划是规划中最为简单的，连小学生都会解这么简单的问题，为何21世纪前SLAM不这样做呢？——这是因为在每一步迭代中，我们都要求解一个雅可比和一个海塞。而一个图中经常有成千上万条边，几十万个待估计参数，这在以前被认为是无法实时求解的。

　那为何后来又可以实时求解了呢？

　SLAM研究者逐渐认识到，SLAM构建的图，并非是全连通图，它往往是很稀疏的。例如一个地图里大部分路标点，只会在很少的时刻被机器人看见，从而建立起一些边。大多数时候它们是看不见的。体现在数学公式中，虽然总体目标函数F(x)有很多项，但某个顶点xk就只会出现在和它有关的边里面！

这会导致什么？这导致许多和xk无关的边，比如说ej，对应的雅可比Jj就直接是个零矩阵！而总体的雅可比J中，和xk有关的那一列大部分为零，只有少数的地方，也就是和xk顶点相连的边，出现了非零值。

相应的二阶导矩阵H中，大部分也是零元素。这种稀疏性能很好地帮助我们快速求解上面的线性方程。稀疏代数库包括SBA、PCG、CSparse、Cholmod等等。g2o正是使用它们来求解图优化问题的。

3. 图优化的流程

最后总结一下做图优化的流程。

1、选择你想要的图里的节点与边的类型，确定它们的参数化形式；

2、往图里加入实际的节点和边；

3、选择初值，开始迭代；

4、每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；

5、求解稀疏线性方程HkΔx=bk，得到梯度方向；

6、继续用GN或LM进行迭代。如果迭代结束，返回优化值。

参考资料：

https://www.cnblogs.com/gaoxiang12/p/5244828.html

http://blog.csdn.net/heyijia0327/article/details/47686523