视觉SLAM笔记(六)-PnP
PnP算法是在已知 n n n个特征点像素位置及其对应的3D空间点位置时估计相机位姿的方法。在两张图像中，若其中一张图像的特征点的3D位置已知，则可以使用PnP方法估计相机运动，特征点的3D位置可由三角测量或RGBD/双目相机确定。目前常见的PnP计算方法主要有DLT(直接线性变换)，P3P，EPnP，UPnP等
DLT
设某个空间点的坐标为 P = ( X , Y , Z , 1 ) T P=(X,Y,Z,1)^T P=(X,Y,Z,1)T，设 P P P在归一化平面上的投影坐标为 x = ( u , v , 1 ) x=(u,v,1) x=(u,v,1)(这里使用归一化平面上坐标的目的是省去相机内参矩阵)，定义一个增广矩阵 [ R ∣ t ] ∈ R 3 × 4 [R|t]\in R^{3\times4} [R∣t]∈R3×4，根据相机投影模型，列出等式如下:
s ( u v 1 ) = ( t 1 t 2 t 3 t 4 t 5 t 6 t 7 t 8 t 9 t 10 t 11 t 12 ) ( X Y Z 1 ) s\left(\begin{array}{c}{u} \\ {v} \\ {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}{l}{X} \\ {Y} \\ {Z} \\ {1}\end{array}\right) s⎝⎛uv1⎠⎞=⎝⎛t1t5t9t2t6t10t3t7t11t4t8t12⎠⎞⎝⎜⎜⎛XYZ1⎠⎟⎟⎞
消除 s s s后等式如下:
u = t 1 X + t 2 Y + t 3 Z + t 4 t 9 X + t 10 Y + t 11 Z + t 12 v = t 5 X + t 6 Y + t 7 Z + t 8 t 9 X + t 10 Y + t 11 Z + t 12 u=\frac{t_{1} X+t_{2} Y+t_{3} Z+t_{4}}{t_{9} X+t_{10} Y+t_{11} Z+t_{12}}\\ v=\frac{t_{5} X+t_{6} Y+t_{7} Z+t_{8}}{t_{9} X+t_{10} Y+t_{11} Z+t_{12}} u=t9X+t10Y+t11Z+t12t1X+t2Y+t3Z+t4v=t9X+t10Y+t11Z+t12t5X+t6Y+t7Z+t8
定义:
t 1 = ( t 1 , t 2 , t 3 , t 4 ) T , t 2 = ( t 5 , t 6 , t 7 , t 8 ) T , t 3 = ( t 9 , t 10 , t 11 , t 12 ) T t_{1}=\left(t_{1}, t_{2}, t_{3}, t_{4}\right)^{T}, t_{2}=\left(t_{5}, t_{6}, t_{7}, t_{8}\right)^{T}, t_{3}=\left(t_{9}, t_{10}, t_{11}, t_{12}\right)^{T} t1=(t1,t2,t3,t4)T,t2=(t5,t6,t7,t8)T,t3=(t9,t10,t11,t12)T
将 u , v u,v u,v的等式简化如下:
t 1 T P − t 3 T P u 1 = 0 , t 2 T P − t 3 T P v 1 = 0 t_{1}^{T} P-t_{3}^{T} P u_{1}=0,t_{2}^{T} P-t_{3}^{T} P v_{1}=0 t1TP−t3TPu1=0,t2TP−t3TPv1=0
每个特征点可提供两个关于 t t t的约束， t t t共有12维，故共需要6个特征点实现DLT求解，但解出的 R R R可能不满足 S O ( 3 ) SO(3) SO(3)，故需要通过QR分解等操作找到一个最好的旋转矩阵来近似。
P3P
P3P仅使用3对匹配点和一对验证点(从可能的解中选出正确的)，已知3D点在世界坐标系下坐标和2D点在相机成像平面上投影坐标，则通过相似三角形关系计算出3D点在相机坐标系下的坐标，得到3D-3D的对应点，再通过ICP算法计算出相机位姿
如图所示， A , B , C A,B,C A,B,C为3D点， a , b , c a,b,c a,b,c为其对应的2D点，三角形间存在相似关系如下:
Δ O a b ∼ Δ O A B , Δ O b c ∼ Δ O B C , Δ O a c ∼ Δ O A C \Delta O a b\sim\Delta O A B, \quad \Delta O b c\sim\Delta O B C, \quad \Delta O a c\sim\Delta O A C ΔOab∼ΔOAB,ΔObc∼ΔOBC,ΔOac∼ΔOAC
利用余弦定理:
O A 2 + O B 2 − 2 O A ⋅ O B ⋅ cos ⟨ a , b ⟩ = A B 2 O B 2 + O C 2 − 2 O B ⋅ O C ⋅ cos ⟨ b , c ⟩ = B C 2 O A 2 + O C 2 − 2 O A ⋅ O C ⋅ cos ⟨ a , c ⟩ = A C 2 O A^{2}+O B^{2}-2 O A \cdot O B \cdot \cos \langle a, b\rangle= A B^{2}\\ O B^{2}+O C^{2}-2 O B \cdot O C \cdot \cos \langle b, c\rangle= B C^{2}\\ O A^{2}+O C^{2}-2 O A \cdot O C \cdot \cos \langle a, c\rangle= A C^{2} OA2+OB2−2OA⋅OB⋅cos⟨a,b⟩=AB2OB2+OC2−2OB⋅OC⋅cos⟨b,c⟩=BC2OA2+OC2−2OA⋅OC⋅cos⟨a,c⟩=AC2
上式同时除以 O C 2 OC^2 OC2，记 x = O A / O C , y = O B / O C x=OA/OC,y=OB/OC x=OA/OC,y=OB/OC， v = A B 2 / O C 2 v=AB^2/OC^2 v=AB2/OC2， u = B C 2 / A B 2 u=BC^2/AB^2 u=BC2/AB2， w = A C 2 / A B 2 w=AC^2/AB^2 w=AC2/AB2，将上式简化如下
x 2 + y 2 − 2 x y cos ⟨ a , b ⟩ − v = 0 y 2 + 1 − 2 y cos ⟨ b , c ⟩ − u v = 0 x 2 + 1 − 2 x cos ⟨ a , c ⟩ − w v = 0 x^{2}+y^{2}-2 x y \cos \langle a, b\rangle- v=0\\ y^{2}+1-2 y \cos \langle b, c\rangle- u v=0\\ x^{2}+1-2 x \cos \langle a, c\rangle- w v=0 x2+y2−2xycos⟨a,b⟩−v=0y2+1−2ycos⟨b,c⟩−uv=0x2+1−2xcos⟨a,c⟩−wv=0
消去未知参数 v v v，得
( 1 − u ) y 2 − u x 2 − cos ⟨ b , c ⟩ y + 2 u x y cos ⟨ a , b ⟩ + 1 = 0 ( 1 − w ) x 2 − w y 2 − cos ⟨ a , c ⟩ x + 2 w x y cos ⟨ a , b ⟩ + 1 = 0 (1-u) y^{2}-u x^{2}-\cos \langle b, c\rangle y+2 u x y \cos \langle a, b\rangle+ 1=0\\ (1-w) x^{2}-w y^{2}-\cos \langle a, c\rangle x+2 w x y \cos \langle a, b\rangle+ 1=0 (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=0
在上式中， u , v u,v u,v均可通过 A , B , C A,B,C A,B,C的世界坐标系算出， cos ⟨ a , b ⟩ \cos \langle a, b\rangle cos⟨a,b⟩等均是已知的，所以未知量仅有 x , y x,y x,y，可通过解上述方程算出，解出 x , y x,y x,y后可计算得到 A , B , C A,B,C A,B,C在相机坐标系下的3D坐标，再通过ICP算法即可得到相机位姿
参考文献
[1] 高翔, 张涛, 颜沁睿, 刘毅, 视觉SLAM十四讲：从理论到实践, 电子工业出版社, 2017