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

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

Control Points & Barycentric Coordinates

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

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 piw=j=14αijcjw,  with j=14αij=1
其中 α i j \alpha_{ij} αij是齐次barycentric坐标。一旦虚拟控制点确定后,且满足4个控制点不共面的前提, α i , j , j = 1 , ⋯   , 4 \alpha_{i,j},j = 1,\cdots,4 αi,j,j=1,,4是唯一确定的。在摄像头坐标系中,存在同样的加权和关系:
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 pic=j=14αijcjc

Jesse’s Comment: 假设摄像头的外参为 [ R t ] \left[\begin{array}{cc} R & \mathbf{t} \end{array}\right] [Rt],那么虚拟控制点 c j w \mathbf{c}_j^w cjw c j c \mathbf{c}_j^c cjc之间存在关系:
c j c = [ R t ] [ c j w 1 ] \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=[Rt][cjw1]
考虑到EPnP算法将参考点坐标表示为控制点坐标的加权和,可以得到:
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 & \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=[Rt][piw1]=[Rt][j=14αijcjw1]
进一步,
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 & \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=[Rt][j=14αijcjwj=14αij]=j=14αij[Rt][cjw1]=j=14αijcjc
在上述推导过程中,用到了EPnP对权重 α i j \alpha_{ij} αij的重要约束条件 ∑ j = 1 4 α i j = 1 \sum_{j=1}^4\alpha_{ij} = 1 j=14α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 pic=j=14αijcjc。那么问题来了:在一般的情形下,为什么需要4个控制点?要知道 p i w \mathbf{p}_i^w piw是非齐次的3D坐标, p i w ∈ R 3 \mathbf{p}_i^w\in\mathbb{R}^3 piwR3,为什么3个控制点就不可以呢?

假设3个控制点满足要求,那么
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 & \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=xiwyiwziw=[c1wc2wc3w]αi1αi2αi3
∑ i = 1 3 α i j = 1 \sum_{i = 1}^3\alpha_{ij} = 1 i=13α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 & \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] [piw1]=Cαi1αi2αi3αi4=[c1w1c2w1c3w1c4w1]αi1αi2αi3αi4
显然, [ p i w T 1 ] T \left[\begin{array}{cc} \mathbf{p}_i^{w^T} & 1\end{array}\right]^T [piwT1]T [ c j w T 1 ] T \left[\begin{array}{cc}\mathbf{c}_j^{w^T} & 1 \end{array}\right]^T [cjwT1]T都是齐次坐标,而论文中的
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 piw=j=14αijcjw,  with j=14αij=1
本质上就是:3D参考点的齐次坐标是控制点齐次坐标的线性组合。论文中也有一句话:where the α i j \alpha_{ij} αij are homogenous barycentric coordinates。从上述分析过程中,我们也可以得到barycentric coodinates的计算方法:
[ α 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] αi1αi2αi3αi4=C1[piw1]

control points的选择

原则上,只要控制点满足 C C C可逆就可以,但是论文中给出了一个具体的控制点确定方法。3D参考点集为 { p i w , i = 1 , ⋯   , n } \{\mathbf{p}_i^w, i = 1,\cdots,n\} {piw,i=1,,n},选择3D参考点的重心为第一个控制点:
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 c1w=n1i=1npiw
进而得到矩阵:
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=p1wTc1wTpnwTc1wT
A T A A^TA ATA的特征值为 λ c , i , i = 1 , 2 , 3 \lambda_{c,i}, i = 1, 2, 3 λc,i,i=1,2,3,对应的特征向量为 v c , i , i = 1 , 2 , 3 \mathbf{v}_{c,i}, i = 1, 2, 3 vc,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 cjw=c1w+λc,j121vc,j1, j=2,3,4

在这里插入图片描述
在这里插入图片描述
以上两图都是EPnP算法控制点的选择实例,蓝色的是3D reference points,红色的是4个控制点,参考坐标系为世界坐标系。

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

K K 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投影,那么
∀ 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 i,    wi[ui1]=Kpic=Kj=14αijcjc
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 cjc=[xjc,yjc,zjc]T(回顾:前文已经提到了 c j c \mathbf{c}_j^c cjc这些坐标是非齐次坐标)代入上式,而且把 K K K写成焦距 f u , f v f_u, f_v fu,fv和光心 ( u c , v c ) \left(u_c, v_c\right) (uc,vc)的形式:
∀ 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 & 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,    wiuivi1=fu000fv0ucvc1j=14αijxjcyjczjc
从上式可以得到两个线性方程:
∑ 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 j=14αijfuxjc+αij(ucui)zjc=0j=14αijfvyjc+αij(vcvj)zjc=0
把所有 n n n个点串联起来,我们可以得到一个线性方程组:
M x = 0 M\mathbf{x} = \mathbf{0} Mx=0
其中 x = [ c 1 c T ,   c 2 c T ,   c 3 c T ,   c 4 c T ] T \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 x \mathbf{x} x就是控制点在摄像头坐标系下的坐标,显然这是一个 12 × 1 12\times 1 12×1的向量,且 x \mathbf{x} x M M M的右零空间中,或者 x ∈ ker ⁡ ( M ) \mathbf{x}\in\ker\left(M\right) xker(M)
x = ∑ i = 1 N β i v i \mathbf{x} = \sum_{i = 1}^N\beta_i\mathbf{v}_i x=i=1Nβivi
上式中, v i \mathbf{v}_i vi M M M N N N个零特征值对应的 N N N特征向量。对于第 i i 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]} cic=k=1Nβkvk[i]
上式中, v k [ i ] \mathbf{v}_k^{\left[i\right]} vk[i]是特征向量 v k \mathbf{v}_k vk的第 i i i 3 × 1 3\times 1 3×1 sub-vector。

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

根据仿真发现 M T M M^T M MTM为0的特征值的个数和摄像头的焦距有关(注意:是和焦距有关,而不是和参考点的位置有关!),EPnP算法建议只考虑 N = 1 , 2 , 3 , 4 N=1,2,3,4 N=1,2,3,4的情况。摄像头的外参描述的只是坐标变换,不会改变控制点之间的距离, ∥ c i c − c j c ∥ 2 = ∥ c i w − c j w ∥ 2 \left\|\mathbf{c}_i^c - \mathbf{c}_j^c\right\|^2 = \left\|\mathbf{c}_i^w - \mathbf{c}_j^w\right\|^2 ciccjc2=ciwcjw2
∥ ∑ 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 k=1Nβkvk[i]k=1Nβkvk[j]2=ciwcjw2
这是一个关于 { β 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的一次项。如果将这些二次项 β i β j \beta_i\beta_j βiβj变量替换为 β i j \beta_{ij} β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 C_4^2 = 6 C42=6个这样的方程。

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

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

N = 4 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}} βabβcd=βaβbβcβd=βabβcd,其中 { 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}的一个排列,我们可以减少未知数的个数。举例,如果我们求出了 β 11 \beta_{11} β11 β 12 \beta_{12} β12 β 13 \beta_{13} β13,那么我们可以得到 β 23 = β 12 β 13 β 11 \beta_{23} = \frac{\beta_{12}\beta_{13}}{\beta_{11}} β23=β11β12β13。这样,即使对于 N = 4 N = 4 N=4,我们也可以求解出 { β i j } i , j = 1 , ⋯   , N \{\beta_{ij}\}_{i,j = 1,\cdots,N} {βij}i,j=1,,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 Error(β)=(i,j) s.t. i<j(ciccjc2ciwcjw2)2
这是一个无约束的非线性最优化问题,简单。

计算摄像头的位姿

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

  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 cic=j=1Nβkvk[i], i=1,,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 pic=j=14αijcjc, i=1,,n

  3. 计算 { p i w } i = 1 , ⋯ &ThinSpace; , n \{\mathbf{p}_i^w\}_{i=1,\cdots,n} {piw}i=1,,n的重心 p 0 w \mathbf{p}_0^w p0w和矩阵 A 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] p0w=n1i=1npiwA=p1wTp0wTpnwTp0wT

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

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

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

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

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

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

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

代码

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

已标记关键词 清除标记
相关推荐
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页