Bundle Adjustment原理及应用

虽然现在的轮子很多，但我们在使用过程中会碰到很多问题，而我们经常不知道从哪里下手，说明轮子不是你造的你不熟悉。因此我们不仅要重复造轮子，还要好好造，深入造，才能用好轮子，把轮子转化成自身的力量。同样的道理适用于这篇文章。虽然网上BA的资料无穷无尽，但我们还是要好好深入理解其原理，并且一定要通过实践才能懂得其中原理。在“第一届SLAM论坛”中沈劭劼老师的发言中，他提到团队的成员都要手写BA，既然大佬都这么做，我们就照做吧。这篇文章是我手写BA的笔记，主要从原理推导入手，把公式都写一遍，然后通过g2o、ceres和eigen三种方式来编程实现，以便加深对BA的理解。

本文所有例子代码地址：在公众号「计算机视觉工坊」，后台回复「Bundle Adjustment」，即可直接下载。

一、前言

Bundle Adjustment中文译作光束平差法、捆集调整等，是指从视觉重建中提炼出最优的3D模型和相机参数（内参和外参）。从每个特征点反射出来的几束光线（bundles of light rays），在我们把相机姿态和特征点的位置做出最优的调整（adjustment）之后，最后收束到光心的这个过程，简称BA。

二、原理

三、必备知识

1.SO(3)的对数映射 Exponential Map

2.向量外积

3.SO(3)的Jacobian

根据公式(12)，SO3的对数映射的导数是：

4.SE(3)的对数映射 Exponential Map

5.SE(3)的Jacobian

7.Pose-Pose的Jacobian

那么，在位姿求导的公式(23)中，位姿用4*4矩阵来表达。但当它用向量展开之后，最后一行是被忽略的。位姿变成12个向量。虽然这表明一个6自由度的实体过度参数化了，但通过这样的表达，很多重要的步骤变成线性的了，使得我们可以更加有效地获取精确的导数。我们用T代替p，则公式(23)表示成：

其结果可表示成：

把f设为F，根据上式则有：

记住公式(26)，待会要用来计算左乘模型的Jacobian矩阵。

9.Pose-Point的Jacobian

该问题则转换成重投影误差函数对位姿的求导，利用公式(33)和(31)，结果为：

四、推导

1.针孔相机的投影函数

假设针孔相机的内参矩阵是K，则有：

2.三维坐标点p重投影函数的偏导数

函数对位姿的偏导数最终表达式已求得。

至此，函数的偏导数已求取完毕，分别是公式(40)和(42)。

五、g2o应用

六、ceres应用

这部分在ceres文件夹里面。之前ceres用的不多，总结一下其使用步骤：

1. 构建cost fuction，即代价函数。
2. 通过代价函数构建待求解的优化问题。
3. 配置求解器参数并求解问题。

针对公式(43)提出的问题，定义一个类，里面包含有观测值和估计值，最重要的是误差计算（ceres里面常用重载运算符来实现），然后利用该类生成代价函数，最终求解问题。

这里要说一下，ceres例子用的是AutoDiffCostFunction，即自动计算导数，所以不用自己去求导。其他的看代码就清楚了。

七、Eigen应用

这部分在eigen文件夹里面。只用Eigen就比较痛苦了，数据结构、Jacobian、高斯牛顿等都要自己一步步理解和实现，也踩了一些坑，最后还是完成了，不过过多展开，具体看代码好了，有几点强调：

1. 所有的实现都只优化了pose，没有优化point；
2. 高斯牛顿的步骤要记牢，根据理论的步骤一步步来，不要贪心；
3. Jacobian和deltase3的纬度和对应关系要搞清，否则会晕，导致中途放弃；
4. 求解出来的deltase3的旋转和平移顺序要注意，否则会导致错误的结果。

八、总结

总体来说，g2o比较经典，容易让人理解，但有一定工程量，性能也不如ceres。Ceres实现起来最方便，不用过多关注细节，可快速开发。手写工程量就很大，性能最差，但可以让人上手，加深对BA的理解。本文所有例子代码参见文中开头下载地址，代码中的refs文件夹有关于非线性优化库的性能指标的一些论文，感兴趣的可自行查阅。