【泡泡机器人原创专栏】Bundle Adjustment简述 （二）

【泡泡机器人原创专栏】Bundle Adjustment简述 （二）

该进入数学模式了！

上一次感觉像写小说一样写了一堆堆的文字，既然BA是个数学问题，不用数学讲讲好像不太行，接下来就看看BA的数学模型是怎么构建的吧。

对BA有点了解的同学可能知道BA是一个图优化模型，那首先肯定要构造一个图模型了（没学过图论也没事，后面还是会回到一般的优化模型）。既然是图模型那自然就有节点和边了，这个图模型的节点由相机Pi和三维空间点Xj构成，如果Xj投影到相机Pi的图像上则将这两个节点连接起来。还是来张图吧。

image

这样就一目了然了。那么我们现在就可以通过这个图来构造优化模型了。令点Xj在相机Pi拍摄到的图像归一化坐标系上的坐标，其重投影后的图像归一化坐标系下坐标为，其中是为了在计算时能不受相机内参影响k和k'是将齐次坐标转换为非齐次坐标的常数项（当然你在做BA的时候也想考虑相机内参或者在图像平面上计算都是可以的，具体的模型只要合理就好，这里只是举一例而已），可以得到该重投影误差为

BA是要将所有重投影误差的和最小化，那么这里自然就要开始求和了。

image

其中当点Xj在相机Pi中有投影时=1，否则=0。

到此我们就得到了BA优化模型的数学形式了。

接下来就应该开始计算了！

既然是优化模型，那自然就应该用各种优化算法来进行计算了。这里先小小的剧透一下，BA现在基本都是利用LM（Levenberg-Marquardt）算法并在此基础上利用BA模型的稀疏性质来进行计算的，LM算法是最速下降法（梯度下降法）和Gauss-Newton的结合体，至于是怎么结合的接下来就来慢慢介绍了。

最速下降法

如果你对梯度比较熟悉的话，那你应该知道梯度方向是函数上升最快的方向，而此时我们需要解决的问题是让函数最小化。你应该想到了，那就顺着梯度的负方向去迭代寻找使函数最小的变量值就好了嘛。梯度下降法就是用的这种思想，用数学表达的话大概就是这样

其中为步长。

最速下降法保证了每次迭代函数都是下降的，在初始点离最优点很远的时候刚开始下降的速度非常快，但是最速下降法的迭代方向是折线形的导致了收敛非常非常的慢。

Newton型方法

现在先回顾一下中学数学，给定一个开口向上的一元二次函数，如何知道该函数何处最小？这个应该很容易就可以答上来了，对该函数求导，导数为0处就是函数最小处。

Newton型方法也就是这种思想，首先将函数利用泰勒展开到二次项：

其中J为Jacobi矩阵，对矩阵函数求一次偏导而来，梯度也是对向量函数求一次偏导而来。将标量考虑为1x1的矩阵，将向量考虑nx1的矩阵，其实这些求导都是求Jacobi矩阵。H为Hessian矩阵，也就是二次偏导矩阵。

也就是说Newton型方法将函数局部近似成一个二次函数进行迭代，然后令x在方向上迭代直至收敛，接下来自然就对这个函数求导了：

image

Newton型方法收敛的时候特别快，尤其是对于二次函数而言一步就可以得到结果。但是该方法有个最大的缺点就是Hessian矩阵计算实在是太复杂了，并且Newton型方法的迭代并不像最速下降法一样保证每次迭代都是下降的。

Gauss-Newton方法

既然Newton型方法计算Hessian矩阵太困难了，那有没有什么方法可以不计算Hessian矩阵呢？将泰勒展开式的二次项也去掉好像就可以避免求Hessian矩阵了吧，就像这样：

这好像变成了一个线性函数了啊，线性函数如果要最小化的话好像是需要增加其他的约束条件的啊。那这里有没有其他的约束条件呢？仔细思考一下，我们需要最小化的是重投影误差，它的最小值是什么呢？理想状态下当然是等于0了。所以这个时候就不应该求导了，而是直接令函数为0。此时，令有

image

由此x在方向上迭代直至最小。

Gauss-Newton方法就避免了求Hessian矩阵，并且在收敛的时候依旧很快。但是依旧无法保证每次迭代的时候函数都是下降的。

LM方法

LM方法就是在以上方法基础上的改进，通过参数的调整使得优化能在最速下降法和Gauss-Newton法之间自由的切换，在保证下降的同时也能保证快速收敛。

Gauss-Newton最后需要求解的方程为

LM算法在此基础上做了更改，变成了

通过参数的调节在最速下降法和Gauss-Newton法之间切换。做个不很数学的直观分析吧，当很小时，显然和Gauss-Newton法是一样的；当很大时，就变成了这样：

image

然后再看看前面的最速下降法？

这里还存在一个问题，当取某个值的时候可能会导致不可逆，所以这里变成了

其实LM算法的具体形式就笔者看到的就有很多种，但是本质都是通过参数在最速下降法和Gauss-Newton法之间切换。这里选用的是维基百科上的形式。

LM算法就由此保证了每次迭代都是下降的，并且可以快速收敛。

这次我们对BA的数学模型和优化方式做了简介，但是问题还没有完，接下来该解最小二乘方程了，敬请期待下一期。

【版权声明】泡泡机器人SLAM的所有文章全部由泡泡机器人的成员花费大量心血制作而成的原创内容，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！

【注】商业转载请联系刘富强（[email protected]）进行授权。普通个人转载，请保留版权声明，并且在文章下方放上“泡泡机器人SLAM”微信公众账号的二维码即可。