位姿估计之EPnP算法学习笔记

首先给出<视觉slam十四讲>中是如何定义PnP:
PnP(Perspective-n-point )是求解3D到2D点对运动的方法。它描述了当我们知道n个3D空间点以及它们的投影位置时，如何估计相机所在的位姿。

**这里有两个概念比较重要：

1.3D空间点：指的是物体在世界坐标系下的坐标。
2.投影位置： 指的是物体在图像坐标系下的坐标。**

举一个例子：在空间中有一个立方体（8个顶点），这8个点在图像上分别对应着一个投影点。

那么pnp理解起来就比较容易了：
用自己的话说就是：已知物体在世界坐标系下N个点的世界坐标以及这N个坐标在图像上的投影点的坐标，来计算相机位姿的过程就叫做pnp求解。

已知量：N个点的世界坐标、图像坐标、摄像机内参矩阵K
求解量：相机位姿
求解方法：最小化重投影误差
Pnp的求解方法有很多：P3P, DLT, EPnp等

DLT:直接构建一个12个未知数的[R|t]增广矩阵（先不考虑旋转矩阵的自由度只有3），取六个点对，去求解12个未知数（每一个3D点到归一化平面的映射给出两个约束），最后将[R|t]左侧33矩阵块进行QR分解，用一个旋转矩阵去近似（将33矩阵空间投影到SE(3)流形上）。

P3P:P3P方法是通过3对3D/2D匹配点，求解出四种可能的姿态，在OpenCV calib3d模块中有实现，但是对于相机远离3D平面（low parallax）或者视角垂直于3D平面的情况下效果不佳。论文Complete Solution Classification for the Perspective-Three-Point Problem中提到了一种改进的方法，可以消除这种退化的情况。

EPnP:需要N对3D-2D匹配点，是目前最有效的PnP求解方法。
解决思路：通常选取世界坐标下的四个控制点坐标为Cw=[0,0,0,1]T, [1,0,0,1]T,[0,1,0,1]T,[0,0,1,1]T；通过n个3D点在相机平面的投影关系，以及与这四个控制点的权重关系，构建一个12*12方阵，求得其零空间特征向量，可以得到虚拟控制点的相机平面坐标，然后使用POSIT算法即可求出相机位姿。通常在用EPnP求得四对点下的封闭解后，可以将该解作为非线性优化的初值，优化提高精度

EPnP算法流程：

这里先解释一下下面符号的意义：

i=1,2,…,n 表示的是3D参考点在世界坐标系下的坐标；

i=1,2,…,n 表示的是3D参考点投影点的坐标；

j=1,2,3,4 表示的是4个控制点在世界坐标系下的坐标；

j=1,2,3,4 表示的是4个控制点投影点的坐标；

那么对于世界坐标系中的任意3D点都可用四个控制点表示：

其中：是齐次barycentric坐标。
控制点一旦确定，且不共面的话，那么，j=1,2,3,4就是唯一确定的。

同样的对于相机坐标系也有这样的关系：

假设摄像头的外参为：【R, t】,那么虚拟控制点和之间存在：

由于EPnp算法考虑到将参考点坐标表示为控制点坐标的加权和,那么就有：

又因为
，带入上式中就有：

化简得到：

那么就可以得出，任意点P的相机坐标系的值可以由4个控制点表示。

控制点选取参考：https://blog.csdn.net/jessecw79/article/details/82945918

直接求解：ui为空间点pi在图像上的2d投影。K为摄像头的内参矩阵。

带入

并且把K写成焦距和焦点的形式，有：

化简就会得到两个方程组：


如果把n个点串联起来，可以得到一个线性方程组：
Mx = 0
其中：

(表示的是控制点在摄像头坐标系下的坐标(X,Y,Z))
那么x就是一个12*1的向量，x属于M的右零空间：

vi是M的N个0特征值对应的N特征向量，对于第i个控制点有：

Vk是特征向量Vk的第i个3x1的子向量，βi是一个线性组合。
我们可以计算出MTM的特征向量得到Vi,【注意】使用MTM比使用M计算量更少，因为MTM是求解常数复杂度，而M是O(n3)的复杂度，计算MTM的复杂度是O(n)。

还需要求出{βi} i=1,2,…,N。

求解β的策略是：摄像机的外参只是描述的是坐标的变换，不会改变两个坐标之间的距离，无论是在相机坐标系还是世界坐标系中，都有：
即：

这是一个关于{βi} i=1,2,…,N的二次方程。这个方程的特点是没有关于{βi} i=1,2,…,N的一次项，如果将这些二次项βiβj替换为βij，那么该方程就是{βij}，i，j=1，2，…,N的线性方程。对于4个控制点就可以得到6个这么多个方程。
如果不挖掘任何附加条件，N取不同值的时候，新的线性未知数的个数分别为：
N=1, 线性未知数的个数为1;
N=2，线性未知数的个数为3;
N=3，线性未知数的个数为6;
N=4，线性未知数的个数为10;

N=4 的时候，未知数的个数多于方程的个数了。注意到βabβcd=βaβbβcβd=βa′b′βc′d′ ，其中{a′,b′,c′,d′}是{a,b,c,d}的一个排列，我们可以减少未知数的个数。举例，如果我们求出了β11，β12，β13，那么我们可以得到β23=β12β13/β11 。这样，即使对于N=4，我们也可以求解出{βij} i,j=1,⋯,N了。

.

高斯-牛顿最优化：

优化的目标函数：

得到β之后，我们就可以计算摄像头位姿了：

1.计算控制点在摄像头参考坐标系下的坐标：

2.计算3D参考点在摄像头参考坐标系下的坐标：

3.计算的Pw重心和矩阵A:


4.计算的Pc重心和矩阵B":


5.计算H

6.计算H的SVD分解

7.计算位姿中的旋转R:

8.计算位姿中的平移t:

参考：
https://www.jianshu.com/p/b3e9fb2ad0dc
https://so.csdn.net/so/search/s.do?q=%E4%BD%8D%E5%A7%BF&t=blog&u=App_12062011