SfM算法

参考
https://blog.csdn.net/kokerf/article/details/72630863
https://blog.csdn.net/qq_42399848/article/details/89348740
https://blog.csdn.net/lpj822/article/details/82716971
《基于无序图像集的运动恢复结构研究与实现》
Structure from Motion可以认为一个相机在运动中，重建周围的环境，这里要求相机是运动的，环境是静止的
1.特征点提取与描述
2.本质矩阵E(essential)估计
2.1 对极几何介绍

P是场景中的点，他和相机光心O1O2构成极平面，向量t从O1指向O2表示相机间的平移量，O1O2为基线。E1是光心O2在相机1中的像点，E2是光心O1在相机2中的像点，E1，E2称为极点。P1P2是P在相机O1和相机O2中的像点，P1E1,P2E2称为极线，极线约束指一个点的像如果在极线e1上，那么他肯定也在极线e2上。
2.2本质矩阵推导
假设以O1为世界坐标系，则O1的投影矩阵为T1=(E,0)，其中E是单位阵，假设O2相对于O1有一个变换（R^-1,-t），那么O2的投影矩阵T2=(R,t)，其中R是旋转矩阵,t是平移向量。
假设P在O1世界坐标系下的坐标为X1，在O2世界坐标系下的坐标为X2
所以X2=RX1+t，X2-t=RX1,在O2中，X2-t，X2和t共面，所以(X2-t)的转置与(t叉乘X2)的内积为0。将叉乘线性化
（X2-t）转置St2X2=0,其中St2=[0,-t3,t2;t3,0,-t1;-t2,t1,0]是t中元素构成的反对称阵。

其中E=St2R，其秩为2，因为反对称阵的秩为2.
本质矩阵描述的是同一个三维点在不同的相机坐标系下的坐标的对应关系
3.基础矩阵F(fundamental)估计
而图像获得的是二维点，E表达的是三维点的关系，从相机坐标系到图像坐标系，中间差一个内参矩阵，所以基础矩阵为
x2转置K2EK1x1=0
其中K2EK1是基础矩阵，表征了不同相机坐标系下的一点两张图像像素之间的关系，他的秩是2
4.根据配准关系，用8点法求解F，因为相机标定后就得到了K1,K2这样也就可以求出E。



基础矩阵中有9个元素，由于尺度是任意的，所以只需要8个方程。因为算法中需要8个对应点来算基础矩阵F，所以该算法叫做八点法。
这时候就需要求最小二乘解，这里就可以用SVD来求解，ff的解就是系数矩阵A最小奇异值对应的奇异向量，也就是A奇异值分解后A=UDVT，A=UDVT中矩阵V的最后一列矢量，这是在解矢量f取∥Af∥最小的解。
由于基本矩阵有一个重要的特点就是奇异性，F矩阵的秩是2。如果基本矩阵是非奇异的，那么所计算的对极线将不重合。所以在上述算法解得基本矩阵后，会增加一个奇异性约束。最简便的方法就是修正上述算法中求得的矩阵F。设最终的解为F′，令detF′=0下求得Frobenius范数（二范数）‖F−F′‖最小的F′。这种方法的实现还是使用了SVD分解，若F=UDVT，此时的对角矩阵D=diag(r,s,t)，满足r≥s≥tr≥s≥t，则F′=Udiag(r,s,0)最小化范数‖F−F′‖，也就是最终的解

得到基本矩阵之后，由于相机经过标定，已经确定了K1和K2，那么就可以求本质矩阵E
4.本质矩阵分解
E矩阵SVD分解得到R和T，这里具体的分解步骤没有深入学习
SVD分解
存在4种可能的解，寻找正确的解
检查旋转矩阵R的正确性
R的行列式必须为1或者-1
5.三维点计算
已经知道了两个相机之间的变换矩阵(R和T)，还有每一对匹配点的坐标，通过这些已知信息还原匹配点在空间当中的坐标，根据公式

这里s2还是未知数，用s2对等式两边做叉乘可以消去s2

用SVD求X左边矩阵的零空间，再将最后一个元素归一化到1，即可求得X。其几何意义相当于分别从两个相机的光心作过x1和x2的延长线，延长线的焦点即为方程的解，将三维点三角化并重映射到摄像机得到二维点，计算与最初二维点之间的距离，说明三角化误差。
6.计算第三个摄像机到到世界坐标系的变换矩阵(R和T）
用新加入的图像和相邻图像进行特征匹配，然后计算E，但这是计算的是相对变换，比如相机三到相机二的变换，而我们需要的是相机三到相机一的变换。有人说，既然知道相机二到相机一的变换，又知道相机到三到相机二的变换，不就能求出相机三到相机一的变换吗？实际上，通过这种方式，你只能求出相机三到相机一的旋转变换（旋转矩阵R），而他们之间的位移向量T，是无法求出的。这是因为上面两个函数求出的位移向量，都是单位向量，丢失了相机之间位移的比例关系。
对于输入的第三幅图片，计算第三幅图片与第二幅图片的匹配点，这些匹配点中，肯定有一部分也是图像二与图像一之间的匹配点，也就是说，这些匹配点中有一部分的空间坐标是已知的，同时又知道这些点在第三幅图像中的像素坐标，即可计算变换矩阵。
透视N点法（PNP）三角化更多的点并查看这些点是如何融入存在的几何结构中，然后进行求解，迭代最近点法（ICP）
得到第三个摄像机的变换矩阵后，就可以计算匹配点的在空间中的坐标，得到三维点云，将新得到的三维点云与之前计算得三维点云进行融合（已经存在的空间点，就没必要再添加了，只添加在图像二和三之间匹配，但在图像一和图像三中没有匹配的点）。然后循环迭代
7.重构的细化与优化
原因：随着图像的不断增加，误差会不断累积，最后误差过大以至于完全偏离重建的目标。
目的：三维点云的位置和摄像机的位置优化
算法：
光束法平差（Bundle Adjustment）
BA本质上是一个非线性优化算法
简单稀疏光束调整（SSBA）
Ceres Solver
Google的一个开源项目，用来求解非线性最小二次问题的库，因此可以用来求解BA。
Ceres Solver专为求解此类问题进行了大量的优化，有很高的效率，尤其在大规模问题上，其优势更加明显。
BA是迭代的过程，在一次迭代过后，将所有三维点反向投影到图像中，计算投影点和初始坐标对比，如果大于一定阈值就将他从track中去掉，直到优化到没有可用的点为止