《视觉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)⊤ P = ( X , Y , Z ) ⊤ ，投影到特征点 x⃗ 1=(u1,v1,1) x → 1 = ( u 1 , v 1 , 1 ) ，为求解 R R 和 t⃗  t → ,定义增广矩阵 [R|t⃗ ] [ R | t → ] :
展开等式后可得到：

s⎡⎣⎢u1v11⎤⎦⎥=⎡⎣⎢t1t5t9t2t6t10t3t7t11t4t8t12⎤⎦⎥⎡⎣⎢⎢⎢XYZ1⎤⎦⎥⎥⎥ s [ u 1 v 1 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 s 后可得到约束：

u1=t1X+t2Y+t3Z+t4t9X+t10Y+t11Z+t12  v1=t5X+t6Y+t7Z+t8t9X+t10Y+t11Z+t12 u 1 = 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 = 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)⊤ t → 1 = ( t 1 , t 2 , t 3 , t 4 ) ⊤ ,     t → 2 = ( t 5 , t 6 , t 7 , t 8 ) ⊤ ,     t → 3 = ( t 9 , t 10 , t 11 , t 12 ) ⊤

则有：

t⃗ ⊤1P−t⃗ ⊤3Pu1=0,t⃗ ⊤2P−t⃗ ⊤3Pv1=0 t → 1 ⊤ P − t → 3 ⊤ P u 1 = 0 , t → 2 ⊤ P − t → 3 ⊤ P v 1 = 0

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

⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢P⊤10⋮P⊤N00P⊤1⋮0P⊤N−u1P⊤1−v1P⊤1⋮−uNP⊤N−vNP⊤N⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎡⎣⎢⎢t⃗ 1t⃗ 2t⃗ 3⎤⎦⎥⎥=0 [ P 1 ⊤ 0 − u 1 P 1 ⊤ 0 P 1 ⊤ − v 1 P 1 ⊤ ⋮ ⋮ ⋮ P N ⊤ 0 − u N P N ⊤ 0 P N ⊤ − v N P N ⊤ ] [ t → 1 t → 2 t → 3 ] = 0

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

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

P3P问题

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

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

△Oab−△OAB,  △Obc−△OBC,  △Oac−△OAC △ O a b − △ O A B ,     △ O b c − △ O B C ,     △ O a c − △ O A C

考虑余弦关系：

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 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 B C 2 − 2 ⋅ O A ⋅ O C ⋅ cos < a , c >= A C 2

左右两边同时除以 OC2 O C 2 , 令 x=OA/OC x = O A / O C , y=OB/OC y = O B / O C 有：

x2+y2−2xycos<a,b>=AB2/OC2y2+12−2ycos<b,c>=BC2/OC2x2+12−2xcos<a,c>=AC2/OC2 x 2 + y 2 − 2 x y cos < a , b >= A B 2 / O C 2 y 2 + 1 2 − 2 y c o s < b , c >= B C 2 / O C 2 x 2 + 1 2 − 2 x cos < a , c >= A C 2 / O C 2

再令 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 x 2 + y 2 − 2 x y cos < a , b > − v = 0 y 2 + 1 2 − 2 y cos < b , c > − u v = 0 x 2 + 1 2 − 2 x cos < a , c > − w v = 0

从上式中先解出 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 ( 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 x 2 + 1 2 − 2 x cos < a , c > − w v = 0

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

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

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

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