VSLAM基础（七）————光束平差法Bundle Adjustment

上一篇我们讲了线性的超定方程的解法，这节我们就接着讲讲slam里非线性方程的优化方法。

一、BA光束平差法

偷个图：

首先到底啥是BA呢？

在slam前端中，我们通过匹配特征点、对极约束、三角化就可以恢复出相机姿态（R，t）与三维物点坐标（用X表示），这两数据是我们通过观测到的像点坐标（用Z表示）求得。

但是实际情况中往往存在大量的噪声，点与点不是精确地对应甚至出现一些错误匹配。 这就导致我们预测求出的R，t，X存在误差，我们需要在已知的观测值下找到一个方法能够调整优化我们求出的R，t，X使得尽可能的靠近真实值。

光束平差法由Bundle Adjustment翻译得来： 
            对场景中任意三维点P，由从每个视图所对应的的摄像机的光心发射出来并经过图像中P对应的像素后的光线，都将交于P这一点，对于所有三维点，则形成相当多的光束（bundle）；实际过程中由于噪声等存在，每条光线几乎不可能汇聚与一点，因此在求解过程中，需要不断对待求信息进行调整（adjustment），来使得最终光线能交于点P。

具体来说：

可以想象存在很多条光线，其中每一条连接了相机光心，三维物点，物点在相片上的投影，这个模型（为了简单化暂时不考虑畸变的影响）我们通过之前所讲过的P矩阵描述，即Z = PX，因为误差的存在PX并不绝对与Z相等，此时我们就是要调整（R，t，X）使得代价 最小。因为从每一个位姿处观察物点都可以视为一个光束，则BA就是优化整体所有光束代价最小时的（R，t，X）。如下式，对于m帧，每帧含N个特征点的目标函数如下：

 
 
其中：x˜表示受白噪声影响的估计二维点坐标，π为投影函数， 如果点j出现在图i上，则θ=1，否则θij=0。 

到了这里也就能明白BA是啥了，普遍意义上BA就是通过P矩阵（共线方程）建立非线性方程组，并求解最小二乘解得优化方法，核心是这个共线方程，这里涉及到几个变量——3D点坐标与相机位姿，于是就存在不同的应用情景，在vslam里常见的有：

1. 在预测出相机姿态和物点坐标后，将这些都作为变量进行全局优化
2. 在PnP预测位姿后，只将位姿作为变量，物点坐标作为不变量进行位姿的优化

可以说BA是个万金油的方法，只要建立好模型进行优化就好，那么接下来的问题：上面的那个式子怎么求解呢？

二、非线性最小二乘

首先我们回顾一下线性最小二乘的解法————将方程化为Ax = b的形式，接着就可以求解析解了。当然这么做的基础是该问题为线性的我们可以直接对目标函数求导，并且令其等于零，以此求得其极值，并通过比较求取全局最小值(Global Minimizer)，并将其最为目标函数的解。

但是如果问题为非线性，此时我们通常无法直接写出其导数形式（函数过于复杂），因此我们不再去试图直接找到全局最小值，而是退而求其次通过不停的迭代计算寻找到函数的局部最小值(Local Minimizer)，并认为该局部最小值能够使得我们的目标函数取得最优解（最小值），这就是非线性最小二乘的通常求解思路。

这样就将一个求导的问题转化为不断寻找梯度并下降的方法：

1. 给定某个初值；
2. 对于第K次迭代，寻找一个增量，使得达到极小值（局部最小）
3. 若足够小则停止迭代
4. 否则 另，返回第二步

 所以可以看到在上述步骤中，核心是第二步，于是问题继续简化为求的局部极小值问题。很明显有最直接的方法将在处泰勒展开，之后对求导并令之为0，就能解得。这里如果为一阶泰勒展开则称为最速下降法（过于贪心增加了迭代次数），若是二阶展开称为牛顿法（需要求H矩阵——二阶导数Hessian矩阵——过于复杂）。因为slam问题里方程规模一般比较大，所以不会采用这两种方式。下面介绍两种在slam问题中更加实用的优化方法。

2.1高斯牛顿法Gauss-Newton

对展开

将其带入有化简并对求导并令之为0可以得到：

               其中J(x)为f(x)的一阶导数

可以看到上式已经是一个线性方程了（Ax = b），于是解这个方程就行了。

2.2列文伯格-马夸尔特方法Levenberg-Marquardt

LM算法大致与高斯牛顿算法理论相同，但是不同于高斯牛顿算法，LM算法是基于信赖区域理论(Trust Region Method)进行计算的。这是因为高斯牛顿法中的泰勒展开只有在展开点附近才会有比较好的效果，因此为了确保近似的准确性我们需要设定一个具有一定半径的区域作为信赖区域。

采用信赖区域法我们就需要明确该区域该怎么确定。在LM算法中信赖区大小的确定也是运用增益比例来进行判定的：

 基于信赖区域我们能够重新构建一个更有效的优化框架：

所以求出L-M总的增量方程可以写为(H+λI)△x=b：

特殊的：

1. 当λ= 0时，为G-N；
2. 当λ= ∞时，L-M等于一阶梯度下降

所以一般SLAM中常用L-M：G-N中的H矩阵可能为奇异矩阵或者病态矩阵，导致算法不收敛。而且当步长较大时，也无法保证收敛性，所以采用L-M求解增量方程，但是它的收敛速度可能较慢。

三、增量方程的求解

在第二小节中我们将非线性最小二乘问题都归结到了求解一个Hx = b的线性方程的问题上。这里当然可以按着我们上一讲所说求得解析解，但是在slam的BA求解中我们需要优化的变量可能极其庞大（假设有N张相片，m个物点，则优化量有6N+3m，在大规模BA里m一般上万到几十万）这时我们需要对H矩阵求逆就会浪费大量空间与时间。

显然想要求解增量线性方程需要找到其他的方法。

而人们发现了H的稀疏性后便找到了简化运算的方法：

eg：在上述中V分块矩阵为对角块矩阵它的逆很容易求