slam基础——基础矩阵和本质矩阵

1. 参考文献

【1】https://blog.csdn.net/electech6/article/details/83818218?spm=1001.2014.3001.5502

2. 推导过程

2.1 从一张图开始

首先是下面这幅经典的图，从图中可以看出
（1）世界中三维坐标点P，投影在相机C0下，得到点p0；如果单从p0出发，是无法得到点P的实际位置，整条C0P线上的点都会投影落在p0点处。

（2）从相机C1看，直线C0P上的点都会投影落到极线上，如果此时已知p1的位置，在没有误差的情况下，就可以确定空间点P的具体位置。

（3）C0PC1三点共面。

（4）由于三点共面，所以有：
$$C_{0}P\cdot (C_0{C}_1\times C_{1}P)=0$$

2.2 统一坐标系表达

假设，P点在C0坐标系下坐标$[X,Y,Z{]}^T$，p0点图像像素坐标$[u_0,v_0,1{]}^T$。坐标系C0和坐标系C1之间的变换用R和t矩阵表示。

根据相机模型有：
$$s_0\cdot p_0=KP$$
展开可得：
$$\left[\begin{array}{c}{s}_{0}u\\ s_{0}v\\ s_0\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]$$
求解得到：
$$\begin{array}{rl}X& =\frac{s_0}{f_x}(u-c_x)\\ Y& =\frac{s_0}{f_y}(v-c_y)\\ Z& =s_0\end{array}$$
从上式可以看出：
（1）$s_0$可以类似地看作这个向量的长度，
（2）剩下的部分可以类似地看作$C_{0}P$向量的方向，这里的方向是表达在C0坐标系下。
（3）后续的过程仅仅用$C_{0}P$的方向，因此取$s_0=1$，将上述求解过程记作:
$$C_{0}P=K^{-1}{p}_0$$
同理，可以将$C_{1}P$向量方向表达成类似形式，同时考虑将该方向向量表达在C_0坐标系下，因此有：
$$C_{1}P=R{K}^{-1}{p}_1$$
上式中R矩阵的作用就是贱向量$C_{1}P$旋转到坐标系C0下。

接着利用前述三点共面的表达式
$$C_{0}P\cdot (C_0{C}_1\times C_{1}P)=0$$

将前文求解的公式分别代入可得：
$$p_0^T{K}^{-T}{t}^{\wedge }R{K}^{-1}{p}_1=0$$
中间部分就是基础矩阵，最中间部分是本质矩阵。