今天学习了稀疏的光束平差法，基于上一篇博文Levenberg–Marquardt算法学习，这里对学习内容做一个理论梳理。本次内容包括：
BA简介
BA迭代步长的数学推导
稀疏BA迭代步长的算法求解过程
1.BA简介
摄像机在静态环境中移动，得到不同时刻拍摄的多幅图像。假设这些图像是同一刚性物体的投影，则可由图像特征对应关系估计出摄像机的运动参数。在计算机视觉中 ，这一过程称为运动分析或由运动重建物体结构(structure frommotion)。
Bundle Adjustment即光束平差法，作为SFM这种多视重建视觉算法的最后一步，它利用LM算法使得观测的图像点坐标与预测的图像点坐标之间的误差最小。若给定图像特征点的对应关系及初始三维点，BA可以同时精化这些特征点对应的3D坐标及相应的相机参数。
Bundle Adjustment的名字由来于空间中每个物点和相机光学中心“发射”出的光束，人们可以根据这些光束对结构和视角参数进行调节，获得空间结构及视角参数的最优解。
2.BA迭代步长的数学推导
以下推导来自对希腊人论文的翻译：”The Design andImplementation of a Generic Sparse Bundle Adjustment Software Package Based onthe Levenberg-Marquardt Algorithm”
假设空间中有n个三维物点，现在围绕这些物点拍摄了m张照片，则第j张图片上看到的第i个物点为xij。Bundle adjustment旨在优化初始多个相机与结构的参数估计，以便于找到合理的参数使得我们能够精确计算出m张照片中n个物点的空间坐标。更具体的说，每个相机j用向量aj表示(内参和外参)，每个三维物点i用向量bi表示。为了简化问题，假设现在所有的图片中能看到所有物点(不看到也没关系，后边的矩阵相应位置为0呗)。BA的核心问题就是最小化下面的重投影误差函数(非线性)：
函数Q(aj,bi)表示物点bi在相机aj下的投影坐标，也是我们的预测值。函数d(x,y)表示观测的图像坐标与预测的图像坐标之间的欧氏距离。
现在我们用向量P代表m个投影矩阵和n个三维物点所有参数：
J是关于投影关系f的雅各比矩阵，是迭代步长，使得我们获取合理的P让残差函数最小(阻尼因子的处理见后文)。上述方程同之前LM算法那篇文章里的迭代公式几乎一样(因为此处的协方差矩阵是单位矩阵)。此外由于各个照片与三维物点之间的参数没有交集，我们发现上述公式其实是非常稀疏的。简便期间，我们举个简单的例子：
假设现在有m=3张照片拍摄了n=4个物点，即观测坐标X与参数几何P分别为：
由于各个照片与三维物点之间的参数没有交集，比如对于不属于当前相机的二维图像坐标的偏导数为0，对于不属于当前三维物点投影的二维图像坐标的偏导数为0
那么对于投影函数关系X’=f(P)，它的偏导数集合，也就是雅可比矩阵J可以写成：
观测矩阵X的协方差矩阵是对角块结构的：
将协方差矩阵和雅可比矩阵代入LM的迭代方程，方程的左边将是如下形式：
若定义：
则LM迭代公式的左侧可以写成：
LM迭代公式的右边为：
若定义：
完整的LM迭代公式如下：
再次简化，如果定义：
那么迭代方程可以进一步简化成：
将U*,W,V*代入(1)，那么(1)的左边为：
反过来倒过去的定义，只是为了公式更清晰的展示，发现规律：我们发现对于任意数量的n物点和m照片均可以求解 LM的迭代步长。如果点k没有在照片l中出现，那么Akl=0并且Bkl=0.接下来我们介绍下基于LM的稀疏BA的迭代步长的算法流程。
3.稀疏BA迭代步长的算法求解过程
算法输入：
m个初始相机参数aj，j=1,…,m；n个初始三维物点坐标bi，i=1,…,n，观测的特征点坐标xij(第j张图第i个点)，LM算法的阻尼因子μ
算法输出：
基于LM的稀疏BA的迭代步长的解
算法流程：
计算偏导数矩阵，Q表示投影函数，i=1,…,n，j=1,…,m
将Uj和Vi的主对角线元素上加上阻尼因子μ，我们得到Uj*和Vi
计算Yij=WijVi*-1
按照(1)(2)式计算LM迭代步长：
现在有了迭代步长，我们把迭代步长的计算步骤嵌入标准的LM算法流程使得重投影残差最小。至于阻尼因子是否要用信頼域的方法，随便吧，已经够麻烦的了，反正代码用现成的接口opencv和openmvg都有。
下面是我的想法：
BA的目标是帮我们求得相机参数和三维坐标，每次LM迭代修改的都是参数集合P(由相机内外参数和三维点坐标组成)，而观测向量X每次都是恒定的。这里初始参数P0作用和LM博文中函数拟合的初始参数一样。
而初始参数P0是怎么获取的呢？
当空间物体结构参数未知时，SFM问题可以分为两类：单目视觉下的二维特征对应和多目视觉下的三维特征对应。
采用二维特征对应关系估计相对运动需要给定先验的空间尺度信息, 这为单目视觉里程计的实现带来一定的不便(就是拿个相机对着某个物体不同角度拍N张照片，计算的3D点都是假的)。如果相机已经标定，可以利用各个照片上特征点的对应关系，并在极几何性质帮助下，求出相机的外部参数R是真的，T只是方向，反推的3D点也是up-to-scale的。咱就利用这些东西作为初始P0.
三维特征对应关系下求解运动估计问题的一般方法为: 首先采用双目或多目摄像机三维重建得到空间物体的三维数据; 然后由二维图像特征对应关系建立空间物体的三维特征对应, 进而进行三维运动问题求解. 三维数据的信息量远高于二维图像, 因此三维运动估计问题的求解大为简化. 但是, 由于立体视觉中三维重建过程对像素误差有放大作用, 三维运动估计的结果对图像点误差非常敏感, 需要采取一定的措施对三维重建结果进行优化以提高运动估计精度。我认为双目测距由于baseline已知，咱可以根据disparity恢复出真正的3D坐标。然后各个相机之间的RT，同样利用之前的极几何性质，可以求出来。然后把相机参数和3D坐标作为初始值P0.
另外，BA所使用的每张图片的二维特征点和图片像素比起来是稀疏的，所以最终还原的三维空间也是稀疏的，只能看个大概。
