《视觉SLAM十四讲》学习笔记-3D->2D: PnP问题的由来

PnP问题

• PnP为 Perspective-n-Point的简称，是求解3D到2D点对的运动的方法:即给出n个3D空间点时，如何求解相机的位姿。
• 典型的PnP问题求解方式有很多种，例如P3P， 直接线性变换(DLT), EPnP(Efficient PnP), UPnP。还有非线性的Bundle Adjustment.

DLT, 直接线性变换

高空间点$P$的齐次方程为$\mathbf{P}=(X, Y, Z)^\top$，投影到特征点$\vec{x}_1=(u_1, v_1, 1)$，为求解$\mathbf{R}$和$\vec{t}$,定义增广矩阵$[\mathbf{R}|\vec{t}]$:
展开等式后可得到：

$$s\begin{bmatrix} u_1\\ v_1 \\ 1 \end{bmatrix}= \begin{bmatrix} 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{bmatrix} \begin{bmatrix} X\\ Y \\ Z\\ 1 \end{bmatrix}$$
消去 $s$后可得到约束：
$$u_1 = \frac{t_1X + t_2Y + t_3Z + t_4}{t_9X + t_{10}Y + t_{11}Z + t_12}~~v_1=\frac{t_5X + t_6Y + t_7Z + t_8}{t_9X + t_{10}Y + t_{11}Z + t_12}$$
假设：
$$\vec{t}_1 = (t_1, t_2, t_3, t_4)^\top,~~\vec{t}_2 = (t_5, t_6, t_7, t_8)^\top,~~\vec{t}_3 = (t_9, t_{10}, t_{11}, t_{12})^\top$$
则有：
$$\vec{t}_1^\top\mathbf{P} - \vec{t}_3^\top\mathbf{P}u_1 = 0, \vec{t}_2^\top\mathbf{P} - \vec{t}_3^\top\mathbf{P}v_1 = 0$$
上式中 $\vec{t}$是待求的变量。易知一个特征点可提供两个关于 $\vec{t}$的约束，假若存在 $N$个特征点，则有如下方程成立：
$$\begin{bmatrix} \mathbf{P}_1^\top & 0 & -u_1\mathbf{P}_1^\top \\ 0 & \mathbf{P}_1^\top & -v_1\mathbf{P}_1^\top \\ \vdots& \vdots & \vdots \\ \mathbf{P}_N^\top & 0 & -u_N\mathbf{P}_N^\top \\ 0 & \mathbf{P}_N^\top & -v_N\mathbf{P}_N^\top \\ \end{bmatrix} \begin{bmatrix} \vec{t}_1\\ \vec{t}_2 \\ \vec{t}_3 \end{bmatrix} =0$$

观察到$\vec{t}$有12个变量，通过方程形式可知最少可以通过6对匹配点即可得到$\mathbf{T}$的解。所以本方法又可称为直接线性变换法；当匹配点大于六对时，可以使用SVD等方法对超定方程求最小二乘解。

注意到DLT解出的T是由R和t两部分构成的，因而$\mathbf{R}$满足$\mathbf{R}=SO(3)$，所以对于T矩阵需要寻找一个最好的旋转矩阵，这可以由QR分解完成，相当于把结果从矩阵空间重影到$SE(3)$流形上，转成旋转和平移两部分。

P3P问题

首先设标记符号定义如上图所示。其中A,B,C为世界坐标系。图中为3D到3D的对应点，所以是把PnP问题转化为ICP问题。

先利用三解形近似关系有以下三解形相似：

$$\triangle Oab -\triangle OAB, ~~\triangle Obc -\triangle OBC, ~~\triangle Oac -\triangle OAC$$

考虑余弦关系：

$$\begin{aligned} OA^2 + OB^2-2\cdot OA \cdot OB \cdot \cos<a, b> = AB^2\\ OB^2 + OC^2-2\cdot OB \cdot OC \cdot \cos<b, c> = BC^2\\ OA^2 + OBC^2-2\cdot OA \cdot OC \cdot \cos<a,c> = AC^2\\ \end{aligned}$$
左右两边同时除以 $OC^2$, 令 $x=OA/OC$, $y=OB/OC$有：
$$\begin{aligned} x^2 + y^2 -2xy\cos<a,b> = AB^2/OC^2\\ y^2 + 1^2 - 2ycos<b,c> = BC^2/OC^2\\ x^2 + 1^2 -2x\cos<a,c> = AC^2/OC^2 \end{aligned}$$
再令 $v=AB^2/OC^2$, $v=BC^2/OC^2OC^2/AB^2$, $w=AC^2/OC^2 OC^2/AB^2$,有
$$\begin{aligned} x^2 + y^2 -2xy\cos<a,b> - v = 0\\ y^2 + 1^2 -2y \cos<b,c> -uv = 0\\ x^2 + 1^2 -2x \cos<a,c> -wv = 0 \end{aligned}$$
从上式中先解出 $v$，代入第二和第三个式子，有

$$\begin{aligned} (1-u)y^2 - ux^2 -\cos<b,c>y + 2uxy\cos<a,b> + 1 = 0\\ (1-w)x^2 - wy^2 -\cos<a,c>x + 2wxy\cos<a,b> + 1 = 0\\ x^2 + 1^2 -2x \cos<a,c> -wv = 0 \end{aligned}$$
上式中由于 $A, B, C$已知， $x, y$未知。是一个二元二次方程，最多可得到4个解。需要一个验证点来获得最优解。

可以看到， 利用三角形的相似性质，PnP问题转为了一个3D到3D的位姿估计问题。

P3P的问题：
1. 只利用三个点的信息，当给定的配对点多于3组时，难以利用更多的信息；
2. 如果数据点存在噪声时，或者匹配是误匹配的情况下，算法失败。

PnP在SLAM问题中的角色：进行相机位姿估计，然后构建最小二乘优化问题对估计值进行调整(Bundle Adjustment, BA)