[原创]深入EPnP算法

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

PnP问题的定义

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

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

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

Control Points & Barycentric Coordinates

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

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

Jesse’s Comment: 假设摄像头的外参为$\left[\begin{array}{cc} R & \mathbf{t} \end{array}\right]$[R​t​]，那么虚拟控制点$\mathbf{c}_j^w$cjw​和$\mathbf{c}_j^c$cjc​之间存在关系：
$\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]$cjc​=[R​t​][cjw​1​]
考虑到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]$pic​=[R​t​][piw​1​]=[R​t​][∑j=14​αij​cjw​1​]
进一步，
$\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$pic​=[R​t​][∑j=14​αij​cjw​∑j=14​αij​​]=j=1∑4​αij​[R​t​][cjw​1​]=j=1∑4​αij​cjc​
在上述推导过程中，用到了EPnP对权重$\alpha_{ij}$αij​的重要约束条件$\sum_{j=1}^4\alpha_{ij} = 1$∑j=14​αij​=1。如果没有这个约束条件，上述推导将不成立，我们也无法得出$\mathbf{p}_i^c = \sum_{j=1}^4\alpha_{ij}\mathbf{c}_j^c$pic​=∑j=14​αij​cjc​。那么问题来了：在一般的情形下，为什么需要4个控制点？要知道$\mathbf{p}_i^w$piw​是非齐次的3D坐标，$\mathbf{p}_i^w\in\mathbb{R}^3$piw​∈R3，为什么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]$piw​=⎣⎡​xiw​yiw​ziw​​⎦⎤​=[c1w​​c2w​​c3w​​]⎣⎡​αi1​αi2​αi3​​⎦⎤​
且$\sum_{i = 1}^3\alpha_{ij} = 1$∑i=13​α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]$[piw​1​]=C⎣⎢⎢⎡​αi1​αi2​αi3​αi4​​⎦⎥⎥⎤​=[c1w​1​c2w​1​c3w​1​c4w​1​]⎣⎢⎢⎡​αi1​αi2​αi3​αi4​​⎦⎥⎥⎤​
显然，$\left[\begin{array}{cc} \mathbf{p}_i^{w^T} & 1\end{array}\right]^T$[piwT​​1​]T和$\left[\begin{array}{cc}\mathbf{c}_j^{w^T} & 1 \end{array}\right]^T$[cjwT​​1​]T都是齐次坐标，而论文中的
$\mathbf{p}_i^w = \sum_{j = 1}^4\alpha_{ij}\mathbf{c}_j^w,\ \ \text{with}\ \sum_{j=1}^4\alpha_{ij} = 1$piw​=j=1∑4​αij​cjw​,  with j=1∑4​αij​=1
本质上就是：3D参考点的齐次坐标是控制点齐次坐标的线性组合。论文中也有一句话：where the $\alpha_{ij}$α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]$⎣⎢⎢⎡​αi1​αi2​αi3​αi4​​⎦⎥⎥⎤​=C−1[piw​1​]

control points的选择

原则上，只要控制点满足$C$C可逆就可以，但是论文中给出了一个具体的控制点确定方法。3D参考点集为$\{\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$c1w​=n1​i=1∑n​piw​
进而得到矩阵：
$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=⎣⎡​p1wT​−c1wT​⋯pnwT​−c1wT​​⎦⎤​
记$A^TA$ATA的特征值为$\lambda_{c,i}, i = 1, 2, 3$λc,i​,i=1,2,3，对应的特征向量为$\mathbf{v}_{c,i}, i = 1, 2, 3$vc,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$cjw​=c1w​+λc,j−121​​vc,j−1​, j=2,3,4

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

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

设$K$K是摄像头的内参矩阵，可以通过标定获得。$\{\mathbf{u}_i\}_{i=1,\cdots,n}${ui​}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$∀i,    wi​[ui​1​]=Kpic​=Kj=1∑4​αij​cjc​
用$\mathbf{c}_j^c = \left[x_j^c, y_j^c, z_j^c\right]^T$cjc​=[xjc​,yjc​,zjc​]T（回顾：前文已经提到了$\mathbf{c}_j^c$cjc​这些坐标是非齐次坐标）代入上式，而且把$K$K写成焦距$f_u, f_v$fu​,fv​和光心$\left(u_c, v_c\right)$(uc​,vc​)的形式：
$\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]$∀i,    wi​⎣⎡​ui​vi​1​⎦⎤​=⎣⎡​fu​00​0fv​0​uc​vc​1​⎦⎤​j=1∑4​αij​⎣⎡​xjc​yjc​zjc​​⎦⎤​
从上式可以得到两个线性方程：
$\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$j=1∑4​αij​fu​xjc​+αij​(uc​−ui​)zjc​=0j=1∑4​αij​fv​yjc​+αij​(vc​−vj​)zjc​=0
把所有$n$n个点串联起来，我们可以得到一个线性方程组：
$M\mathbf{x} = \mathbf{0}$Mx=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$x=[c1cT​, c2cT​, c3cT​, c4cT​]T，$\mathbf{x}$x就是控制点在摄像头坐标系下的坐标，显然这是一个$12\times 1$12×1的向量，且$\mathbf{x}$x在$M$M的右零空间中，或者$\mathbf{x}\in\ker\left(M\right)$x∈ker(M)：
$\mathbf{x} = \sum_{i = 1}^N\beta_i\mathbf{v}_i$x=i=1∑N​βi​vi​
上式中，$\mathbf{v}_i$vi​是$M$M的$N$N个零特征值对应的$N$N特征向量。对于第$i$i个控制点：
$\mathbf{c}_i^c = \sum_{k = 1}^N\beta_k\mathbf{v}_k^{\left[i\right]}$cic​=k=1∑N​βk​vk[i]​
上式中，$\mathbf{v}_k^{\left[i\right]}$vk[i]​是特征向量$\mathbf{v}_k$vk​的第$i$i个$3\times 1$3×1 sub-vector。

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

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

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

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

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

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

高斯-牛顿最优化

优化的目标函数为：
$\text{Error}\left(\boldsymbol{\beta}\right) = \sum_{(i,j)\ \text{s.t.}\ i &lt; j}\left(\left\|\mathbf{c}_i^c - \mathbf{c}_j^c\right\|^2 - \left\|\mathbf{c}_i^w - \mathbf{c}_j^w\right\|^2\right)^2$Error(β)=(i,j) s.t. i<j∑​(∥∥​cic​−cjc​∥∥​2−∥∥​ciw​−cjw​∥∥​2)2
这是一个无约束的非线性最优化问题，简单。

计算摄像头的位姿

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

1. 计算控制点在摄像头参考坐标下的坐标：
   $\mathbf{c}_i^c = \sum_{j=1}^N\beta_k\mathbf{v}_k^{\left[i\right]},\ i = 1, \cdots, 4$cic​=j=1∑N​βk​vk[i]​, i=1,⋯,4

2. 计算3D参考点在摄像头参考坐标系下的坐标：
   $\mathbf{p}_i^c = \sum_{j = 1}^4\alpha_{ij}\mathbf{c}_j^c,\ i = 1,\cdots, n$pic​=j=1∑4​αij​cjc​, i=1,⋯,n

3. 计算$\{\mathbf{p}_i^w\}_{i=1,\cdots,n}${piw​}i=1,⋯,n​的重心$\mathbf{p}_0^w$p0w​和矩阵$A$A:
   $\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]$p0w​=n1​i=1∑n​piw​A=⎣⎡​p1wT​−p0wT​⋯pnwT​−p0wT​​⎦⎤​

4. 计算$\{\mathbf{p}_i^c\}_{i=1,\cdots,n}${pic​}i=1,⋯,n​的重心$\mathbf{p}_0^c$p0c​和矩阵$B$B:
   $\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]$p0c​=n1​i=1∑n​pic​B=⎣⎡​p1cT​−p0cT​⋯pncT​−p0cT​​⎦⎤​

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

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

7. 计算位姿中的旋转$R$R:
   $R = UV^T$R=UVT
   如果$\vert R \vert &lt; 0$∣R∣<0，那么$R\left(2,:\right) = -R\left(2,:\right)$R(2,:)=−R(2,:)。

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

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

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