SFMedu执行过程

1、作者PPT文档：链接：https://pan.baidu.com/s/1xPcJbAsuEPxbkLei7jtJfg

提取码：akp7

2、代码：链接：https://pan.baidu.com/s/1f5AHab8-4ndgi0RU80BBwQ

提取码：y17j

3、基本公式：sm=K[R|t]M 

 

内容：

1、K为内参，为[f,0,0;0,f,0;0,0,1]

2、F为基础矩阵

   使用Ransac和DLt求得F。

3、E 本质矩阵

         E=k'Fk

 

4、Rt和E的关系

5、Mot 为Rt矩阵，大小为3*4，设第一张图片的相机坐标为世界坐标系的原点。

    Mot矩阵表示第c个相机与o1原点坐标系的关系，如[R2o1|t2o1] ，[R3o1|t3o1]，将o1坐标原点转移到相机3所在的原点世界坐标系的变换矩阵。

6、Str 为o1为原点的建立的三维空间坐标

7、ObsVal为图像中的匹配点坐标，格式为2*88，前44个点属于图1，后者为图2中的坐标

8、ObsIdx 为三维空间点的索引：在简单的两幅图像情况下，例如共44个点，第一幅图像索引为1:44，第二幅对应45:88；在3幅图像的情况下，共有59个点，其中1:44为1和2对应的空间点，剩余15个为2和3对应的空间点，对应关系为Idx（3,59）=118，其中59表示第59个空间点，3表示第三个相机，118表示点的图像坐标存储在ObsVal的第118列中

9、三角测量：

   函数：triangulate.m 中：P=K*[R|t]，sm=PM

   其中，x为一组图像中的匹配点

   Step1，对图像压缩：P=H*P；H=[2/H, 0,  -1;

                                                        0,  2/W, -1;

                                                          0,  0,    1]

   对匹配特征点归一化，u=H（1:2；1:2）*x+H（1:2，3）

   原公式转换为：sHm=HK[R|t]M，u=Hm，由P=HP,此时su=PM

   Step2，系数转换：1)反对称矩阵转换，坐标u^为：[0,X(3),-X(2)；-X(3),0,X(1)；X(2),-X(1),0]

       2）由公式u^PX=0，得4组有效的约束方程，令A=[u1^P1; u2^P2],、

   Step3，SVD分解A，求得系数X为三维空间点的齐次形式，X为4*1

   Step4，分解X，X=T*[Y’,1]，Y的初始值为[0,0,0],T=SVD（X’，0），T的大小为4*4，

           有su=PM=PTM，由Q=PT，有su=QM

   Step5，牛顿法迭代优化Y，计算残差，e=QM/s-u

          计算更新Y

          迭代结束后得，X=T*[Y’，1]

   Step6，迭代计算当前两幅图所有的点，并转换为3维坐标。

 

10、BA

    Step1：由旋转矩阵计算旋转轴和旋转角度：

 

上述公式展开为以下矩阵形式：

求取旋转角度theta和旋转轴（rx，ry，rz）

 

 

 

 

Step2，由旋转角度和旋转轴计算旋转矩阵，将空间点转移到对应图像中计算误差。

Step3，使用LM算法迭代，误差：图像投影后的坐标损失。

       参数迭代：三维坐标3*N，旋转轴和角度（rx，ry，rz），三维平移坐标（Tx，Ty，Tz）

 

11、融合点云

Step1，以第一幅图片为中心，若第二幅图片为第三幅和第一幅之间的过渡，则满足:

       Sm1=K[R1o1|t1o1]X，

       Sm2=K[R2o1|t2o1]X  [R2o1|t2o1]为2与1的对应guanxi，X为由第一幅图片与第二幅图片得到的空间点，世界坐标系的原点为o1，由两个相机的旋转和平移矩阵确定。

       Sm3=K[R2o2|t2o2]X

      Sm4=K[R3o2|t3o2]X

      A为相机1和2的对应关系，B为相机2和相机3之间的关系

      代码：concatRts(invRt(A.Mot(:,:,A.frames==2)), B.Mot(:,:,B.frames==2))

           GraphB.Str = transformPtsByRt(GraphB.Str, RtBW2AW);

      其中，X1=[R2o1|t2o1]-1*[R2o2|t2o2]*X3， 将o2世界坐标系原点转换至o1世界坐标系原点。X3是以相机坐标系为o2原点估计得到的世界坐标系中的点。[R2o2|t2o2]*X3将X3从原先o2为原点的坐标系转移到以相机2对应的相机中心为原点的坐标系，在通过[R2o1|t2o1]-1将坐标原点转移到o1中。

其等价形式为：

        [R2o1|t2o1]*X1= [R2o2|t2o2]*X3

Step 2，更新旋转和平移矩阵：

     代码：concatenateRts(GraphB.Mot(:,:,i), inverseRt(RtBW2AW))

       [R2o1|t2o1]-1*[R2o2|t2o2]，将o2世界坐标系原点转换至o1世界坐标系原点，过渡图片为图片2，

       RtBW2AW=[R2o1|t2o1]-1*[R2o2|t2o2]

     更新后为：[R2o2|t2o2]*[R2o2|t2o2]-1*[R2o1|t2o1]= [R2o1|t2o1]，

    将旋转、平移矩阵更新成[R2o1|t2o1]。

    以及，图像3转换为：

          [R3o1|t3o1]=[R3o2|t3o2]*[R2o2|t2o2]-1*[R2o1|t2o1]，

     其中， [R3o1|t3o1]为将o1坐标原点转移到相机3所在的原点世界坐标系的变换矩阵。

在后续结算中，以图片3为过渡图像,则有

      RtBW2AW=[R3o1|t3o1]-1*[R3o3|t3o3]

     其中，[R3o1|t3o1]-1是以第三个相机的原点为坐标系，转移到以o1为原点的坐标系。

     Step 3，找出已有点和新添加点

    已有点通过intersect函数求取交集，新添加点通过setdiff函数求差集，上述两者的数目总和等于图片中去重后的匹配点数目。

    在融合后的数据中添加公共点的匹配对，并在融合后的共有数据ObsIdx

的相应标签下，添加新的匹配对。例如，第2幅图像中和第三幅图像有公共点，将第三幅图像的公共点的位置与第二幅图像相对应。

    在新增点的数据中，不仅需要添加对应关系，也需要添加转换到o1为坐标原点的点云数据。

   Step 4，三角测量

    在之前的三角测量部分，只考虑两幅图像之间的单应，在此处考虑到一个点对应多幅（ncamera>2）图像，使用原理不变，但方程增多，结果也更精确。

Step 5，BA

    针对相机c从1到nCam来说，对所有的空间点Str，找出在相机c中存在的点，并将其映射至相机c所在的图像空间中，计算重投影误差。

    

 

最终将nCam个相机中的误差汇总。使用LM优化算法，优化的参数为空间位置和相机的旋转平移参数，参数量为：3*Npoints+nCam*3*2。

matlab代码：

[vec,resnorm,residuals,exitflag] = lsqnonlin(@(x) reprojectionResidual(graph.ObsIdx,graph.ObsVal,px,py,f,x), [Mot(:); Str(:)],[],[],options);

Step 6，去离群点

Step 7， 再次BA

12、得到最终结果