[原创]深入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=1∑4αijcjw, with j=1∑4α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=1∑4α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αijcjw∑j=14αij]=j=1∑4αij[Rt][cjw1]=j=1∑4α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