基础矩阵F的计算(2D-2D)

目的：通过特征点匹配，计算两个图像对应的相机位姿，理解SLAM的过程，记录下，供后面理解。

之前一篇博客，计算过fundmental 矩阵。详细见那篇博客。
计算极限的方程。

这篇博客记录计算fundmental矩阵，再通过fundmental矩阵转化为相机的$[R,t]$

计算fundmetal矩阵，还是通过极限约束。计算fundmental矩阵。

假设

１）$O_1$为世界坐标系。通过$[R,t]$矩阵转化到$O_2$坐标系。$x$为$x_1,x_2$转换到世界坐标系的交点。

２）$O_1$相机的内参矩阵可以写为$K_1$,$O_2$的相机内参矩阵为$K_2$;因为相机$O_1$为世界坐标系下,因此它的外参矩阵为$[I,0]$;同时相机$O_2$为$[R,t]$。

因为相机的投影公式
$d_1{x}_1=K_{1}Ix(1)$
$d_2{x}_2=K_2(Rx+t)(2)$

由$(1)$公式得到如下：
$$d_1{x}_1=K_{1}Ix=>x=d_1{K}_1^{-1}{x}_1=d_1\widetilde{x_1}(3)$$
其中$\widetilde{x_1}=K_1^{-1}{x}_1$表示像平面的顶点，意思就是$d_1=1$(深度)时候的平面，在$x{O}_1$射线和这个像平面交点，坐标$\widetilde{x_1}$在$x{O}_1$射线上。

由公式$(2)$可以得到公式：
$$\begin{gathered} d_2{x}_2=K_2(Rx+t) \\ =>x=R^{-1}(d_2{K}_2^{-1}{x}_2-t)=R^{-1}(d_2\widetilde{x_2}-t)(4) \end{gathered}$$
其中$\widetilde{x_2}$是在$O_2$坐标系下，$O_{2}x$和深度$d_2=1$的相交点，它也在$O_{2}x$的射线上。

有公式$(3)(4)$得到：
$$\begin{gathered} d_1\widetilde{x_1}=R^{-1}(d_2\widetilde{x_2}-t) \\ =>d_2\widetilde{x_2}=R{d}_1\widetilde{x_1}+t \\ =>d_2(t\times \widetilde{x_2})=d_1(t\times R\widetilde{x_1})+t\times t(5) \end{gathered}$$
因为公式中$t\times t=0$,因此公式(5)中后面一项为$0$。
公式$(5)$可以得到以下：
$$\begin{gathered} d_2(t\times \widetilde{x_2})=d_1(t\times R\widetilde{x_1}) \\ =>d_2({\widetilde{x_2}}^{T}R(t\times \widetilde{x_2}))=d_1({\widetilde{x_2}}^T(t\times R\widetilde{x_1}))(6) \end{gathered}$$
同样在叉乘中$t\times \widetilde{x_2}$和$\widetilde{x_2}$垂直，因为叉乘的几何意义。公式$(6)$为$0$因此得到：
$$d_1({\widetilde{x_2}}^T(t\times R\widetilde{x_1}))=0(7)$$
简化公式$(7)$得到如下：
$$\begin{gathered} {\widetilde{x_2}}^T(t\times R\widetilde{x_1})=0 \\ =>{\widetilde{x_2}}^{T}E\widetilde{x_1}=0(8) \end{gathered}$$
其中$E=t\times R$

代入公式$K_1^{-1}{x}_1=\widetilde{x_1}$;$K_2^{-1}{x}_2=\widetilde{x_2}=>{\widetilde{x_2}}^T=x_2^T{K}_2^{-T}$;得到如下：
$$\begin{gathered} {\widetilde{x_2}}^{T}E\widetilde{x_1}=0 \\ =>x_2^T{K}_2^{-T}E{K}_1^{-1}{x}_1=0 \\ =>x_2^{T}F{x}_1=0(9) \end{gathered}$$
其中$F=K_2^{-T}E{K}_1^{-1}$
以上是基于极限约束计算的$F$矩阵，它也是相交的内外参数的乘积。至于怎么求得$F$下面将介绍。

比较基本的方法就是下面的八点法
八点线性方法获取相机直接的fundmental矩阵。
先看一下基础矩阵$F$，

1. $3\times 3$的矩阵，但是它的秩是$2$
2. 有７个自由度
3. 满足上述的极限约束
4. 有奇异值约束$[{\sigma }_1,{\sigma }_2,0]$(有两个非０奇异值)

因此可以得到８个匹配点可以优化得到$F$矩阵。

对于一对匹配点$[u_1,v_1,1]$, $[u_2,v_2,1]$,它的基础矩阵为
$$F=\left[\begin{array}{ccc}{F}_{11}& F_{12}& F_{13}\\ F_{21}& F_{22}& F_{23}\\ F_{31}& F_{32}& F_{33}\end{array}\right]$$

用极限约束可以得到如下：
$$[u_1,v_1,1]F\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]=0$$

将$F$矩阵拉成一个线性，可以得到如下公式
$$[u_2{u}_1,u_2{v}_1,u_2,u_1{v}_2,v_1{v}_2,v_2,u_1,v_1,1]f=0$$
其中$f=[F_{11},F_{12},F_{13},F_{21},F_{22},F_{23},F_{31},F_{32},F_{33}{]}^T$

上述的公式，如果有８个对应点，就可以计算得到$F$矩阵。
上述的公式，如果有８个匹配点，它们可以组成一个矩阵，就可以求得$f$的值。如果大于$8$个匹配点，就可以利用最小二乘法求解。得到$F$.

上述方法有自己缺点，不能保证奇异值约束，因此需要重构。重构的公式如下：
$$min||F-\hat{F}||,svd(F)=[{\sigma }_1,{\sigma }_2,0]$$

一般情况下奇异值分解如下：
$$\begin{gathered} \hat{F}=US{V}^T \\ withS=diag({\sigma }_1,{\sigma }_2,{\sigma }_3) \end{gathered}$$

而基础矩阵只有两个非$0$奇异值。因此得到：
$$F=Udiag({\sigma }_1,{\sigma }_2,0)V^T$$
利用上述的奇异值约束对基础矩阵进行重构。

上述的只是单独获取８对匹配点$(x_1^n,x_2^n)$
但是匹配顶点多数，因此需要用RANSAC作为一个框架计算基础矩阵。
下面的RANSAC的框架。
它的算法流程：

1. 随机采样8个顶点匹配点
2. 8个顶点求解基础矩阵$\hat{F}$
3. 奇异值约束获取基础矩阵$F$
4. 计算误差，统计内点个数
5. 重复上述过程，选择内点数最多的结果
6. 对所有内点执行2,3，重新计算$F$

内点判断标准-Sampson Distance
$$d(x_1,x_2)=\frac{(x_2^{T}F{x}_1{)}^2}{(F{x}_1{)}_x^2+(F{x}_2{)}_y^2+(x_2^{T}F{)}_x^2+(x_2^{T}F{)}_y^2}$$
其中有$d(x_1,x_2)<\tau$

上述使用ransac计算基础矩阵。

上述计算基础矩阵$F$，要获取本征矩阵$E$
$$\begin{gathered} \hat{E}=K_2^{T}F{K}_1 \\ =>\hat{E}=Udiag({\sigma }_1,{\sigma }_2,0)V^T \\ =>E=Udiag(\frac{{\sigma }_1+{\sigma }_2}{2},\frac{{\sigma }_1+{\sigma }_2}{2},0)V^T \end{gathered}$$