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

PnP问题

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

DLT, 直接线性变换

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

s⎡⎣⎢u1v11⎤⎦⎥=⎡⎣⎢t1t5t9t2t6t10t3t7t11t4t8t12⎤⎦⎥⎡⎣⎢⎢⎢XYZ1⎤⎦⎥⎥⎥$$s\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=\left[\begin{array}{cccc}{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{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$$

消去s$s$后可得到约束：

u1=t1X+t2Y+t3Z+t4t9X+t10Y+t11Z+t12  v1=t5X+t6Y+t7Z+t8t9X+t10Y+t11Z+t12$$u_1=\frac{t_{1}X+t_{2}Y+t_{3}Z+t_4}{t_{9}X+t_{10}Y+t_{11}Z+t_{1}2}{v}_1=\frac{t_{5}X+t_{6}Y+t_{7}Z+t_8}{t_{9}X+t_{10}Y+t_{11}Z+t_{1}2}$$

假设：

t⃗ 1=(t1,t2,t3,t4)⊤,  t⃗ 2=(t5,t6,t7,t8)⊤,  t⃗ 3=(t9,t10,t11,t12)⊤$${\overrightarrow{t}}_1=(t_1,t_2,t_3,t_4{)}^{\mathrm{\top }},{\overrightarrow{t}}_2=(t_5,t_6,t_7,t_8{)}^{\mathrm{\top }},{\overrightarrow{t}}_3=(t_9,t_{10},t_{11},t_{12}{)}^{\mathrm{\top }}$$

则有：

t⃗ ⊤1P−t⃗ ⊤3Pu1=0,t⃗ ⊤2P−t⃗ ⊤3Pv1=0$${\overrightarrow{t}}_1^{\mathrm{\top }}\mathbf{P}-{\overrightarrow{t}}_3^{\mathrm{\top }}\mathbf{P}{u}_1=0,{\overrightarrow{t}}_2^{\mathrm{\top }}\mathbf{P}-{\overrightarrow{t}}_3^{\mathrm{\top }}\mathbf{P}{v}_1=0$$

上式中t⃗ $\overrightarrow{t}$是待求的变量。易知一个特征点可提供两个关于t⃗ $\overrightarrow{t}$的约束，假若存在N$N$个特征点，则有如下方程成立：

⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢P⊤10⋮P⊤N00P⊤1⋮0P⊤N−u1P⊤1−v1P⊤1⋮−uNP⊤N−vNP⊤N⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎡⎣⎢⎢t⃗ 1t⃗ 2t⃗ 3⎤⎦⎥⎥=0$$\left[\begin{array}{ccc}{\mathbf{P}}_1^{\mathrm{\top }}& 0& -u_1{\mathbf{P}}_1^{\mathrm{\top }}\\ 0& {\mathbf{P}}_1^{\mathrm{\top }}& -v_1{\mathbf{P}}_1^{\mathrm{\top }}\\ ⋮& ⋮& ⋮\\ {\mathbf{P}}_N^{\mathrm{\top }}& 0& -u_N{\mathbf{P}}_N^{\mathrm{\top }}\\ 0& {\mathbf{P}}_N^{\mathrm{\top }}& -v_N{\mathbf{P}}_N^{\mathrm{\top }}\end{array}\right]\left[\begin{array}{c}{\overrightarrow{t}}_1\\ {\overrightarrow{t}}_2\\ {\overrightarrow{t}}_3\end{array}\right]=0$$

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

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

P3P问题

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

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

△Oab−△OAB,  △Obc−△OBC,  △Oac−△OAC$$\mathrm{△}Oab-\mathrm{△}OAB,\mathrm{△}Obc-\mathrm{△}OBC,\mathrm{△}Oac-\mathrm{△}OAC$$

考虑余弦关系：

OA2+OB2−2⋅OA⋅OB⋅cos<a,b>=AB2OB2+OC2−2⋅OB⋅OC⋅cos<b,c>=BC2OA2+OBC2−2⋅OA⋅OC⋅cos<a,c>=AC2$$\begin{array}{r}O{A}^2+O{B}^2-2\cdot OA\cdot OB\cdot \cos <a,b>=A{B}^2\\ O{B}^2+O{C}^2-2\cdot OB\cdot OC\cdot \cos <b,c>=B{C}^2\\ O{A}^2+OB{C}^2-2\cdot OA\cdot OC\cdot \cos <a,c>=A{C}^2\end{array}$$

左右两边同时除以OC2$O{C}^2$, 令x=OA/OC$x=OA/OC$, y=OB/OC$y=OB/OC$有：

x2+y2−2xycos<a,b>=AB2/OC2y2+12−2ycos<b,c>=BC2/OC2x2+12−2xcos<a,c>=AC2/OC2$$\begin{array}{r}{x}^2+y^2-2xy\cos <a,b>=A{B}^2/O{C}^2\\ y^2+1^2-2ycos<b,c>=B{C}^2/O{C}^2\\ x^2+1^2-2x\cos <a,c>=A{C}^2/O{C}^2\end{array}$$

再令v=AB2/OC2$v=A{B}^2/O{C}^2$, v=BC2/OC2OC2/AB2$v=B{C}^2/O{C}^{2}O{C}^2/A{B}^2$, w=AC2/OC2OC2/AB2$w=A{C}^2/O{C}^{2}O{C}^2/A{B}^2$,有

x2+y2−2xycos<a,b>−v=0y2+12−2ycos<b,c>−uv=0x2+12−2xcos<a,c>−wv=0$$\begin{array}{r}{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{array}$$

从上式中先解出v$v$，代入第二和第三个式子，有

(1−u)y2−ux2−cos<b,c>y+2uxycos<a,b>+1=0(1−w)x2−wy2−cos<a,c>x+2wxycos<a,b>+1=0x2+12−2xcos<a,c>−wv=0$$\begin{array}{r}(1-u)y^2-u{x}^2-\cos <b,c>y+2uxy\cos <a,b>+1=0\\ (1-w)x^2-w{y}^2-\cos <a,c>x+2wxy\cos <a,b>+1=0\\ x^2+1^2-2x\cos <a,c>-wv=0\end{array}$$

上式中由于A,B,C$A,B,C$已知，x,y$x,y$未知。是一个二元二次方程，最多可得到4个解。需要一个验证点来获得最优解。

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

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

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