虽然现在的轮子很多，但我们在使用过程中会碰到很多问题，而我们经常不知道从哪里下手，说明轮子不是你造的你不熟悉。因此我们不仅要重复造轮子，还要好好造，深入造，才能用好轮子，把轮子转化成自身的力量。同样的道理适用于这篇文章。虽然网上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.针孔相机的投影函数

由于排版问题，剩下部分请读者跳转至微信公众号「3D视觉工坊」阅读原文：https://mp.weixin.qq.com/s/hIxM3dNCmL6kb3_VW_66-g