《視覺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$,有