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

1. 对极几何运算

1.1 推导过程

假设在第一帧的坐标系下P的空间位置是
$$P=[XYZ{]}^{\prime }$$
R,t 为两个坐标系的相机运动，则可以得到相机归一化平面坐标的方程。
$$\begin{gathered} s_1\ast x_1=P \\ {s}_2\ast x_2=R\ast P+t \end{gathered}$$
s_1和s_2位尺度因子进一步得出
$$s_2\ast x_2=s_1\ast R\ast x_1+t$$
两边同时叉乘t
$$s_2\ast t_2^x=s_1\ast t^R\ast x_1$$
然后两边同时x_2^T,
$$s_2\ast x_2^T\ast t^{\wedge }{x}_2=s_1\ast x_2^T\ast t^{\wedge }R\ast x_1$$
右边的式子为0。
$$x_2^T\ast t^{\wedge }R\ast x_1=0$$
E=t^R是本质矩阵。

1.2 本质矩阵数学求解

$$\left[\begin{array}{ccc}u1& v1& 1\end{array}\right]\left[\begin{array}{ccc}e1& e2& e3\\ e4& e5& e6\\ e7& e8& e9\end{array}\right]\left[\begin{array}{c}u2\\ v2\\ 1\end{array}\right]=0$$
$$\left[\begin{array}{ccccccccc}u1\ast u2& u1\ast v2& u1& v1\ast u2& v1\ast v2& v1& u2& v2& 1\end{array}\right]\left[\begin{array}{c}e1\\ e2\\ e3\\ e4\\ e5\\ e6\\ e7\\ e8\\ e9\end{array}\right]=0$$
求解使用传统的8点法求解。构成8*9的系数矩阵，根据SVD分解法可以得到E矩阵。由于尺度效应，E有无穷多组解，求解出模值为1的那一组。 (这里使用SVD分解的一个应用，求解齐次线性方程组)

1.3 通过本质矩阵求解位姿

$$E=U\ast \Sigma \ast V^T$$
可以得到以下两组结果
$$\begin{gathered} t_1^{\wedge }=U\ast R_z(\frac{\pi }{2})\ast \Sigma \ast U^T,R_1=U\ast R_z(\frac{\pi }{2})\ast V^T \\ {t}_1^{\wedge }=U\ast R_z(-\frac{\pi }{2})\ast \Sigma \ast U^T,R_1=U\ast R_z(-\frac{\pi }{2})\ast V^T \end{gathered}$$
通时利用-E也可以得到两组解。不过，任意一点在相机中具有正的深度，带入任意一点带入四组解就可以求出正确的唯一解。

2. 三角化运算

s为尺度因子，p[u,v]为归一化平面坐标，P为特征三维世界点(待求)，R,t 代表位姿。
$$s\ast p=R\ast P+t=T\ast P$$
$$\begin{gathered} s\ast u_1=T_{row1}\ast P \\ s\ast v_1=T_{row2}\ast P \\ s=T_{row3}\ast P \end{gathered}$$
构造出关于P的方程
$$\begin{gathered} u_1\ast T_{row3}\ast P-T_{row1}\ast P=0 \\ {v}_1\ast T_{row3}\ast P-T_{row2}\ast P=0 \end{gathered}$$
如果存在多次观测
$$\left[\begin{array}{c}{u}_1\ast T_{1\_row3}-T_{1\_row1}\\ v_1\ast T_{1\_row3}-T_{1\_row2}\\ u_2\ast T_{2\_row3}-T_{2\_row1}\\ v_2\ast T_{2\_row3}-T_{2\_row2}\end{array}\right]\ast P=0$$
通过SVD分解求出齐次线性方程组的解P，把P的最后一维归一化就可以得到坐标。

3. PNP运算

s为尺度因子，p[u,v]为归一化平面坐标，P为特征三维世界点，R,t 代表位姿组成T(待求)。
$$s\ast p=R\ast P+t=T\ast P$$
$$\begin{gathered} s\ast u_1=T_{row1}\ast P \\ s\ast v_1=T_{row2}\ast P \\ s=T_{row3}\ast P \end{gathered}$$
构造关于T的方程
$$\begin{gathered} u_1\ast T_{row3}\ast P-T_{row1}\ast P=0 \\ {v}_1\ast T_{row3}\ast P-T_{row2}\ast P=0 \end{gathered}$$
即
$$\left[\begin{array}{ccc}-P^T& 0& u_1\ast P^T\\ -P^T& v_1\ast P^T& 0\end{array}\right]\ast \left[\begin{array}{c}{T}_{row1}^T\\ T_{row2}^T\\ T_{row3}^T\end{array}\right]=0$$
如果存在多组解，再叠加左边矩阵的行数
$$\left[\begin{array}{ccc}-P_1^T& 0& u_1\ast P_1^T\\ -P_1^T& v_1\ast P_1^T& 0\\ -P_2^T& 0& u_2\ast P_2^T\\ -P_2^T& v_2\ast P_2^T& 0\\ -P_3^T& 0& u_3\ast P_3^T\\ -P_3^T& v_3\ast P_3^T& 0\\ -P_4^T& 0& u_4\ast P_4^T\\ -P_4^T& v_4\ast P_4^T& 0\\ -P_5^T& 0& u_5\ast P_5^T\\ -P_5^T& v_5\ast P_5^T& 0\\ -P_6^T& 0& u_6\ast P_6^T\\ -P_6^T& v_6\ast P_6^T& 0\end{array}\right]\ast \left[\begin{array}{c}{T}_{row1}^T\\ T_{row2}^T\\ T_{row3}^T\end{array}\right]=0$$
SVD分解可以得到T矩阵。分离尺度并正交化的到R和t。
$$\begin{gathered} svd(T(1:3,1:3))=U\ast \Sigma \ast V^T \\ R=\pm U\ast V^T \\ \beta =\pm tr(\Sigma )/3 \end{gathered}$$
beta是尺度因子。通过确定深度必须为正，确定符号。
$$\beta \ast (x\ast T_{31}+y\ast T_{32}+z\ast T_{33}+T_{34})>0$$
进而确定t
$$t=\beta \ast (T_{1,4},T_{2,4},T_{3,4})$$