PnP算法

PnP (Perspective-n-Point)是求解3D到2D点对运动的方法。解决的问题是:当我们知道n个3D空间点以及他们的投影位置时,估算相机所在的位姿。特征点的3D位置可以由三角化或者RGB-D相机的深度图确定。因此,在双目或者RGB-D的视觉里程计中,可以直接使用PnP估计相机运动。

直接线性变换(DLT):

空间点P,齐次坐标为P=(X,Y,Z,1)^{T}。投影到归一化平面的特征点齐次坐标为x_{1}=(u_{1},v_{1},1)^{T},此时相机的位姿R,t是未知的。定义一个增广矩阵[R|t]为一个3X4的矩阵,包含了旋转与平移信息。将投影矩阵展开如下:

s\begin{pmatrix} u\\ v\\ 1 \end{pmatrix}=\begin{pmatrix} t _{1}&t_{2}&t_{3}&t_{4} \\ t_{5}&t{6}&t_{7} &t_{8} \\ t_{9}&t_{10}&t_{11}&t_{12} \end{pmatrix}\begin{pmatrix} X\\ Y\\ Z\\ 1 \end{pmatrix}

用最后一行消去s后,可以得到两个约束:

u_{1}=\frac{t_{1}X+t_{2}Y+t_{3}Z+t_{4}}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}                  v_{1}=\frac{t_{5}X+t_{6}Y+t_{7}Z+t_{8}}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}

定义T的行向量为:

t_{1}=(t_{1},t_{2},t_{3},t_{4})^{T}       t_{2}=(t_{5},t_{6},t_{7},t_{8})^{T}   t_{3}=(t_{9},t_{10},t_{11},t_{12})^{T}

于是有:

t_{1}^{T}P-t_{3}^{T}Pu_{1}=0         t_{2}^{T}P-t_{3}^{T}Pv_{1}=0

假设有N个特征点:可以求出线性方程组为

\begin{pmatrix} P^{T}_{1} &0 &-u_{1}P^{T}_{1} \\ 0&P^{T}_{1} &-v_{1}P^{T}_{1} \\ .& . &. \\ .& . &. \\ .& . & .\\ P^{T}_{n} &0 &-u_{N}P^{T}_{n} \\ 0& P^{T}_{N} & -v_{N}P^{T}_{n} \end{pmatrix}\begin{pmatrix} t_{1}\\ t_{2}\\ t_{3} \end{pmatrix}=0

由于t一共是12维度,所以最少需要六对匹配点,即可实现矩阵T的线性求解,这种方法称为直接线性变换。当匹配点对大于六对时,可以使用SVD等方法对超定方程求最小二乘解

你可能感兴趣的:(杂七杂八,计算机视觉)