第四篇 对极几何、三角化和PNP运算

1. 对极几何运算

1.1 推导过程

假设在第一帧的坐标系下P的空间位置是
P=[XYZ]′ P=[X\quad Y \quad Z]' P=[XYZ]
R,t 为两个坐标系的相机运动,则可以得到相机归一化平面坐标的方程。
s1∗x1=Ps2∗x2=R∗P+t s_1*x_1=P\\ s_2*x_2=R*P+t s1x1=Ps2x2=RP+t
s_1和s_2位尺度因子进一步得出
s2∗x2=s1∗R∗x1+t s_2*x_2=s_1*R*x_1+t s2x2=s1Rx1+t
两边同时叉乘t
s2∗t2x=s1∗tR∗x1 s_2*t^x_2=s_1*t^R*x_1 s2t2x=s1tRx1
然后两边同时x_2^T,
s2∗x2T∗t∧x2=s1∗x2T∗t∧R∗x1 s_2*x_2^T*t^\wedge x_2=s_1*x_2^T*t^\wedge R*x_1 s2x2Ttx2=s1x2TtRx1
右边的式子为0。
x2T∗t∧R∗x1=0 x_2^T*t^\wedge R*x_1 = 0 x2TtRx1=0
E=t^R是本质矩阵。

1.2 本质矩阵数学求解

[u1v11][e1e2e3e4e5e6e7e8e9][u2v21]=0 \begin{bmatrix} u1 & v1 & 1 \end{bmatrix} \begin{bmatrix} e1 & e2 & e3\\ e4 & e5 & e6\\ e7 & e8 & e9 \end{bmatrix} \begin{bmatrix} u2 \\ v2 \\ 1 \end{bmatrix}=0 [u1v11]e1e4e7e2e5e8e3e6e9u2v21=0
[u1∗u2u1∗v2u1v1∗u2v1∗v2v1u2v21][e1e2e3e4e5e6e7e8e9]=0 \begin{bmatrix} u1*u2 & u1*v2 & u1 & v1*u2 & v1*v2 & v1 & u2 & v2 & 1 \end{bmatrix} \begin{bmatrix} e1 \\ e2 \\ e3 \\ e4 \\ e5 \\ e6 \\ e7 \\ e8 \\ e9 \end{bmatrix}=0 [u1u2u1v2u1v1u2v1v2v1u2v21]e1e2e3e4e5e6e7e8e9=0
求解使用传统的8点法求解。构成8*9的系数矩阵,根据SVD分解法可以得到E矩阵。由于尺度效应,E有无穷多组解,求解出模值为1的那一组。 (这里使用SVD分解的一个应用,求解齐次线性方程组)

1.3 通过本质矩阵求解位姿

E=U∗Σ∗VT E=U *\Sigma *V^T E=UΣVT
可以得到以下两组结果
t1∧=U∗Rz(π2)∗Σ∗UT,R1=U∗Rz(π2)∗VTt1∧=U∗Rz(−π2)∗Σ∗UT,R1=U∗Rz(−π2)∗VT t_1^\wedge=U*R_z(\frac{\pi}{2})*\Sigma*U^T,R_1=U*R_z(\frac{\pi}{2})*V^T\\ \\ t_1^\wedge=U*R_z(-\frac{\pi}{2})*\Sigma*U^T,R_1=U*R_z(-\frac{\pi}{2})*V^T t1=URz(2π)ΣUT,R1=URz(2π)VTt1=URz(2π)ΣUT,R1=URz(2π)VT
通时利用-E也可以得到两组解。不过,任意一点在相机中具有正的深度,带入任意一点带入四组解就可以求出正确的唯一解。

2. 三角化运算

s为尺度因子,p[u,v]为归一化平面坐标,P为特征三维世界点(待求),R,t 代表位姿。
s∗p=R∗P+t=T∗P s*p=R*P+t=T*P sp=RP+t=TP
s∗u1=Trow1∗Ps∗v1=Trow2∗Ps=Trow3∗P s*u_1=T_{row1}*P\\ s*v_1=T_{row2}*P\\ s=T_{row3}*P su1=Trow1Psv1=Trow2Ps=Trow3P
构造出关于P的方程
u1∗Trow3∗P−Trow1∗P=0v1∗Trow3∗P−Trow2∗P=0 u_1*T_{row3}*P-T_{row1}*P=0 \\ v_1*T_{row3}*P-T_{row2}*P=0 u1Trow3PTrow1P=0v1Trow3PTrow2P=0
如果存在多次观测
[u1∗T1_row3−T1_row1v1∗T1_row3−T1_row2u2∗T2_row3−T2_row1v2∗T2_row3−T2_row2]∗P=0 \begin{bmatrix} u_1*T_{1\_row3} -T_{1\_row1} \\ v_1*T_{1\_row3} -T_{1\_row2} \\ u_2*T_{2\_row3} -T_{2\_row1} \\ v_2*T_{2\_row3} -T_{2\_row2} \end{bmatrix}*P=0 u1T1_row3T1_row1v1T1_row3T1_row2u2T2_row3T2_row1v2T2_row3T2_row2P=0
通过SVD分解求出齐次线性方程组的解P,把P的最后一维归一化就可以得到坐标。

3. PNP运算

s为尺度因子,p[u,v]为归一化平面坐标,P为特征三维世界点,R,t 代表位姿组成T(待求)。
s∗p=R∗P+t=T∗P s*p=R*P+t=T*P sp=RP+t=TP
s∗u1=Trow1∗Ps∗v1=Trow2∗Ps=Trow3∗P s*u_1=T_{row1}*P\\ s*v_1=T_{row2}*P\\ s=T_{row3}*P su1=Trow1Psv1=Trow2Ps=Trow3P
构造关于T的方程
u1∗Trow3∗P−Trow1∗P=0v1∗Trow3∗P−Trow2∗P=0 u_1*T_{row3}*P-T_{row1}*P=0 \\ v_1*T_{row3}*P-T_{row2}*P=0 u1Trow3PTrow1P=0v1Trow3PTrow2P=0

[−PT0u1∗PT−PTv1∗PT0]∗[Trow1TTrow2TTrow3T]=0 \begin{bmatrix} -P^T & 0 & u_1*P^T \\ -P^T & v_1*P^T& 0 \\ \end{bmatrix}* \begin{bmatrix} T_{row1}^T\\T_{row2}^T\\T_{row3}^T \end{bmatrix}=0 [PTPT0v1PTu1PT0]Trow1TTrow2TTrow3T=0
如果存在多组解,再叠加左边矩阵的行数
[−P1T0u1∗P1T−P1Tv1∗P1T0−P2T0u2∗P2T−P2Tv2∗P2T0−P3T0u3∗P3T−P3Tv3∗P3T0−P4T0u4∗P4T−P4Tv4∗P4T0−P5T0u5∗P5T−P5Tv5∗P5T0−P6T0u6∗P6T−P6Tv6∗P6T0]∗[Trow1TTrow2TTrow3T]=0 \begin{bmatrix} -P_1^T & 0 & u_1*P_1^T \\ -P_1^T & v_1*P_1^T& 0 \\ -P_2^T & 0 & u_2*P_2^T \\ -P_2^T & v_2*P_2^T& 0 \\ -P_3^T & 0 & u_3*P_3^T \\ -P_3^T & v_3*P_3^T& 0 \\ -P_4^T & 0 & u_4*P_4^T \\ -P_4^T & v_4*P_4^T& 0 \\ -P_5^T & 0 & u_5*P_5^T \\ -P_5^T & v_5*P_5^T& 0 \\ -P_6^T & 0 & u_6*P_6^T \\ -P_6^T & v_6*P_6^T& 0 \\ \end{bmatrix}* \begin{bmatrix} T_{row1}^T\\T_{row2}^T\\T_{row3}^T \end{bmatrix}=0 P1TP1TP2TP2TP3TP3TP4TP4TP5TP5TP6TP6T0v1P1T0v2P2T0v3P3T0v4P4T0v5P5T0v6P6Tu1P1T0u2P2T0u3P3T0u4P4T0u5P5T0u6P6T0Trow1TTrow2TTrow3T=0
SVD分解可以得到T矩阵。分离尺度并正交化的到R和t。
svd(T(1:3,1:3))=U∗Σ∗VTR=±U∗VTβ=±tr(Σ)/3 svd(T(1:3,1:3))=U*\Sigma*V^T\\ R=\pm U*V^T\\ \beta=\pm tr(\Sigma)/3 svd(T(1:3,1:3))=UΣVTR=±UVTβ=±tr(Σ)/3
beta是尺度因子。通过确定深度必须为正,确定符号。
β∗(x∗T31+y∗T32+z∗T33+T34)>0 \beta *(x*T_{31} + y*T_{32} + z*T_{33} + T_{34})>0 β(xT31+yT32+zT33+T34)>0
进而确定t
t=β∗(T1,4,T2,4,T3,4) t=\beta*(T_{1,4},T_{2,4},T_{3,4}) t=β(T1,4,T2,4,T3,4)

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值