3d-2d位姿估计之EPnP算法

1。问题

假设我们得到了一组特征点,并且对于每个特征点 i ,我们已知如下信息:

a. 特征点 i 在世界坐标系的坐标 P_{i}^{w}=\left [ \begin{matrix} x_{i}^{w}\\  y_{i}^{w}\\  z_{i}^{w} \end{matrix} \right ]

b. 特征点 i 在当前帧的投影坐标 p_{i}=\left [ \begin{matrix} u_{i}\\  v_{i} \end{matrix} \right ]

求:相机相对于世界坐标系的位姿 T_{cw}=\begin{bmatrix} R_{cw} & t\\   0& 1 \end{bmatrix}


视觉slam中的追踪、闭环检测以及重定位都会遇到上述问题。在orb-slam[1]的重定位中,采用EPnP算法[2]解决上述问题。

2。控制点

EPnP算法首先在世界坐标系内寻找四个控制点,记作 C_{1}^{w}C_{2}^{w}C_{3}^{w}C_{4}^{w} ,使得:


对于世界坐标系内任意一点 P_{i}^{w} ,存在对应的 \alpha _{i}=\begin{bmatrix} \alpha _{i1} &  \alpha _{i2}&  \alpha _{i3}& \alpha _{i4} \end{bmatrix}^{T} ,满足


现在来讨论这四个控制点的存在性,上式可以写作:

\begin{bmatrix} p_{i}^{w}\\  1 \end{bmatrix} = \begin{bmatrix} C_{1}^{w} &  C_{2}^{w} & C_{3}^{w} &C_{4}^{w}  \\   1& 1 & 1 & 1 \end{bmatrix} \alpha _{i}

可见,只要矩阵 \begin{bmatrix} C_{1}^{w} &  C_{2}^{w} & C_{3}^{w} &C_{4}^{w}  \\   1& 1 & 1 & 1 \end{bmatrix} 是非奇异的,就一定能找到满足条件的 \alpha _{i}

接下来,我们讨论相机坐标系下,控制点和参考点之间的关系。

p_{i}^{c}=R_{cw}p_{i}^{w}+t=R_{cw}\left (\sum_{j=1}^{4} \alpha _{ij}c_{i}^{w} \right ) +t

由于 \sum_{j=1}^{4} \alpha _{ij}=1 ,因此 t=\sum_{j=1}^{4} \alpha _{ij}t ,带入到上式,化简,得

p_{i}^{c}= \sum_{j=1}^{4} \alpha _{ij} \left(R_{cw}c_{i}^{w} \right) +t )=\sum_{j=1}^{4} \alpha _{ij}c_{i}^{c}

也就是说,系数 \alpha _{i} 在相机坐标系中具有不变性。如果我们能够求出控制点在相机坐标系中的坐标C_{1}^{c}C_{2}^{c}C_{3}^{c}C_{4}^{c} ,那么:

对于任何一个参考点 k , 根据方程()求出 \alpha _{k} ,进而求出它在相机坐标系中的坐标 p_{k}^{c}=\sum_{j=1}^{4} \alpha _{kj}c_{i}^{c} ,这就把问题转化为3d-3d位姿估计的问题。


3。求解控制点在相机坐标系中的坐标

详细见论文[2]

有点需要提下:

论文中求解

Mx=x

是转化为

M^{T}Mx=0

进行求解的。这两个方程是等价的,即:

Mx=0 \Leftrightarrow M^{T}Mx=0

证明如下:

1)显然, Mx=0 \Rightarrow M^{T}Mx=0

2) M^{T}Mx=0\Rightarrow \left(M x \right)^{T}Mx=x^{T}M^{T}Mx=0\Rightarrow Mx=0

综合1)和2),命题成立。


4。利用SVD分解进行3d-3d位姿估计

论文[3]已经讲得够清楚了。。。


5。疑问:

1)有没有论文对[2]中的矩阵 M 的性质进行探讨,例如:[2]指出,对于透视模型的相机,Mx=0 解空间的维数是1,如何证明?


6。参考文献

[1] Mur-Artal R, Montiel J M M, Tardos J D. ORB-SLAM: a versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5): 1147-1163.

[2] Lepetit V, Moreno-Noguer F, Fua P. Epnp: An accurate o (n) solution to the pnp problem[J]. International journal of computer vision, 2009, 81(2): 155.

[3] Arun K S, Huang T S, Blostein S D. Least-squares fitting of two 3-D point sets[J]. IEEE Transactions on Pattern Analysis & Machine Intelligence, 1987 (5): 698-700.

编辑于 2018-10-14

文章被以下专栏收录