位姿估计之PnP算法

        最近实验室学习安排是了解Pnp问题解法，于是就在网上找了各种文章学习，在此进行总结，给我卷！！！

1.什么是PnP问题？

        ​​​​​​PnP(全称Perspective-n-Points)，指3D到2D点对的物体运动定位问题，即已知物体在世界坐标系下的坐标，以及物体在相机的图像平面的像素坐标，求解相机的位姿（六个自由度，位置坐标和三个方向角）。

2.PnP问题的可解性

        知道了PnP问题后，就要讨论n在不同取值下的可解性了。

case1：

        n=1时，也就是特征点只有一个的时候，假设特征点为p1，相机光心为Oc，假设特征点在图像正中间，即p1Oc为相机z轴，此时相机可能在以特征点p1为圆心，半径任意的球上任意一点，这是有无数解的，图示如下：

        相机可能在球面O1、O2、O3上任意一点。

case2:

        n=2时，假设两个特征点分别为p1和p2，相机光心为Oc，已知p1和p2的世界坐标，则可知距离p1p2，又知特征点的像素坐标，以及相机内参，可求出∠p1Ocp2的余弦值（如何求后面会说明），利用小孔成像模型（相似三角形）可求出其中一条边Ocp1=r1，再利用余弦定理求出另一条边Ocp2=r2，然后分别以p1和p2为圆心，r1和r2为半径画球面，两个球面相交的地方就是相机可能的位置，此时仍有无数解，图示如下：

          两球面交界处是一片区域，相机可能在这片区域任意位置。

case3:

        n=3时，在原有的基础上又增加了一个球面，这时相机就位于三个球面的交界处，此时交界处有四个点，即相机位置有四个可能解，其中一个是最优解；

case4:

        n=4时，又增加了一个球面，暂时不考虑特殊情况，则四个球面相交于一点，即相机的位置。但由case3可知，在已知三个特征点的情况下，已经可以求出四个可能解，最优解就是四个解中的一个，而为了找出这个最优解又多找考虑一个特征点，对于计算效率方面是不划算的。基于此，通常实际应用中，我们选择用第四个特征点作为检验点，即分别从求出的四个可能解中计算出相机位姿，然后把第四个点经过重投影后计算误差，误差值最小（实际也不可能完全重合）的即为最优解。

        到此，PnP问题的可解性暂且讨论完毕。

3.PnP问题求解及数学推导

        3.1 DLT(Direct Linear Tranform，直接线性变换)

                设一个空间特征点的世界坐标为Pw(X，Y，Z)，其对应的像素坐标为p(u，v)，列一下当前的条件，已知相机内参K（通过相机标定求得，而且内参不会随着物体运动而变），要求的是旋转矩阵R和平移矩阵t，有世界坐标->像素坐标转化可列如下方程（为了能表示为矩阵相乘的形式，简化运算，都转化为齐次坐标）：           

               

我上面写的这个公式忽略了尺度系数，其实它和内参K一样，后面都会被消去，这里提醒大家注意。

等号两边左乘K逆，同时定义变换矩阵T=[R|t],展开如下：

把K逆用第三行表示，代入前两行化简得到两个约束：

 这里，定义变换矩阵T的行向量：

则上述两个约束方程可化为：

 

未知量是Tt，其它已知，一个特征点提供两个方程， 若有n个特征点，则方程如下：

变换矩阵有12个参数，因此需要至少6对特征匹配点才能求出线性解，同时，若n>6时，方程数大于未知数，方程属于超定方程，可以利用SVD分解使用最小二乘法寻找近似最优解，求解过程暂不列出（有点复杂）参考如下：最小二乘法求解相机位姿。

 

 3.2 P3P

         已知空间中三个特征点，根据小孔成像模型可以简单表示如下： 

                         

        图中A、B、C三点为空间特征点，在成像平面的对应投影点分别为a、b、c，O为相机光心，先讨论一下已知条件，A、B、C三点世界坐标已知，则可求出AB、BC、AC；a、b、c为像素坐标，也可求出ab、ac、bc，除此之外还能求出图中不易看出的∠aOb=α，∠aOc=β，∠bOc=γ的余弦值，至于如何求放到后面，先假设它们已知。

        我们目的是什么呢？相机的位姿，也就是R和t，怎么求？很简单，就是先求出A、B、C三点在相机坐标系下的坐标，然后世界坐标->相机坐标的转换，即经过旋转R和平移t。这里我们可以看出3D-2D的点对运动问题被转化成了3D-3D的刚体变换，之后通过ICP(Iterative Closest Point)迭代最近点算法求解。之后可以看到EPnP求解也是3D->3D。

        从上图中找出