对极几何的基础矩阵和本质矩阵3

对极几何系列篇：
对极几何的基础矩阵和本质矩阵1
对极几何的基础矩阵和本质矩阵2

前面介绍了对极几何的原理和基础矩阵F的求解，接下来介绍如何求解E矩阵和根据E矩阵分解相机的姿态 R和t。
世界坐标点P，在两个相机上的投影分别为p1，p2， 内参矩阵为k1, K2, 外参为[R1 T1], [R2 T2]

计算E矩阵

根据基础矩阵F
$$F=K_2^{-T}E{K}_1^{-T}$$
K1,K2是已知的，因而可以根据F矩阵计算E，E矩阵是奇异矩阵，det(E)=0, 并且有两个相等的特征值。这样解出来的E矩阵不一定满足该性质，需要SVD分解。

由于本质矩阵的尺度等价性，S也可以设置为 diag([1 1 0])

从E矩阵中分解相机姿态R和t

分解的结果是两个相机之间的旋转和平移。假设左相机的外参[I 0], 右相机的为[R t]。
这里推导R和t.
$$\begin{gathered} p_1=R_{1}P+T_1 \\ {p}_2=R_{2}P+T_2 \end{gathered}$$
那么
$$p_2=R_2{R}_1^{-1}{p}_1+T_2-R_2{R}_1^{-1}{T}_1$$
所以
$$\{\begin{array}{c}R=R_2{R}_1^{-1}\\ t=T_2-R{T}_1\end{array}$$

如果已知两个相机的外参，比如同时拍摄同一张棋盘格，那么R和t ，其实就可以解出来了。
$$E=[t_{\times }]R$$

但在只知道特征点匹配的情况下，只能通过矩阵分解的方式。
矩阵E分解步骤：

1. 对E进行分解， $E=US{V}^T$, 并且 $det(U{V}^T)=1$.

2. 对任意一个E，存在两种可能的t，R与它对应：
   $$\begin{gathered} t_{1\times }=U{R}_z(\frac{\pi }{2})S{U}^T \\ {t}_{2\times }=U{R}_z(-\frac{\pi }{2})S{U}^T \\ {R}_1=U{R}_z^T(\frac{\pi }{2})S{V}^T \\ {R}_2=U{R}_z^T(-\frac{\pi }{2})S{V}^T \end{gathered}$$
   其中$R_z(\frac{\pi }{2})$ 是沿z轴旋转90度,也就是
   $$\begin{gathered} R_z^T(\frac{\pi }{2})=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right] \\ {R}_z^T(-\frac{\pi }{2})=\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right] \end{gathered}$$

3. 这样子就会有4种解，如何确定一种解呢，根据已知的世界坐标点，应该在两个相机的前方，也就是坐标的z值是大于0的，那么代入就可以确定R和t。