菜鸟追梦旅行

这个屌丝很懒，什么也没留下！

热门标签

EPNP算法详解

作者：菜鸟追梦旅行 | 2024-05-06 19:37:53

踩

epnp算法

最近的研究内容，需要涉及到相机位姿的计算，然后看网上的资料大多是关于P3P的较多，而对于EPNP的较少，且讲的多为笼统，不利于小白学习。因此本文结合原作者Vincent Lepetit的论文《EPnP: An Accurate O(n) Solution to the PnP Problem》，对该方法进行讲解。但我是属于这个领域的入门小白，仅学了一个月，所以不足之处，欢迎大家指出并交流。

1.了解PNP问题

PNP（Perspective-n-Point）问题又称n点透视问题，它是根据摄影测量原理反算图像拍摄时的相机位姿（包括相机位置和方向），主要手段是通过选定图像与物理对象之间对应的特征点，在给定相机内参的条件下反算相机位姿，目前主要的求解方法有3对点估计位姿的P3P、直接线性变换（DLT）、EPNP、UPnP等等，其中P3P和EPNP在OpenCV中直接提供了相关函数。除此之外，还有很多迭代的求解算法，以及近年来，还有一些学者提出基于深度学习的位姿求解方法，目前这些新算法在应用方面较少。本文主要讲解经典的EPNP算法原理，该方法是非迭代求解算法，在保证较高的求解精度上，还有较快的求解速度，计算复杂度为O(n)。同时该方法的作者说EPNP可以用来给迭代的求解算法提供初始值，以提高其算法稳定性和收敛速度。

EPNP算法的核心思想是：用4个控制点表示3D坐标点，然后将PNP问题简化为估计相机坐标系中4个控制点的坐标，求解方法是将这些未知坐标表示为一个12 × 12 矩阵的特征向量的加权和，并求解一个的常数二次方程来选择正确的权重。由于只针对4个控制点优化，所以速度很快，精度较高。

2. 控制点

选取n个世界坐标系下的3D参考点，表示为piw (i=1,···,n)；相应的4个控制点是表示为ciw (i=1,···,n)；而在相机坐标系中，用上标c表示，如pic和cic。论文中指出：理论上可以任意选取4个控制点（要求不共面），但实践中将参考点的质心作为第一个控制点，而选取的其余控制点，应能构成一个与参考点数据方向一致的基。对此，论文中并没有详细说明，而古月居上微鉴道长的《SLAM学习笔记（十六）快速了解EPnP算法原理》和CSDN上JesseChen79的《深入EPnP算法》给出的解释是如下：

选择第一个控制点的计算式：

进而得到矩阵：

记QTQ的特征值为λc,i，i=1,2,3，对应的特征向量为vc,i，i=1,2,3，那么剩余的3个控制点可以按照下面的公式来确定：

EPNP用4个控制点将每个参考点表示为控制点坐标的加权和：

式中αij是齐次重心坐标，并且是唯一确定的，能够容易估计。这个表示方法可以查看空间坐标点的几何性质进行理解。同样地，在相机坐标系中，存在同样的加权和关系：

3. 控制点的相机坐标

设A是相机的内参矩阵，{ui}i=1,…,n是参考点{Pi}i=1,…,n的2D投影，那么

式中wi是投影参数。然后将控制点在相机坐标系下的坐标cjc=[xjc, yjc, zjc]T，2D坐标ui=[ui,vi]T，以及焦距系数fu，fv和像主点坐标（uc, vc）代入上式，化为：

式中未知参数是4个控制点的12个坐标值{xjc, yjc, zjc}j=1,2,3,4和n个参考点的投影系数{ wi}i=1,…,n。根据上式的最后一行可得：

消去wi，进而可得到以下两个线性方程：

把所有控制点串联起来，我们可以得到一个线性方程组：

式中

是一个由4个控制点的未知坐标值组成的12维向量。M是2n×12的矩阵，由n个参考点代入式8和9构成，且这个矩阵M中αij、fu、fv、uc,、vc均已知。这个线性方程组的解x就是控制点在相机坐标系中的坐标，且x在M的零空间或者核中。按照线性代数中线性方程组的解结构，x可以表示为：

式中vi是矩阵M的N个零特征值对应的右奇异矩阵的列，这可以通过求解方阵MTM（12×12）的特征向量来求解。接下来的关键就是求解{βi}i=1,….,N，N是零特征向量的个数。

理论上，对于透视相机，至少需要6个参考点的图像坐标，才能保证方阵MTM只有1个0特征值，也就是N=1。而对于正交相机（实际不存在），由于改变4个控制点的深度不会影响参考点的投影位置，方阵MTM将有4个0特征值，也就是N=4。论文中通过试验来验证了这个结论，如下图。图中横坐标是方阵MTM的特征值编号，共有12个特征值；纵坐标是特征大小，f是焦距。可以看到：对于小焦距，MTM只有一个零特征值。然而，随着焦距增加并且相机变得更接近正交，所有四个最小特征值都接近零。因此，论文认为MTM的零空间的维度N和相机焦距有关，且在1到4之间变化。论文中还指出N取值还与参考点的配置和噪声有关，但N的取值还是在1到4之间变化。

接下来，论证N取值不同时，求解{βi}i=1,….,N的可行性。首先，我们知道相机的外参描述的只是坐标变换，不会改变控制点之间的距离，因此可得：

根据上述描述，控制点的世界坐标ciw是已知的。并且由上式，可知对于第i个控制点，它对应的解（相机坐标）可以表示为：

式中 ${v_{k}}^{[i]}$ 是特征向量vk的第i个3×1子向量。结合上面两个式子，可得一下关系：

这是一个关于{βk}k=1,….,N的二次方程，这个方程的特点是没有关于{βk}k=1,….,N的一次项。如果将这些二次项βaβb变量替换为βab，那么该方程就变成关于{βab}a,b=1,….,N的线性方程了。对于4个控制点，可以得到 $^{{C_{4}}^{2}}$ =6个这样的方程。

如果不挖掘任何附加条件，N取不同值的时候，新的线性未知数的个数分别为：

N = 1, 线性未知数的个数为1;

N = 2，线性未知数的个数为3;

N = 3，线性未知数的个数为6;

N = 4，线性未知数的个数为10;

可以看到，当N=4的时候，未知数的个数多于方程的个数了。但注意到

式中 $\left \{ a\acute{} ,b\acute{},c\acute{},d\acute{ } \right \}$ 是{a, b, c, d}的任意排列。通过这种方式可以减少未知数的个数。比如，如果已经求出了 $\beta _{11},\beta _{12},\beta _{13}$ ，那么可以得到 $\beta _{23}=\frac{\beta _{12}\beta _{13}}{\beta _{11}}$ 。这样，既是对于N=4，也可以求解出{βab}a,b=1,….,N了。

至此，我们就可以求解出4个控制点在相机坐标系中的坐标了。实际上，EPNP在求解控制点的相机坐标时，不是尝试从{1, 2, 3, 4}中选择 N 的值（如果多个特征值具有相似的大小，这将很容易出错），而是计算 N 的所有四个值的解，并保留产生最小重投影误差的一种。

4. 求解相机位姿

首先计算控制点在相机坐标系下的坐标：

然后计算3D参考点在相机坐标系下的坐标：

现在我们有n个3D参考点在世界坐标系下的坐标piw (i=1,···,n)，也有对应的相机坐标系下的坐标pic (i=1,···,n)，就可以进行3D-3D的相机位姿求解了，进而得到欧式变换的旋转R和位置t。

这个问题可以采用迭代最近点(Iterative Closest Point，ICP）求解，详细求解方法可以参考高翔老师的《SLAM14讲》，在此，不在赘述。

参考文献：

[1] JesseChen79的《深入EPnP算法》

链接：https://blog.csdn.net/jessecw79/article/details/82945918

[2] 3D视觉工坊的《EPnP：一种复杂度为O(N)的求解PnP问题的方法》

链接：EPnP：一种复杂度为O(N)的求解PnP问题的方法-腾讯云开发者社区-腾讯云

[3] Vincent Lepetit, Francesc Moreno-Noguer, Pascal Fua. EPnP: An Accurate O(n) Solution to the PnP Problem.

[4] 流星雨的《EPnP原理与源码详解》

链接：https://zhuanlan.zhihu.com/p/361791835

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/菜鸟追梦旅行/article/detail/545644