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
s1∗x1=Ps2∗x2=R∗P+t
s_1和s_2位尺度因子进一步得出
s2∗x2=s1∗R∗x1+t
s_2*x_2=s_1*R*x_1+t
s2∗x2=s1∗R∗x1+t
两边同时叉乘t
s2∗t2x=s1∗tR∗x1
s_2*t^x_2=s_1*t^R*x_1
s2∗t2x=s1∗tR∗x1
然后两边同时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
s2∗x2T∗t∧x2=s1∗x2T∗t∧R∗x1
右边的式子为0。
x2T∗t∧R∗x1=0
x_2^T*t^\wedge R*x_1 = 0
x2T∗t∧R∗x1=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
[u1∗u2u1∗v2u1v1∗u2v1∗v2v1u2v21]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∧=U∗Rz(2π)∗Σ∗UT,R1=U∗Rz(2π)∗VTt1∧=U∗Rz(−2π)∗Σ∗UT,R1=U∗Rz(−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
s∗p=R∗P+t=T∗P
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
s∗u1=Trow1∗Ps∗v1=Trow2∗Ps=Trow3∗P
构造出关于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
u1∗Trow3∗P−Trow1∗P=0v1∗Trow3∗P−Trow2∗P=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
u1∗T1_row3−T1_row1v1∗T1_row3−T1_row2u2∗T2_row3−T2_row1v2∗T2_row3−T2_row2∗P=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
s∗p=R∗P+t=T∗P
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
s∗u1=Trow1∗Ps∗v1=Trow2∗Ps=Trow3∗P
构造关于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
u1∗Trow3∗P−Trow1∗P=0v1∗Trow3∗P−Trow2∗P=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
[−PT−PT0v1∗PTu1∗PT0]∗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
−P1T−P1T−P2T−P2T−P3T−P3T−P4T−P4T−P5T−P5T−P6T−P6T0v1∗P1T0v2∗P2T0v3∗P3T0v4∗P4T0v5∗P5T0v6∗P6Tu1∗P1T0u2∗P2T0u3∗P3T0u4∗P4T0u5∗P5T0u6∗P6T0∗Trow1TTrow2TTrow3T=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=±U∗VTβ=±tr(Σ)/3
beta是尺度因子。通过确定深度必须为正,确定符号。
β∗(x∗T31+y∗T32+z∗T33+T34)>0
\beta *(x*T_{31} + y*T_{32} + z*T_{33} + T_{34})>0
β∗(x∗T31+y∗T32+z∗T33+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)