EPnP: Accurate Non-Iterative O(n) Solution to the PnP Problem

前言

EPnP是一种非迭代的求解PnP问题的方法，根据3D地标点和2D特征点之间的匹配求解相机的位姿，本方法的时间复杂度增加是线性的。本方法的核心是根据虚拟的控制点的权重和来表示n个3D点，因此PnP问题转换为估计控制点在相机坐标系下的坐标，需要计算12x12矩阵的特征向量，求解固定数量的四元方程来求解权重。并且闭式解可以通过高斯牛顿法来优化。
所谓PnP问题就是相机的内参以及n对3D地标点和2D特征点之间的匹配关系，以及3D地标点在世界坐标系下的坐标， 求解相机位姿的问题。现有的非迭代方法只能选取小部分的点进行计算，并且对噪声比较敏感，比较慢。
迭代方法可以达到良好的效果，但是需要良好的初值，因此本方法的计算结果可以提供良好的初值，对位姿进行优化，得到更准确的结果。
通常的PnP算法是思想是，首先根据地标点的深度来求解地标点在相机坐标系下的坐标，这样就将3D-2D匹配问题转换为3D-3D匹配问题，然后使用ICP算法，求解相机的位姿，例如P3P，但是该算法只使用了4对匹配点，容易受到误差噪声的影响，当使用更多匹配点的时候，需要求解4次方程，增加了时间复杂度。

详解

首先选取世界坐标系下的四个控制点，通过控制点的线性组合来表示世界坐标系下的地标点坐标。由于对于同一个地标点来说，世界坐标系下控制点的地标点坐标和相机坐标系下控制点的地标点坐标相同，因此获得了相机坐标系地标点下地标点的坐标和匹配特征点坐标之间的匹配关系，通过这个约束可以求出四个控制点在相机坐标系下的坐标，这样就求得了地标点在相机坐标系下的坐标，就将3D-2D问题转换为了3D-3D问题，然后就根据ICP，求出相机的位姿。

选取控制点

需要选取四个控制点，第一个控制点的坐标是4个地标点的质心坐标，然后计算每个地标点的去质心坐标，构成一个矩阵A，对矩阵ATA进行特征值分解，得到三个特征值和三个特征向量，剩余三个控制点的坐标等于质心坐标加上特征向量乘以对应的特征值系数，特征值系数等于根号下特征值除以地标点的个数。因此求得世界坐标系下4个控制点的坐标。

计算世界控制点坐标系下的地标点坐标

在得到世界坐标系下的四个控制点坐标后，可以求解世界坐标系控制点下的地标点坐标。首先获得减去后三个控制点的地标点坐标cp=p_i-c1，然后求解减去第一个控制点的其他三个控制点组成向量的逆向量[cc2 cc3 cc4]^(-1)，然后可以求解地标点在世界坐标系控制点下的坐标[a2 a3 a4]^T = [cc2 cc3 cc4]^(-1) * cp。
对于同一个地标点，它的世界坐标系控制点下的坐标和相机坐标系控制点下的坐标是相同的，所以同时得到了相机坐标系控制点下的地标点坐标。

构造M矩阵

在得到世界坐标系控制点坐标系下的地标点坐标之后，可以构建对应匹配特征点的等式，未知数为相机坐标系下的四个控制点的坐标，已知数为相机坐标系控制点下的地标点坐标和匹配特征点坐标，几何意义是控制点坐标系3D地标点坐标根据相机内参转换值匹配特征点坐标：


化简之后得到，每对3D地标点和2D特征点的匹配，可以构建两个等式约束：

因此，所有的匹配可以构造一个M矩阵：

未知数X为相机坐标系下的四个控制点坐标。

求解

方程的解为M矩阵零奇异值对应右奇异值向量的线性组合，每个有奇异值向量为12x1的列向量。需要将M矩阵进行奇异值分解，也就是求出MTM，然后对MTM进行特征值分解，它的特征值为0的特征向量就是右奇异值向量。

方程解空间的维度N，由于每个匹配点可以提供两个约束方程，有六个匹配点，总共有12个未知数，所以此时解空间的维度N=1。如果相机的焦距逐渐增大，相机模型从透视相机模型转换为正交相机模型，次数零空间的维度N=4，所以N的取值范围是[1, 4]，具体N的取值与焦距以及地标点有关，因此在求解过程中，分别求解每个N对应的解，最后根据重投影误差选取最优解。
由刚体结构不变性可知，不同坐标系下两个点的相对距离是恒定的，所以相机坐标系控制点下两个点的距离等于世界坐标系控制点下两个点的距离，通过这个约束，来求解线性组合的系数。

N=1的情况

这时候未知数β只有一个，所以根据刚体不变性可得：

整理得：

也就是从四个匹配点中任取两个，进行计算

N=2的情况

这时未知数β有两个，因此根据刚体不变性可得：

因此引入中间变量：

N=4的情况

这时：

有四个未知数，因此需要然后化简可得到:


这里的Rou是世界坐标系下控制点任意两两相减组成的向量，中间变量dv为特征向量两两相减的组成的向量，然后根据点成进行组合，得到6x10的矩阵

高斯牛顿优化

在得到初始粗糙解之后，使用高斯牛顿对应粗糙解进行优化，将残差设置为两坐标系下两点距离的差值（理想情况下为0），参数块为β值。

求解位姿

当β值求解完成后，相机坐标系下四个控制点的坐标可以求出，然后就可以求出相机坐标系下所有地标点的坐标，这样就完成了2D-3D的转换，之后就可以使用ICP算法求解位姿。