深入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个控制点？要知道${\mathbf{p}}_i^w$是非齐次的3D坐标，${\mathbf{p}}_i^w\in {\mathbb{R}}^3$，为什么3个控制点就不可以呢？

假设3个控制点满足要求，那么
$${\mathbf{p}}_i^w=\left[\begin{array}{c}{x}_i^w\\ y_i^w\\ z_i^w\end{array}\right]=\left[\begin{array}{ccc}{\mathbf{c}}_1^w& {\mathbf{c}}_2^w& {\mathbf{c}}_3^w\end{array}\right]\left[\begin{array}{c}{\alpha }_{i1}\\ {\alpha }_{i2}\\ {\alpha }_{i3}\end{array}\right]$$
且${\sum }_{i=1}^3{\alpha }_{ij}=1$，一共是4个方程，而未知数是3个，这是一个超定方程组，只存在最小二乘意义上的解。换句话，在一般情形下，不存在精确满足4个方程的解。按照同样的思路，把4个控制点时的约束“堆砌”在一起：
$$\left[\begin{array}{c}{\mathbf{p}}_i^w\\ 1\end{array}\right]=C\left[\begin{array}{c}{\alpha }_{i1}\\ {\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]=\left[\begin{array}{cccc}{\mathbf{c}}_1^w& {\mathbf{c}}_2^w& {\mathbf{c}}_3^w& {\mathbf{c}}_4^w\\ 1& 1& 1& 1\end{array}\right]\left[\begin{array}{c}{\alpha }_{i1}\\ {\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]$$
显然，${\left[\begin{array}{cc}{\mathbf{p}}_i^{w^T}& 1\end{array}\right]}^T$和${\left[\begin{array}{cc}{\mathbf{c}}_j^{w^T}& 1\end{array}\right]}^T$都是齐次坐标，而论文中的
$${\mathbf{p}}_i^w=\sum _{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^w,\text{with}\sum _{j=1}^4{\alpha }_{ij}=1$$
本质上就是：3D参考点的齐次坐标是控制点齐次坐标的线性组合。论文中也有一句话：where the ${\alpha }_{ij}$ are homogenous barycentric coordinates。从上述分析过程中，我们也可以得到barycentric coodinates的计算方法：
$$\left[\begin{array}{c}{\alpha }_{i1}\\ {\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]=C^{-1}\left[\begin{array}{c}{\mathbf{p}}_i^w\\ 1\end{array}\right]$$

control points的选择

原则上，只要控制点满足$C$可逆就可以，但是论文中给出了一个具体的控制点确定方法。3D参考点集为 { p i w , i = 1 , ⋯   , n } \{\mathbf{p}_i^w, i = 1,\cdots,n\} {piw​,i=1,⋯,n}，选择3D参考点的重心为第一个控制点：
$${\mathbf{c}}_1^w=\frac{1}{n}\sum _{i=1}^n{\mathbf{p}}_i^w$$
进而得到矩阵：
$$A=\left[\begin{array}{c}{\mathbf{p}}_1^{w^T}-{\mathbf{c}}_1^{w^T}\\ \cdots \\ {\mathbf{p}}_n^{w^T}-{\mathbf{c}}_1^{w^T}\end{array}\right]$$
记$A^{T}A$的特征值为${\lambda }_{c,i},i=1,2,3$，对应的特征向量为${\mathbf{v}}_{c,i},i=1,2,3$，那么剩余的三个控制点可以按照下面的公式来确定：
$${\mathbf{c}}_j^w={\mathbf{c}}_1^w+{\lambda }_{c,j-1}^{\frac{1}{2}}{\mathbf{v}}_{c,j-1},j=2,3,4$$

以上两图都是EPnP算法控制点的选择实例，蓝色的是3D reference points，红色的是4个控制点，参考坐标系为世界坐标系。

求解控制点在摄像机坐标下的坐标

设$K$是摄像头的内参矩阵，可以通过标定获得。 { u i } i = 1 , ⋯   , n \{\mathbf{u}_i\}_{i=1,\cdots,n} {ui​}i=1,⋯,n​是参考点 { p i } i = 1 , ⋯   , n \{\mathbf{p}_i\}_{i=1,\cdots,n} {pi​}i=1,⋯,n​是的2D投影，那么
$$\forall i,w_i\left[\begin{array}{c}{\mathbf{u}}_i\\ 1\end{array}\right]=K{\mathbf{p}}_i^c=K\sum _{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^c$$
用${\mathbf{c}}_j^c={\left[x_j^c,y_j^c,z_j^c\right]}^T$（回顾：前文已经提到了${\mathbf{c}}_j^c$这些坐标是非齐次坐标）代入上式，而且把$K$写成焦距$f_u,f_v$和光心$\left(u_c,v_c\right)$的形式：
$$\forall i,w_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_u& 0& u_c\\ 0& f_v& v_c\\ 0& 0& 1\end{array}\right]\sum _{j=1}^4{\alpha }_{ij}\left[\begin{array}{c}{x}_j^c\\ y_j^c\\ z_j^c\end{array}\right]$$
从上式可以得到两个线性方程：
$$\begin{gathered} \sum _{j=1}^4{\alpha }_{ij}{f}_u{x}_j^c+{\alpha }_{ij}\left(u_c-u_i\right)z_j^c=0 \\ \sum _{j=1}^4{\alpha }_{ij}{f}_v{y}_j^c+{\alpha }_{ij}\left(v_c-v_j\right)z_j^c=0 \end{gathered}$$
把所有$n$个点串联起来，我们可以得到一个线性方程组：
$$M\mathbf{x}=0$$
其中$\mathbf{x}={\left[{\mathbf{c}}_1^{c^T},{\mathbf{c}}_2^{c^T},{\mathbf{c}}_3^{c^T},{\mathbf{c}}_4^{c^T}\right]}^T$，$\mathbf{x}$就是控制点在摄像头坐标系下的坐标，显然这是一个$12\times 1$的向量，且$\mathbf{x}$在$M$的右零空间中，或者$\mathbf{x}\in \ker \left(M\right)$：
$$\mathbf{x}=\sum _{i=1}^N{\beta }_i{\mathbf{v}}_i$$
上式中，${\mathbf{v}}_i$是$M$的$N$个零特征值对应的$N$特征向量。对于第$i$个控制点：
$${\mathbf{c}}_i^c=\sum _{k=1}^N{\beta }_k{\mathbf{v}}_k^{\left[i\right]}$$
上式中，${\mathbf{v}}_k^{\left[i\right]}$是特征向量${\mathbf{v}}_k$的第$i$个$3\times 1$ sub-vector。

我们可以计算$M^{T}M$的特征向量得到${\mathbf{v}}_i$，但还需要求出 { β i } i = 1 , ⋯   , N \{\beta_{i}\}_{i=1,\cdots,N} {βi​}i=1,⋯,N​。

根据仿真发现$M^{T}M$为0的特征值的个数和摄像头的焦距有关（注意：是和焦距有关，而不是和参考点的位置有关！），EPnP算法建议只考虑$N=1,2,3,4$的情况。摄像头的外参描述的只是坐标变换，不会改变控制点之间的距离，${\Vert {\mathbf{c}}_i^c-{\mathbf{c}}_j^c\Vert }^2={\Vert {\mathbf{c}}_i^w-{\mathbf{c}}_j^w\Vert }^2$，
$${\Vert \sum _{k=1}^N{\beta }_k{\mathbf{v}}_k^{\left[i\right]}-\sum _{k=1}^N{\beta }_k{\mathbf{v}}_k^{\left[j\right]}\Vert }^2={\Vert {\mathbf{c}}_i^w-{\mathbf{c}}_j^w\Vert }^2$$
这是一个关于 { β k } k = 1 , ⋯   , N \{\beta_k\}_{k=1,\cdots,N} {βk​}k=1,⋯,N​的二次方程，这个方程的特点是没有关于 { β k } k = 1 , ⋯   , N \{\beta_k\}_{k=1,\cdots,N} {βk​}k=1,⋯,N​的一次项。如果将这些二次项${\beta }_i{\beta }_j$变量替换为${\beta }_{ij}$，那么该方程是 { β i j } i , j = 1 , ⋯   , N \left\{\beta_{ij}\right\}_{i,j = 1,\cdots,N} {βij​}i,j=1,⋯,N​的线性方程了。对于4个控制点，可以得到$C_4^2=6$个这样的方程。

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

• $N=1$, 线性未知数的个数为1;
• $N=2$，线性未知数的个数为3;
• $N=3$，线性未知数的个数为6;
• $N=4$，线性未知数的个数为10;

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

Jesse’s Comment: 减少变量的方法不难，但是很有趣！

高斯-牛顿最优化

优化的目标函数为：
$$\text{Error}\left(\mathbf{\beta }\right)=\sum _{(i,j)\text{s.t.}i<j}{\left({\Vert {\mathbf{c}}_i^c-{\mathbf{c}}_j^c\Vert }^2-{\Vert {\mathbf{c}}_i^w-{\mathbf{c}}_j^w\Vert }^2\right)}^2$$
这是一个无约束的非线性最优化问题，简单。

计算摄像头的位姿

本文中摄像头的位姿和外参并不加以区分。EPnP算法中的摄像头的位姿计算方法如下：

1. 计算控制点在摄像头参考坐标下的坐标：
   $${\mathbf{c}}_i^c=\sum _{j=1}^N{\beta }_k{\mathbf{v}}_k^{\left[i\right]},i=1,\cdots ,4$$

2. 计算3D参考点在摄像头参考坐标系下的坐标：
   $${\mathbf{p}}_i^c=\sum _{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^c,i=1,\cdots ,n$$

3. 计算 { p i w } i = 1 , ⋯ &ThinSpace; , n \{\mathbf{p}_i^w\}_{i=1,\cdots,n} {piw​}i=1,⋯,n​的重心${\mathbf{p}}_0^w$和矩阵$A$:
   $$\begin{gathered} {\mathbf{p}}_0^w=\frac{1}{n}\sum _{i=1}^n{\mathbf{p}}_i^w \\ A=\left[\begin{array}{c}{\mathbf{p}}_1^{w^T}-{\mathbf{p}}_0^{w^T}\\ \cdots \\ {\mathbf{p}}_n^{w^T}-{\mathbf{p}}_0^{w^T}\end{array}\right] \end{gathered}$$

4. 计算 { p i c } i = 1 , ⋯ &ThinSpace; , n \{\mathbf{p}_i^c\}_{i=1,\cdots,n} {pic​}i=1,⋯,n​的重心${\mathbf{p}}_0^c$和矩阵$B$:
   $$\begin{gathered} {\mathbf{p}}_0^c=\frac{1}{n}\sum _{i=1}^n{\mathbf{p}}_i^c \\ B=\left[\begin{array}{c}{\mathbf{p}}_1^{c^T}-{\mathbf{p}}_0^{c^T}\\ \cdots \\ {\mathbf{p}}_n^{c^T}-{\mathbf{p}}_0^{c^T}\end{array}\right] \end{gathered}$$

5. 计算$H$:
   $$H=B^{T}A$$

6. 计算$H$的SVD分解：
   $$H=U\Sigma V^T$$

7. 计算位姿中的旋转$R$:
   $$R=U{V}^T$$
   如果$|R|<0$，那么$R\left(2,:\right)=-R\left(2,:\right)$。

8. 计算位姿中的平移$\mathbf{t}$:
   $$\mathbf{t}={\mathbf{p}}_0^c-R{\mathbf{p}}_0^w$$

Jesse’s Comment: baidu能搜到的另一篇关于EPnP算法的博客在计算$R$的那步有错误。

本文中的所有内容已由博文作者编程确认过正确性。

代码

在参考EPnP作者的代码基础上，博文作者在撰写这篇文章前也用Eigen重写了EPnP算法的代码。代码已经上传至GitHub:
https://github.com/jessecw/EPnP_Eigen