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

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

我们开始解方程吧！

LM算法主体就是一个方程的求解，也是其计算量最大的部分。当其近似于最速下降法的时候没有什么好讨论的，但是当其近似于Gauss-Newton法的时候，这个最小二乘解的问题就该好好讨论一下了。以下的讨论就利用Gauss-Newton的形式来求解。

稠密矩阵的最小二乘解

对于形如Ax=b的超定参数方程而言，有很多求解方式，伪逆、QR分解、SVD等等，这里不展开谈，想具体了解的可以去查阅矩阵理论相关资料。这些方式都有一个共同的特点，我们都是将A看作一般的稠密矩阵，主要得到的解自然非常鲁棒，都是计算量却是和维数的三次方成正比。BA这种超大规模的优化似乎有点不太实用。

稀疏矩阵的Cholesky分解

稠密矩阵计算起来那么复杂，如果是稀疏矩阵的话利用其稀疏的性质可以大幅减少计算量，对于稀疏矩阵的Cholesky分解就是这样。其分解形式为一个上三角矩阵的转置乘上自身：

image

为什么说我们的矩阵是稀疏的？

用一个非常简单的例子来解释吧，考虑有两个相机矩阵P1和P2、两个空间点X1和X2，其中X1只在P2中有投影，X2在两个相机（或视角）中都有投影。令优化函数为f(P1,P2,X1,X2)，此时的Jacobi矩阵为

image

考虑相机位置（图像数量）和空间点都非常多的情况，不难想象Jacobi矩阵不光是一个稀疏矩阵而且还可以写成形如[A|B]的分块矩阵。接下来就该利用这些性质正式进入计算了！

开始计算吧！

现在再回到Gauss-Newton最后的超定参数方程吧。既然Jacobi矩阵可以分块那我们就先分块，分块可以有效降低需要计算的矩阵的维度并以此减少计算量。

image

由此我们可以先求出，然后代回求出。其中被称为舒尔补（Schur complement）。分块降维之后的计算就可以利用稀疏的Cholesky分解进行计算了。

注意事项！

以上就基本将BA的整个过程进行了介绍，当然这只是最基础的思路，接下来一些遗漏点进行补充。

李群及李代数

不知道有没有人注意到，在优化迭代的过程中，我们求的值为，然后利用来更新x的值。这里就应该出现一个问题了，对于空间点的坐标和平移向量这么处理自然没有什么问题，但是对于旋转矩阵呢？难道用来更新R的值吗？好像不太对吧。

对于旋转矩阵R而言是不存在加法的，按理讲应该用来更新R的值，但是优化算法的迭代过程又不能是乘法，这就出现了矛盾。

这里旋转矩阵及相关运算属于李群，此时将旋转矩阵变换到其对应的李代数上进行计算，然后再变回李群。打个不是那么恰当的比方，在计算卷积的时候常常通过傅里叶变换计算乘积然后再反变换回来就是要求的卷积了，这个也是转换到李代数上计算然后再变回李群。具体的推导可以参看李群及李代数相关内容。

协方差矩阵

在我们的推导中是求解方程

但常常加入信息矩阵（协方差矩阵的逆），令求解方程变为

其中为协方差矩阵，令其为分块对角阵表示所有观测向量都不相关。

参考文献

• Triggs B, McLauchlan P F, Hartley R I, et al. Bundle adjustment—a modern synthesis[C]//International workshop on vision algorithms. Springer Berlin Heidelberg, 1999: 298-372.

• Hartley R, Zisserman A. Multiple view geometry in computer vision[M]. Cambridge university press, 2003.

• Barfoot T D. STATE ESTIMATION FOR ROBOTICS[J]. 2017.

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

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