# [原创]深入EPnP算法

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 K ;
求解PnP问题可以得到摄像头的位姿。

## Control Points & Barycentric Coordinates

EPnP算法将参考点的坐标表示为控制点坐标的加权和：
p i w = ∑ j = 1 4 α i j c j w ,   with  ∑ j = 1 4 α i j = 1 \mathbf{p}_i^w = \sum_{j = 1}^4\alpha_{ij}\mathbf{c}_j^w,\ \ \text{with}\ \sum_{j=1}^4\alpha_{ij} = 1

p i c = ∑ j = 1 4 α i j c j c \mathbf{p}_i^c = \sum_{j=1}^4\alpha_{ij}\mathbf{c}_j^c

Jesse’s Comment: 假设摄像头的外参为 [ R t ] \left[\begin{array}{cc} R &amp; \mathbf{t} \end{array}\right] ，那么虚拟控制点 c j w \mathbf{c}_j^w c j c \mathbf{c}_j^c 之间存在关系：
c j c = [ R t ] [ c j w 1 ] \mathbf{c}_j^c = \left[\begin{array}{cc} R &amp; \mathbf{t} \end{array}\right]\left[\begin{array}{c} \mathbf{c}_j^w \\ 1 \end{array}\right]

p i c = [ R t ] [ p i w 1 ] = [ R t ] [ ∑ j = 1 4 α i j c j w 1 ] \mathbf{p}_i^c = \left[\begin{array}{cc} R &amp; \mathbf{t} \end{array}\right]\left[\begin{array}{c}\mathbf{p}_i^w \\ 1 \end{array}\right] = \left[\begin{array}{cc} R &amp; \mathbf{t} \end{array}\right]\left[\begin{array}{c} \sum_{j=1}^4\alpha_{ij}\mathbf{c}_{j}^w \\ 1 \end{array}\right]

p i c = [ R t ] [ ∑ j = 1 4 α i j c j w ∑ j = 1 4 α i j ] = ∑ j = 1 4 α i j [ R t ] [ c j w 1 ] = ∑ j = 1 4 α i j c j c \mathbf{p}_i^c = \left[\begin{array}{cc} R &amp; \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 &amp; \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

p i w = [ x i w y i w z i w ] = [ c 1 w c 2 w c 3 w ] [ α i 1 α i 2 α i 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 &amp; \mathbf{c}_2^w &amp; \mathbf{c}_3^w \end{array}\right]\left[\begin{array}{c} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \end{array}\right]
∑ i = 1 3 α i j = 1 \sum_{i = 1}^3\alpha_{ij} = 1 ，一共是4个方程，而未知数是3个，这是一个超定方程组，只存在最小二乘意义上的解。换句话，在一般情形下，不存在精确满足4个方程的解。按照同样的思路，把4个控制点时的约束“堆砌”在一起：
[ p i w 1 ] = C [ α i 1 α i 2 α i 3 α i 4 ] = [ c 1 w c 2 w c 3 w c 4 w 1 1 1 1 ] [ α i 1 α i 2 α i 3 α i 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 &amp; \mathbf{c}_2^w &amp; \mathbf{c}_3^w &amp; \mathbf{c}_4^w \\ 1 &amp; 1 &amp; 1 &amp; 1 \end{array}\right]\left[\begin{array}{c} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4} \end{array}\right]

p i w = ∑ j = 1 4 α i j c j w ,   with  ∑ j = 1 4 α i j = 1 \mathbf{p}_i^w = \sum_{j = 1}^4\alpha_{ij}\mathbf{c}_j^w,\ \ \text{with}\ \sum_{j=1}^4\alpha_{ij} = 1

[ α i 1 α i 2 α i 3 α i 4 ] = C − 1 [ p i w 1 ] \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 1 w = 1 n ∑ i = 1 n p i w \mathbf{c}_1^w = \frac{1}{n}\sum_{i=1}^n\mathbf{p}_i^w

A = [ p 1 w T − c 1 w T ⋯ p n w T − c 1 w T ] 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 A^TA 的特征值为 λ c , i , i = 1 , 2 , 3 \lambda_{c,i}, i = 1, 2, 3 ，对应的特征向量为 v c , i , i = 1 , 2 , 3 \mathbf{v}_{c,i}, i = 1, 2, 3 ，那么剩余的三个控制点可以按照下面的公式来确定：
c j w = c 1 w + λ c , j − 1 1 2 v c , j − 1 ,   j = 2 , 3 , 4 \mathbf{c}_j^w = \mathbf{c}_1^w + \lambda_{c,j-1}^{\frac{1}{2}}\mathbf{v}_{c,j-1},\ j = 2, 3, 4

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

K K 是摄像头的内参矩阵，可以通过标定获得。 { u i } i = 1 , ⋯ &ThinSpace; , n \{\mathbf{u}_i\}_{i=1,\cdots,n} 是参考点 { p i } i = 1 , ⋯ &ThinSpace; , n \{\mathbf{p}_i\}_{i=1,\cdots,n} 是的2D投影，那么
∀ i ,      w i [ u i 1 ] = K p i c = K ∑ j = 1 4 α i j c j c \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
c j c = [ x j c , y j c , z j c ] T \mathbf{c}_j^c = \left[x_j^c, y_j^c, z_j^c\right]^T （回顾：前文已经提到了 c j c \mathbf{c}_j^c 这些坐标是非齐次坐标）代入上式，而且把 K K 写成焦距 f u , f v f_u, f_v 和光心 ( u c , v c ) \left(u_c, v_c\right) 的形式：
∀ i ,      w i [ u i v i 1 ] = [ f u 0 u c 0 f v v c 0 0 1 ] ∑ j = 1 4 α i j [ x j c y j c z j c ] \forall i,\ \ \ \ w_i\left[\begin{array}{c} u_i \\ v_i \\ 1 \end{array}\right] = \left[\begin{array}{ccc} f_u &amp; 0 &amp; u_c \\ 0 &amp; f_v &amp; v_c \\ 0 &amp; 0 &amp; 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]

∑ j = 1 4 α i j f u x j c + α i j ( u c − u i ) z j c = 0 ∑ j = 1 4 α i j f v y j c + α i j ( v c − v j ) z j c = 0 \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

M x = 0 M\mathbf{x} = \mathbf{0}

x = ∑ i = 1 N β i v i \mathbf{x} = \sum_{i = 1}^N\beta_i\mathbf{v}_i

c i c = ∑ k = 1 N β k v k [ i ] \mathbf{c}_i^c = \sum_{k = 1}^N\beta_k\mathbf{v}_k^{\left[i\right]}

∥ ∑ k = 1 N β k v k [ i ] − ∑ k = 1 N β k v k [ j ] ∥ 2 = ∥ c i w − c j w ∥ 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

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

N = 4 N = 4 的时候，未知数的个数多于方程的个数了。注意到 β a b β c d = β a β b β c β d = β a ′ b ′ β c ′ d ′ \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\} 的一个排列，我们可以减少未知数的个数。举例，如果我们求出了 β 11 \beta_{11} β 12 \beta_{12} β 13 \beta_{13} ，那么我们可以得到 β 23 = β 12 β 13 β 11 \beta_{23} = \frac{\beta_{12}\beta_{13}}{\beta_{11}} 。这样，即使对于 N = 4 N = 4 ，我们也可以求解出 { β i j } i , j = 1 , ⋯ &ThinSpace; , N \{\beta_{ij}\}_{i,j = 1,\cdots,N} 了。

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

## 高斯-牛顿最优化

Error ( β ) = ∑ ( i , j )  s.t.  i &lt; j ( ∥ c i c − c j c ∥ 2 − ∥ c i w − c j w ∥ 2 ) 2 \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

## 计算摄像头的位姿

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

2. 计算3D参考点在摄像头参考坐标系下的坐标：
p i c = ∑ j = 1 4 α i j c j c ,   i = 1 , ⋯ &ThinSpace; , n \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} 的重心 p 0 w \mathbf{p}_0^w 和矩阵 A A :
p 0 w = 1 n ∑ i = 1 n p i w A = [ p 1 w T − p 0 w T ⋯ p n w T − p 0 w T ] \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]

4. 计算 { p i c } i = 1 , ⋯ &ThinSpace; , n \{\mathbf{p}_i^c\}_{i=1,\cdots,n} 的重心 p 0 c \mathbf{p}_0^c 和矩阵 B B :
p 0 c = 1 n ∑ i = 1 n p i c B = [ p 1 c T − p 0 c T ⋯ p n c T − p 0 c T ] \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]

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

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

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

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

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

## 代码

https://github.com/jessecw/EPnP_Eigen

08-13

05-02 1万+
05-17 1249
05-06 2万+
08-14 3146
08-29 1万+
02-16 586
10-09 1537
02-28 241
06-27 4935
01-04 699
06-18 474
10-13 1万+
05-28 1万+
04-21 945