深入EPnP算法

[原创]深入EPnP算法

本文是Jesse Chen的原创文章。
PnP问题是研究如何从3D-2D匹配对中求解摄像头位姿， EPnP算法是一种非迭代的PnP算法。本文作者用baidu搜索了“EPnP算法”时，能找到的中文介绍不多，而且这些网文并没有深入研究这个算法，找出这个算法的精妙点。因此贴出这篇文章，希望能给大家带来我对EPnP算法的理解。有问题的同学，可以联系754971421@qq.com讨论。

PnP问题的定义

Perspective-n-Point问题(PnP)的已知条件：

• n个世界坐标系中的3D参考点(3D reference points)坐标;
• 与这n个3D点对应的、投影在图像上的2D参考点(2D reference points)坐标;
• 摄像头的内参$K$;
  求解PnP问题可以得到摄像头的位姿。

大多数非迭代的PnP算法会首先求解特征点的深度，以获得特征点在相机坐标系中的3D坐标，而EPnP算法将世界坐标系中的3D坐标表示为一组虚拟的控制点的加权和。对于一般情形，EPnP算法要求控制点的数目为4，且这4个控制点不能共面。因为摄像头的外参未知，这四个控制点在摄像头参考坐标系下的坐标是未知的。而如果能求解出这四个控制点在摄像头参考坐标系下的坐标，我们就可以计算出摄像头的位姿。

Control Points & Barycentric Coordinates

在EPnP论文和本文中，分别用上标${}^w$和${}^c$表示在世界坐标系和摄像头坐标系中的坐标。那么，3D参考点在世界坐标系中的坐标为${\mathbf{p}}_i^w,i=1,\cdots ,n$，在摄像头参考坐标系中的坐标为${\mathbf{p}}_i^c,i=1,\cdots ,n$。4个控制点在世界坐标系中的坐标为${\mathbf{c}}_j^w,j=1,\cdots ,4$，在摄像头参考坐标系中的坐标为${\mathbf{c}}_j^c,j=1,\cdots ,4$。需要指出，在EPnP论文和本文中，${\mathbf{p}}_i^w$, ${\mathbf{c}}_j^w$, ${\mathbf{p}}_i^c$和${\mathbf{c}}_j^c$均非齐次坐标。

EPnP算法将参考点的坐标表示为控制点坐标的加权和：
$${\mathbf{p}}_i^w=\sum _{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^w,\text{with}\sum _{j=1}^4{\alpha }_{ij}=1$$
其中${\alpha }_{ij}$是齐次barycentric坐标。一旦虚拟控制点确定后，且满足4个控制点不共面的前提，${\alpha }_{i,j},j=1,\cdots ,4$是唯一确定的。在摄像头坐标系中，存在同样的加权和关系：
$${\mathbf{p}}_i^c=\sum _{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^c$$

Jesse’s Comment: 假设摄像头的外参为$\left[\begin{array}{cc}R& \mathbf{t}\end{array}\right]$，那么虚拟控制点${\mathbf{c}}_j^w$和${\mathbf{c}}_j^c$之间存在关系：
$${\mathbf{c}}_j^c=\left[\begin{array}{cc}R& \mathbf{t}\end{array}\right]\left[\begin{array}{c}{\mathbf{c}}_j^w\\ 1\end{array}\right]$$
考虑到EPnP算法将参考点坐标表示为控制点坐标的加权和，可以得到：
$${\mathbf{p}}_i^c=\left[\begin{array}{cc}R& \mathbf{t}\end{array}\right]\left[\begin{array}{c}{\mathbf{p}}_i^w\\ 1\end{array}\right]=\left[\begin{array}{cc}R& \mathbf{t}\end{array}\right]\left[\begin{array}{c}{\sum }_{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^w\\ 1\end{array}\right]$$
进一步，
$${\mathbf{p}}_i^c=\left[\begin{array}{cc}R& \mathbf{t}\end{array}\right]\left[\begin{array}{c}{\sum }_{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^w\\ {\sum }_{j=1}^4{\alpha }_{ij}\end{array}\right]=\sum _{j=1}^4{\alpha }_{ij}\left[\begin{array}{cc}R& \mathbf{t}\end{array}\right]\left[\begin{array}{c}{\mathbf{c}}_j^w\\ 1\end{array}\right]=\sum _{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^c$$
在上述推导过程中，用到了EPnP对权重${\alpha }_{ij}$的重要约束条件${\sum }_{j=1}^4{\alpha }_{ij}=1$。如果没有这个约束条件，上述推导将不成立，我们也无法得出${\mathbf{p}}_i^c={\sum }_{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^c$。那么问题来了：在一般的情形下，为什么需要4个控制点？要知道 p i w \mathbf{p}_i^w