PnP问题求解

1.直接线性变换(DLT)
对某空间点P = (X, Y, Z, 1)T,在某图像中对应点为 x1 = (u1, v1, 1)T,则有
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\left(\begin{array}{l} u_{1} \\ v_{1} \\ 1 \end{array}\right)=\left(\begin{array}{llll} 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) su1v11=t1t5t9t2t6t10t3t7t11t4t8t12XYZ1
用最后一行消去s,得
u 1 = t 1 X + t 2 Y + t 3 Z + t 4 t 9 X + t 10 Y + t 11 Z + t 12 v 1 = t 5 X + t 6 Y + t 7 Z + t 8 t 9 X + t 10 Y + t 11 Z + t 12 u_{1}=\frac{t_{1} X+t_{2} Y+t_{3} Z+t_{4}}{t_{9} X+t_{10} Y+t_{11} Z+t_{12}} \quad v_{1}=\frac{t_{5} X+t_{6} Y+t_{7} Z+t_{8}}{t_{9} X+t_{10} Y+t_{11} Z+t_{12}} u1=t9X+t10Y+t11Z+t12t1X+t2Y+t3Z+t4v1=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 \boldsymbol{t}_{1}=\left(t_{1}, t_{2}, t_{3}, t_{4}\right)^{T}, \boldsymbol{t}_{2}=\left(t_{5}, t_{6}, t_{7}, t_{8}\right)^{T}, \boldsymbol{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
则有
t 1 T P − t 3 T P u 1 = 0 t 2 T P − t 3 T P v 1 = 0 \begin{array}{l} \boldsymbol{t}_{1}^{T} \boldsymbol{P}-\boldsymbol{t}_{3}^{T} \boldsymbol{P} u_{1}=0 \\ \boldsymbol{t}_{2}^{T} \boldsymbol{P}-\boldsymbol{t}_{3}^{T} \boldsymbol{P} v_{1}=0 \end{array} t1TPt3TPu1=0t2TPt3TPv1=0
因此一对特征点提供了两个方程, 也就是两个约束,由于t共12个自由度,因此最少需要6对特征点,便可解出t。
当匹配点数大于六对时,便可以用奇异值分解求其最小二乘解。
2.P3P
仅需要三对匹配点,其输入数据为三对3D-2D匹配点,对于3D点A,B,C,其对应2D匹配点为a,b,c。
PnP问题求解_第1张图片
这里之前看的一些博客可能写成了▲Oab相似于▲OAB,看了好久才明白,AB的深度是未知的,如果默认平行的话相当于世界点全部映在了成像平面的平行平面上,后续余弦定理求解并不依赖于相似,只需要∠Oab=∠OAB即可,即
PnP问题求解_第2张图片

记x=OA/OC,y=OB/OC,得
PnP问题求解_第3张图片
记v = AB²/OC²,uv = BC²/OC²,wv = AC²/OC²,则有
PnP问题求解_第4张图片
以v化简得
在这里插入图片描述
其中,A,B,C,a,b,c的坐标是已知的,因此cos⟨a, b⟩, cos⟨b, c⟩, cos⟨a, c⟩,v = AB²/OC²,uv = BC²/OC²,wv = AC²/OC²是可以求出的,于是便得到关于x和y的二元二次方程,通过吴消元法解出。类似于通过本质矩阵E求解,P3P方法最多会得到四个解,可以通过验证点来判断。但P3P只能利用三个点的信息,多出来的匹配点的信息无法利用,并且如果3D或2D点受噪声影响,或存在错误匹配,则算法失效。

你可能感兴趣的:(slam)