PNP问题学习笔记

整理自浙江大学谭平老师的课堂内容

问题引出

PNP问题是指在已知相机内参的前提下，通过N对匹配的图像坐标以及它们的世界坐标计算相机的位姿。是一个3D-2D问题。

关于n的探讨

图像上一个点会对相机位姿产生两个约束，如下图所示，点$x_i$的存在会使得相机平面不能沿垂直$X_i{x}_i$的平面做平移运动，损失2个自由度。 相机位姿变化T含有6个自由度，所以至少需要在两幅图像中有3对点对，才能进行计算。即$n\ge 3$。

PNP问题的求解思路

1. 计算出特征点在相机坐标系下的三维坐标
2. 通过特征点在世界坐标系和相机坐标系里面的位置关系求解相机姿态

P3P

通过图像坐标求解在相机坐标系下的三维坐标

对于两对点对$X_i{x}_i$和$X_j{x}_j$利用余弦定理，有
$$d_{ij}^2=d_i^2+d_j^2-2d_i{d}_j\cos {\theta }_{ij}$$
记
$$f_{ij}\left(d_i,d_j\right)=d_i^2+d_j^2-2d_i{d}_j\cos {\theta }_{ij}-d_{ij}^2$$
其中$d_{ij},{\theta }_{ij}$为已知量，$d_j,d_j$为未知量。其中两点与图像上两点与相机光心夹角的余弦值可以借助相机内参矩阵得出：

$$\cos \theta =\frac{y_1^T{y}_2}{\sqrt{\left(y_{1}T{y}_1\right)\left(y_{2}T{y}_2\right)}}=\frac{x_{1}T\left(K^{-T}{K}^{-1}\right)x_2}{\sqrt{\left(x_{1}T\left(K^{-T}{K}^{-1}\right)x_1\right)\left(x_{2}T\left(K^{-T}{K}^{-1}\right)x_2\right)}}$$

当我们由3个点时，可以构造3个方程
$$\{\begin{array}{l}{f}_{12}\left(d_1,d_2\right)=0\\ f_{23}\left(d_2,d_3\right)=0\\ f_{13}\left(d_1,d_3\right)=0\end{array}$$
通过消元，可以将方程组变为一元四次方程，令$x=d_1^2$
$$g(x)=a_5{x}^4+a_4{x}^3+a_3{x}^2+a_{2}x+a_1=0$$
求解这个方程，我们会得到4个解，这其中只有一个是我们需要的解。也就是说对于P3P问题，虽然只要3个点就可以进行求解，但是通常需要第四个点对结果进行检验。
求解出$d_1,d_2,d_3$，相当于获得了点在图像中的深度信息，也就求出了点在相机坐标系下的坐标。

线性PNP算法

对于$n\ge 5$的情况，对于任意一个点和两个其它点，可以构建一个方程
$$g(x)=a_5{x}^4+a_4{x}^3+a_3{x}^2+a_{2}x+a_1=0$$
一系列这样的方程可以构建一个线性方程组，
$$\left(\begin{array}{ccccc}...& & & & \\ a_5^i& a_4^i& a_3^i& a_2^i& a_1^i\\ ...& & & & \end{array}\right)\left(\begin{array}{c}{x}^4\\ x^3\\ x^2\\ x\\ 1\end{array}\right)=0$$

EPNP

Linear PNP算法的复杂度是$O(n^5)$，EPNP算法复杂度是$O(n)$。