PnP问题
- PnP为
Perspective-n-Point
的简称,是求解3D到2D点对的运动的方法:即给出n个3D空间点时,如何求解相机的位姿。
- 典型的PnP问题求解方式有很多种,例如P3P,
直接线性变换
(DLT), EPnP
(Efficient PnP), UPnP。还有非线性的Bundle Adjustment
.
DLT, 直接线性变换
高空间点
P
的齐次方程为
P=(X,Y,Z)⊤
,投影到特征点
x⃗ 1=(u1,v1,1)
,为求解
R
和
t⃗
,定义增广矩阵
[R|t⃗ ]
:
展开等式后可得到:
s⎡⎣⎢u1v11⎤⎦⎥=⎡⎣⎢t1t5t9t2t6t10t3t7t11t4t8t12⎤⎦⎥⎡⎣⎢⎢⎢XYZ1⎤⎦⎥⎥⎥
消去
s
后可得到约束:
u1=t1X+t2Y+t3Z+t4t9X+t10Y+t11Z+t12 v1=t5X+t6Y+t7Z+t8t9X+t10Y+t11Z+t12
假设:
t⃗ 1=(t1,t2,t3,t4)⊤, t⃗ 2=(t5,t6,t7,t8)⊤, t⃗ 3=(t9,t10,t11,t12)⊤
则有:
t⃗ ⊤1P−t⃗ ⊤3Pu1=0,t⃗ ⊤2P−t⃗ ⊤3Pv1=0
上式中
t⃗
是待求的变量。易知一个特征点可提供两个关于
t⃗
的约束,假若存在
N
个特征点,则有如下方程成立:
⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢P⊤10⋮P⊤N00P⊤1⋮0P⊤N−u1P⊤1−v1P⊤1⋮−uNP⊤N−vNP⊤N⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎡⎣⎢⎢t⃗ 1t⃗ 2t⃗ 3⎤⎦⎥⎥=0
观察到
t⃗
有12个变量,通过方程形式可知最少可以通过6对匹配点即可得到
T
的解。所以本方法又可称为直接线性变换法;当匹配点大于六对时,可以使用SVD等方法对超定方程求最小二乘解。
注意到DLT解出的T是由R和t两部分构成的,因而
R
满足
R=SO(3)
,所以对于T矩阵需要寻找一个最好的旋转矩阵,这可以由QR分解完成,相当于把结果从矩阵空间重影到
SE(3)
流形上,转成旋转和平移两部分。
P3P问题
![P3P问题](https://img-blog.csdn.net/2018061809342922?watermark/2/text/aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1b2h1aXd1/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70)
首先设标记符号定义如上图所示。其中A,B,C为世界坐标系。图中为3D到3D的对应点,所以是把PnP问题转化为ICP问题。
先利用三解形近似关系有以下三解形相似:
△Oab−△OAB, △Obc−△OBC, △Oac−△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
左右两边同时除以
OC2
, 令
x=OA/OC
,
y=OB/OC
有:
x2+y2−2xycos<a,b>=AB2/OC2y2+12−2ycos<b,c>=BC2/OC2x2+12−2xcos<a,c>=AC2/OC2
再令
v=AB2/OC2
,
v=BC2/OC2OC2/AB2
,
w=AC2/OC2OC2/AB2
,有
x2+y2−2xycos<a,b>−v=0y2+12−2ycos<b,c>−uv=0x2+12−2xcos<a,c>−wv=0
从上式中先解出
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
上式中由于
A,B,C
已知,
x,y
未知。是一个二元二次方程,最多可得到4个解。需要一个验证点来获得最优解。
可以看到, 利用三角形的相似性质,PnP问题转为了一个3D到3D的位姿估计问题
。
P3P的问题:
1. 只利用三个点的信息,当给定的配对点多于3组时,难以利用更多的信息;
2. 如果数据点存在噪声时,或者匹配是误匹配的情况下,算法失败。
PnP在SLAM问题中的角色:进行相机位姿估计,然后构建最小二乘优化问题对估计值进行调整(Bundle Adjustment, BA)