相机参数（焦距）初始化对三维重建过程的影响

背景：焦距初始化逻辑的思考

三维重建过程中，如果做过相机标定，可以直接使用标定参数作为先验参数锁定。如果没有，则需要给一个初始化值，通过BA过程自动优化，但是初始化值怎么给？colmap默认没有先验参数是用像素的长宽max乘以初始化系数（默认值1.2，）得到的像素焦距。比如我的照片是6000*4000，就直接得出一个7200的像素焦距，后续BA过程非线性优化直接拟合，运气好也能跑出不错的结果，但是这就完了吗？好像有优化空间！

如果我没法做相机标定，怎么改进参数初始化的过程？相机中心点就不改了，一般认为在正中央，有偏差我也不能主观给出，所以主要就是改变焦距。焦距有两种，一种是摄影爱好者常说的多少mm，一种是计算机视觉所谓的像素距离，这是两种物理单位，并不能等价，需要转换。

首先，我需要拿到相机的物理焦距（mm），然后要把物理焦距转换为像素焦距（pixel），然后把像素焦距作为初始化参数喂进去，我认为更好的初始化参数能提高优化速度，甚至有可能把一些不好优化的情形重建出来。

如果焦距初始化为零会怎么样：

E矩阵无法得到，因为E矩阵需要的输入是相机坐标系的坐标，需要从图像转换到相机坐标系

 const T f = params[0];
 const T c1 = params[1];
 const T c2 = params[2]; 
 *u = (x - c1) / f;
  *v = (y - c2) / f;

其中，f、c1、c2都属于相机内参，xy是图像坐标，uv是投影到一个Z=1的normalized平面的坐标（但是代表的是相机坐标系，算是一个齐次表达），c1 c2是中心点（假设是正中心,c1=6000/2,c2=4000/2）

例，左上角的(0,0)-(c1,c2)=(-c1,-c2)=(-3000,-2000)，不考虑深度变化，新坐标就是(-3000,-2000)

得到的是像素坐标系下的新表示方法，还是在像素坐标系，也就是图像平面，图像平面距离相机中心（基点）距离是f，要把点投影到距离基点1的平面上，就是

u = (x-c1)/f

v = (y-c2)/f

这里如果f是0，除数是0，投影到相机坐标系会得到inf！！！
这个问题如果不处理，会浪费大量的时间进行Ransac重试，并且E矩阵获得失败，继续迭代F、H矩阵（因为EFH三种矩阵都会求，所以可以向下进行）。

F矩阵的输入是两个图像坐标系的二维坐标点得到（F可以无视内参K，F=K2^-1EK1，所以F的（Ransac）评估可以进行（所以无相机内参时用F比较合适），H的输入同F。

估算相对pose，pose有两种方式获得，一个是E矩阵分解，一个是H矩阵，两种情形：依据H和F的内点多少，H内点特别多，用H，否则用E去分解（这就会产生一个bug）：

1.因为只有F和H，却用E矩阵去获得pose很明显不能获得；

2.用H去获得pose，（因为是用的K2^-1HK1，H也不行），结果一个3d点都没法获得。

对匹配点进行三角化：输入为p1、p2、Rt1、Rt2，投影矩阵1是单位矩阵，投影矩阵2是Rt组合的，因为投影矩阵Rt不对，所以得到的3d点是(0,0,0），不可用。

Eigen::Vector3d TriangulatePoint(const Eigen::Matrix3x4d& proj_matrix1,
                                 const Eigen::Matrix3x4d& proj_matrix2,
                                 const Eigen::Vector2d& point1,
                                 const Eigen::Vector2d& point2) {
  Eigen::Matrix4d A;

  A.row(0) = point1(0) * proj_matrix1.row(2) - proj_matrix1.row(0);
  A.row(1) = point1(1) * proj_matrix1.row(2) - proj_matrix1.row(1);
  A.row(2) = point2(0) * proj_matrix2.row(2) - proj_matrix2.row(0);
  A.row(3) = point2(1) * proj_matrix2.row(2) - proj_matrix2.row(1);

  Eigen::JacobiSVD<Eigen::Matrix4d> svd(A, Eigen::ComputeFullV);

  return svd.matrixV().col(3).hnormalized();
}

本质上，都是图像坐标系的点，通过K1和K2转到相同坐标系下，进行SVD分解，E解法是先通过K变换点，变成了inf，H是先得到图像间的关系，再乘以K去恢复点，越不过K的影响，因为K中焦距是0，所以过程无法进行。

所以焦距完全为零在SFM中是无法进行的，无法获得结果，不是所有的参数以及初始值都能自动优化得到，BA过程在位姿估计之后，位姿没有，也就没有优化。“随便”给个默认初始值也是非常有用的，你的烂数据很可能莫名其妙跑好了，不注意这个细节是不知道的。

对初始零焦距的容错处理：

 做处理，防止除以0（双向投影函数都要改）：将inf改为大数，E矩阵估算和初始RT能得到，但是轮不到优化那一步，因为f是零，3d投影2d得到0，因为重投影误差过大，3d点会都被过滤掉。

// Lift points to normalized plane
    u = (x-c1)/(f+1e-6)
    v = (y-c2)/(f+1e-6)


// Transform to image coordinates
  *x = (f + 1e-6) * *x + c1;
  *y = (f + 1e-6) * *y + c2;

双向投影都对0焦距容错后，卡在了验证伪相机参数，近似0甚至负的焦距得不到0.1的最小要求，所以在增量SFM注册新图片过程中，搜索2D-3D匹配时失败，重建过程无法进行下去。

所以一个近0的焦距初始化时不能正常进行重建的。也等不到优化焦距那一步（BA）。

初始化焦距不对->靠容错得到初始图像对并BA优化得到的初始内参（焦距）不对->基于错误的焦距进行的增量图片2D-3D匹配失败（伪参数验证）->重建失败。

初始化焦距的寻找：

前边已经看到了初始焦距的必要性，colmap默认也给了长宽最大值的1.2倍作为焦距的初始值，以保证流程正常进行，那么如果我想进一步优化~

去哪找参数呢？我的航拍模组是5镜头：我找到了镜头的倾斜角和焦距，正射25mm，倾斜35mm，倾斜角度45°，因为同一帧（5相机同一瞬间拍摄，假设叫同一帧）正射看地面100米，那么倾斜的看到地面其实是100倍的根号2，所以25*1.414=35，这很合理。

ccd也有了：23.5mm*15.6mm

 那么如果我的照片是6000*4000，横纵比1.5倍，注意23.5/15.6也是1.5倍。像素是正方形的。

像元物理尺寸就是

23.5/6000==15.6/4000==0.0039mm/pixel

拿到了像元物理尺寸，拿到了“先验”的不太精确的焦距25mm（正射）和35mm（倾斜），直接相除得到像素焦距：

25mm/0.0039mm/pixel=6410pixel

35mm/0.0039mm/pixel=8974pixel

这两个参数明显会比7200好很多！

根据空三之后最终得到的像素焦距来看，某一数据集，多次运行，结果是6900左右和9200左右，比较近似。

注意：是初始化焦距，不是写死先验焦距常量const！

使用分组参数对5镜头焦距进行初始化：

                if (0==strcmp(folder_name.c_str(),"center"))//center 25mm，其余35mm
                {
                    focal_length = 25/0.0039;
                }
                else {
                    focal_length = 35 / 0.0039;
                }

实际看，得到的最终结果相似，空三所需时间确实快了一些！！！

所以数量级相同的情况下，在ceres和重投影误差的优化框架下，初始焦距是否精确那一“点”对结果影响不大，对速度还是有些影响的。

更细的实验，比如焦距只有100呢？参考前边的分析去试探，可能会有一个没参考意义的临界点，超过某个值就能优化，或者达到某个数量级。摸索具体值意义不大，略过。

-------------------------------------------------------------------------------------------------------------------------------

引用一个像素计算方法：

1、像素焦距与毫米焦距转换
fu = ku * dpx;
fv = kv * dpy;

1. ku、kv分别为摄像机内参矩阵的x(u)、y(v)方向的像素焦距;
2. fu、fv分别为摄像机x(u)、y(v)方向的毫米焦距;
3. dpx、dpy单位为mm/pixel;
4. dpx、dpy含义分别是Effective X(Y) dimension of pixel in frame graber，成像平面水平（垂直）方向像素的有效尺寸。

2、根据CCD尺寸CCDSize 计算 水平和垂直方向的像素间隔，间隔单位为毫米（mm）

1. dpx = dx * Ncx / Nfx;
   dpy = dy;

2. dx = (1/CCDSize * 25.4 * 4 /5)/Picture_cx;
   dy = (1/CCDSize * 25.4 * 4 /5)/Picture_cy;
   注：dx、dy 分别为x(Y) dimension of Camera’s sensor element (in mm)；
   25.4含义为：1inch英寸 = 25.4 mm毫米；
   式子中的4和3分别表示CCD的x和y方向像素比，一般也就是图像的宽高比；
   式子中的Picture_cx、Picture_cy一般就是图像的宽和高。
   3、Ncx为X方向感光但愿的数目（厂家提供），一般等于图像的宽。
   Nfx为摄像机每行采样的像素数目，及图像的X方向尺寸（像素的个数），一般也等于图像的宽。
   CCDSize为CCD尺寸，一般为二分之一，三分之一，五分之一等,摄像机参数，厂家提供。