《视觉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) ${\stackrel{\to }{x}}_1=(u_1,v_1,1)$，为求解 R $\mathbf{R}$和 t⃗  $\stackrel{\to }{t}$,定义增广矩阵 [R|t⃗ ] $[\mathbf{R}|\stackrel{\to }{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)⊤ $${\stackrel{\to }{t}}_1=(t_1,t_2,t_3,t_4{)}^{\mathrm{\top }},{\stackrel{\to }{t}}_2=(t_5,t_6,t_7,t_8{)}^{\mathrm{\top }},{\stackrel{\to }{t}}_3=(t_9,t_{10},t_{11},t_{12}{)}^{\mathrm{\top }}$$

则有：

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

上式中 t⃗  $\stackrel{\to }{t}$是待求的变量。易知一个特征点可提供两个关于 t⃗  $\stackrel{\to }{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}{\stackrel{\to }{t}}_1\\ {\stackrel{\to }{t}}_2\\ {\stackrel{\to }{t}}_3\end{array}\right]=0$$

观察到 t⃗  $\stackrel{\to }{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/OC2OC22OC2/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$,有