1. 算法简介
之后进入structure-from-motion部分，关键的第一步就是选择好的图像对去初始化整个BA过程。首先对初始化选择的两幅图片进行第一次BA，然后循环添加新的图片进行新的BA，最后直到没有可以继续添加的合适的图片，BA结束。得到相机估计参数和场景几何信息，即稀疏的3D点云。其中两幅图片之间的bundle adjust用的是稀疏光束平差法sba软件包，这是一种非线性最小二乘的优化目标函数算法。
2. 算法详述
2.1计算符合特征的图片
2.1.1特征检测
对于特征检测这一步，使用的是具有尺度和旋转不变性的SIFT描述子,其鲁棒性较强，适合用来提取尺度变换和旋转角度的各种图片特征点信息，其准确性强，在这种离线算法不需要考虑时间成本的情况下也较有优势。SIFT算法通过不同尺寸的高斯滤波器(DOG)计算得到特征点的位置信息(x,y),同时还提供一个描述子descriptor信息，在一个特征点周围4*4的方格直方图中，每一个直方图包含8个bin的梯度方向，即得到一个4*4*8=128维的特征向量。除此之外，SIFT算法计算得到的尺寸scale和方向orientation两个信息并没有用上。
2.1.2特征匹配
第二步是匹配和建立track,图像对两两匹配,一般采用欧式距离.有两种方法：
- 粗暴匹配,对所有特征点都穷举计算距离
- 邻近搜索,建立KD树,缩小搜索范围,能提高效率,但也有可能不是最优,所以邻域取值是关键,越大越准确,越大计算量越大
事实上算法中用到一个kd-tree的数据结构去计算最近邻匹配。然后令最近邻的距离为d1，再找到第二近的匹配对点之间距离为d2，如果两个距离d1和d2之比小于一个阈值如0.6，就可以判定为可接受的匹配对。这样子，图像I中的特征点在图像J中至多一个匹配特征点，但是图像J中可能匹配图像I中多个特征点，就会出现多对一的情况，实际上特征点之间应该一一对应。所以还需要一个去除重复特征点匹配对的算法去解决这种多对一的情况。最后如果两个图片之间的特征点匹配数不少于16个即为初选图像对。
当距离小于一定阈值的时候就认为匹配成功,但是误匹配也比较多,需要采取多种手段剔除:
- 如果最近距离与次近距离的比值大于某个阈值,应该剔除
- 对匹配点采用采样一致性算法RANSC八点法计算基础矩阵,剔除不满足基础矩阵的匹配对
然而初选的匹配对可能还是不可靠，需要用几何约束去检测。这个测试是基于事实的，假设一个静止场景，不是所有的匹配特征点在实际场景中是符合物理规律的。那么就需要计算对极几何，F矩阵可以把两张图片之间的像素坐标联系起来，并包含相机的内参信息。每一个符合的匹配对像素坐标都需要满足：
像这种F矩阵计算出有很多噪声数据，需要用RANSAC(随机抽样一致性)算法进行滤波，用8点法来进行RANSACA假设，其中外点个数的阈值应该小于图像长与宽的0.6%。
当所有的两两匹配图像对被确定以后，就可以考虑把多个图像中都出现的共同特征匹配点连接起来，就能形成轨迹了。例如，特征f1 ∈ F (I1)匹配特征f2 ∈ F (I2)，f2匹配特征f3 ∈ F (I3) ,这些特征就可以形成一个轨迹{f1, f2, f3}。然后利用宽度优先搜索BFS去找到每个特征点在所有图像对中的完整轨迹。
一旦符合的轨迹都找到后，就构造图像连接图，包含每个图像的节点，和有共同轨迹的图像边缘。
2.2 Structure from motion
描述摄像机的外参数用到3*3的旋转矩阵R和1*3的平移向量(或者摄像机中心坐标向量)，摄像机的内参数用一个焦距f和两个径向畸变参数k1和k2描述。几何场景提供轨迹中的每个3D点Xj，通过投影方程，一个3D点Xj被投影到摄像机的2D图像平面上。投影误差就是投影点和图像上真实点之间的距离。如下图：
对于n个视角和m个轨迹，投影误差的目标优化方程可以写为:
当摄像机i观察到轨迹j的时候Wij取1，反之取0，||qij - P (Ci, Xj)||就是摄像机i中的轨迹j的投影误差累积和。SFM算法的目标就是找到合适的相机和场景参数去优化这个目标函数，g是采用一个非线性最小二乘的优化方法求解，著名的有光束平差法bundle adjustment.
首先选择合适的初始化图像对，这十分重要，一旦错误的初始化，将会陷入局部最优而使得之后的BA陷入死循环，无法正确求解得到全局最优。具体有两点要求：第一，要有足够多的匹配点；第二，要有足够远的相机中心。
特别的，在这里用到两个图像变换之间的单应性模型来找初始化图像对。如果不能很好的符合单应性模型，说明相机中心还是有一定距离的。同样采用RANSAC方法来降噪，改善匹配的可靠性，尽量选取低的内点百分比，但是至少保证100个匹配内点。
系统采用5点法来估计初始化匹配对的外参，然后轨迹三角化后可以提供初始化的3D点，初始化的两帧图片就可以开始进行第一次bundle adjustment了。在这里用的是稀疏光束平差法sparse bundle adjustment(SBA)。Bundle Adjustment是一个迭代的过程，在一次迭代过后，将所有三维点反向投影到各自相片的像素坐标并分别与初始坐标比对，如果大于某个阈值，则应将其从track中去掉，如果track中已经小于2个了，则整个track也去掉，一直优化到没有点可去为止
最后，不断添加新的摄像机和3D点进行BA。这个过程直到剩下的摄像机观察到的点不超过20为止，说明剩下的摄像机没有足够的点可以添加，BA结束。得到相机估计参数和场景几何信息，即稀疏的3D点云。
目前,sfm开源库主要有:
* OpenMVG http://openmvg.readthedocs.io/en/latest/#
* libMV https://developer.blender.org/tag/libmv/
* VisualSFM http://ccwu.me/vsfm/
* Bundler http://www.cs.cornell.edu/~snavely/bundler/
不过，还有个小型库非常适合学习，用opencv实现的，地址是:
https://github.com/royshil/SfM-Toy-Library