位姿估计的来龙去脉——内外参，三维重建，Pnp问题

关于相机的参数，大致可以分为内参与外参。对于内参，就是一些焦距，畸变参数等信息，可以根据张定友标定法进行标定。对于外参，它描述了物体到相机的相对运动，由旋转矩阵与平移矩阵构成，作用是将世界坐标系中的点映射到相机坐标系中。平移和旋转共拥有6个自由度，所以其实外参的标定过程就是相机的位姿估计。

数学模型已经建好了，剩下的就是引入一些先验的对应关系然后去求解。模型：世界坐标系到图像坐标系到变换矩阵称为基础矩阵，它与内参和外参有关。先验知识：可以提前知道指定点的三维世界坐标与图像坐标。此外，对于内参估计，外参可以看做是已知的（认为指定相机的位置与角度）；对于外参估计，内参已经估计得到，所以内参此时也可以看做是已知的。

可以看到aachen数据集，其中一项是内参

The intrinsic calibration is defined by the width `w` and height `h` of the image, its focal length `f`, the position of the principal point (`cx` and `cy`), and a radial distortion parameter `r`. 

以那么现在的问题就是如何去找到对应点。因为是在估计位姿，所以此时的相机位置和角度是固定的，所以我们要坐的就是找到三维点（一般以点云表示）与相机成像的2D点之间的对应关系。而直接去找这种对应关系是比较难的：需要知道投影关系才能找到对应关系，而我们的目的就是去求解投影关系。这怎么办呢，可以使用sfm的三维重建模型。通过sfm，我们可以得到三维点与相机/图像的对应关系：三维点的个数是百万级别的，相机/图像的个数是几千级别的，所以同一个三维点会在多个图像中成像（points3D.bin中以image_idx表示），同一个图像中可能包含多个三维点（images.bin文件中以point3D_ids表示，大于0表示valid，valid的数目正好和特征点数目一样）。数据库中的数据每个图像的二维特征点对应三维坐标已知，只需要知道当前在线图像与数据库中的二维图像匹配关系，就可以间接得到三维坐标，就可以去求解。

为什么三维重建可以知道二维坐标点对应的3D坐标呢？核心：特征点匹配，重投影误差。通过特征点匹配我们知道同一三维位置在各个图像中的对应关系，以不同角度的两幅图像为例：

和对应的坐标和深度分别为，可得：.是两个角度的相对旋转和位移矩阵，可以通过对极约束得到。那么可以由Cramer's法则，或者叉乘，或者mid point method求解得到深度。这一步叫做三角定位triangulation，由光心和深度确定了三维空间点的位置。https://www.cnblogs.com/Jessica-jie/p/7730731.html

得到物体和相机的相对位置之后，按照估计出的投影关系将该位置的点投影到图像中，看重投影得到的点和之前的点（第一次投影）位置是否重合。实际上肯定不会完全重合，所以我们可以计算得到重投影误差reprojection error

可以使用Bundle adjustment最小化sum of reprojection error重投影误差和。https://www.pianshen.com/article/49561166266/

到这里，这个问题就被抽象为了PnP问题。PNP(Perspective N Points)问题首先于1981年由Fishler和Bolles提出，类似的问题可以追溯到1841年摄影测量学领域。PNP问题的已知条件就是三维点坐标与其对应的二维特征点，要求的是三维点在相机坐标系下的坐标。那么求解思路就有两个：1.求三维点到光心动距离；2.求世界坐标系到相机坐标系到变换矩阵（旋转和平移）。

第一种的示意图及方程组：

 第二种就是更符合坐标系变换意义的解。

和N的关系。当N≥6时，PNP问题成为经典的DLT(Direct Linear Transformation   直接线性变换)问题，此时(R,t)均可解，因此研究N≥6的PNP问题已没有多大意义。

当N=3,一般使用P3P方法，它利用三角形相似，将问题转换为了3D到3D的位姿估计问题，但是也只能利用三组点对。 

Reference：

1.两种途径https://blog.csdn.net/Zz8474/article/details/82963651

2.https://www.jianshu.com/p/b3e9fb2ad0dc

3.https://blog.csdn.net/luohuiwu/article/details/80722542

4.https://zhuanlan.zhihu.com/p/61742217

5.https://blog.csdn.net/weixin_43206570/article/details/84797361

6.http://slazebni.cs.illinois.edu/spring16/

7.https://blog.csdn.net/honyniu/article/details/51004397