Bundle Adjustment原理及应用(附实战代码)
作者：晚餐男孩
来源：微信公众号|3D视觉工坊（系投稿）
3D视觉精品文章汇总：https://github.com/qxiaofan/awesome-3D-Vision-Papers/
虽然现在的轮子很多，但我们在使用过程中会碰到很多问题，而我们经常不知道从哪里下手，说明轮子不是你造的你不熟悉。因此我们不仅要重复造轮子，还要好好造，深入造，才能用好轮子，把轮子转化成自身的力量。同样的道理适用于这篇文章。虽然网上BA的资料无穷无尽，但我们还是要好好深入理解其原理，并且一定要通过实践才能懂得其中原理。在“第一届SLAM论坛”中沈劭劼老师的发言中，他提到团队的成员都要手写BA，既然大佬都这么做，我们就照做吧。这篇文章是我手写BA的笔记，主要从原理推导入手，把公式都写一遍，然后通过g2o、ceres和eigen三种方式来编程实现，以便加深对BA的理解。
本文所有例子代码地址：在公众号「3D视觉工坊」，后台回复「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用的不多，总结一下其使用步骤：
- 构建cost fuction，即代价函数。
- 通过代价函数构建待求解的优化问题。
- 配置求解器参数并求解问题。
针对公式(43)提出的问题，定义一个类，里面包含有观测值和估计值，最重要的是误差计算（ceres里面常用重载运算符来实现），然后利用该类生成代价函数，最终求解问题。
这里要说一下，ceres例子用的是AutoDiffCostFunction，即自动计算导数，所以不用自己去求导。其他的看代码就清楚了。
七、Eigen应用
这部分在eigen文件夹里面。只用Eigen就比较痛苦了，数据结构、Jacobian、高斯牛顿等都要自己一步步理解和实现，也踩了一些坑，最后还是完成了，不过过多展开，具体看代码好了，有几点强调：
- 所有的实现都只优化了pose，没有优化point；
- 高斯牛顿的步骤要记牢，根据理论的步骤一步步来，不要贪心；
- Jacobian和deltase3的纬度和对应关系要搞清，否则会晕，导致中途放弃；
- 求解出来的deltase3的旋转和平移顺序要注意，否则会导致错误的结果。
八、总结
总体来说，g2o比较经典，容易让人理解，但有一定工程量，性能也不如ceres。Ceres实现起来最方便，不用过多关注细节，可快速开发。手写工程量就很大，性能最差，但可以让人上手，加深对BA的理解。本文所有例子代码参见文中开头下载地址，代码中的refs文件夹有关于非线性优化库的性能指标的一些论文，感兴趣的可自行查阅。
备注：作者也是我们「3D视觉从入门到精通」特邀嘉宾：一个超干货的3D视觉学习社区
本文仅做学术分享，如有侵权，请联系删文。
3D视觉工坊-ORB-SLAM源码学习交流群
已建立3D视觉工坊-ORB-SLAM源码学习微信交流群！想要进学习交流群的同学，可以直接加微信号：CV_LAB 。加的时候备注一下：ORB-SLAM+学校+昵称，即可。然后就可以拉你进群了。
强烈推荐大家关注3D视觉工坊知乎和3D视觉工坊微信公众号，可以快速了解到最新优质的3D视觉论文。
虽然现在的轮子很多，但我们在使用过程中会碰到很多问题，而我们经常不知道从哪里下手，说明轮子不是你造的你不熟悉。因此我们不仅要重复造轮子，还要好好造，深入造，才能用好轮子，把轮子转化成自身的力量。同样的道理适用于这篇文章。虽然网上BA的资料无穷无尽，但我们还是要好好深入理解其原理，并且一定要通过实践才能懂得其中原理。在“第一届SLAM论坛”中沈劭劼老师的发言中，他提到团队的成员都要手写BA，既然大佬都这么做，我们就照做吧。这篇文章是我手写BA的笔记，主要从原理推导入手，把公式都写一遍，然后通过g2o、ceres和eigen三种方式来编程实现，以便加深对BA的理解。
本文所有例子代码地址：在公众号「3D视觉工坊」，后台回复「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用的不多，总结一下其使用步骤：
- 构建cost fuction，即代价函数。
- 通过代价函数构建待求解的优化问题。
- 配置求解器参数并求解问题。
针对公式(43)提出的问题，定义一个类，里面包含有观测值和估计值，最重要的是误差计算（ceres里面常用重载运算符来实现），然后利用该类生成代价函数，最终求解问题。
这里要说一下，ceres例子用的是AutoDiffCostFunction，即自动计算导数，所以不用自己去求导。其他的看代码就清楚了。
七、Eigen应用
这部分在eigen文件夹里面。只用Eigen就比较痛苦了，数据结构、Jacobian、高斯牛顿等都要自己一步步理解和实现，也踩了一些坑，最后还是完成了，不过过多展开，具体看代码好了，有几点强调：
- 所有的实现都只优化了pose，没有优化point；
- 高斯牛顿的步骤要记牢，根据理论的步骤一步步来，不要贪心；
- Jacobian和deltase3的纬度和对应关系要搞清，否则会晕，导致中途放弃；
- 求解出来的deltase3的旋转和平移顺序要注意，否则会导致错误的结果。
八、总结
总体来说，g2o比较经典，容易让人理解，但有一定工程量，性能也不如ceres。Ceres实现起来最方便，不用过多关注细节，可快速开发。手写工程量就很大，性能最差，但可以让人上手，加深对BA的理解。本文所有例子代码参见文中开头下载地址，代码中的refs文件夹有关于非线性优化库的性能指标的一些论文，感兴趣的可自行查阅。
备注：作者也是我们「3D视觉从入门到精通」特邀嘉宾：一个超干货的3D视觉学习社区
本文仅做学术分享，如有侵权，请联系删文。
3D视觉工坊-ORB-SLAM源码学习交流群
已建立3D视觉工坊-ORB-SLAM源码学习微信交流群！想要进学习交流群的同学，可以直接加微信号：CV_LAB 。加的时候备注一下：ORB-SLAM+学校+昵称，即可。然后就可以拉你进群了。
强烈推荐大家关注3D视觉工坊知乎和3D视觉工坊微信公众号，可以快速了解到最新优质的3D视觉论文。
