ORBSLAM2系列-矩阵模型H和F的分解

矩阵模型的分解

经过前面计算的单应矩阵H和基础矩阵F，下面就开始使用计算的矩阵进行位姿（R,t）的恢复，这其中的公式较多，所以使用手写的方式，便于表达。

单应矩阵H分解

根据之前计算单应矩阵H时的原理，可知单应矩阵H如下：

$$H=K(R-\frac{t{n}^T}{d})K^{-1}\Rightarrow p_2\simeq H{p}_1$$

表明的是两张图像中同一空间点的特征点之间的关系。那么H分解成为R（旋转矩阵）和t（位移向量）就如下所示（较复杂，需要明白每一步时候的已知条件和未知条件）：

基础矩阵F分解

根据之前计算基础矩阵F时的原理，可知基础矩阵的计算是由对极约束得到的，也就是如下：

$$E=\hat{t}R,F=k^{-T}E{K}^{-1},x_2^{T}E{x}_1=p_2^{T}F{p}_1=0$$

那么想要从基础矩阵F恢复出R,t，就可以使用本质矩阵E，因为$E=\hat{t}R$， 方便计算。

本质矩阵E的性质

尺度等价性

本质矩阵E是由对极约束定义的。由于对极约束是等式为零的约束，所以对E乘以任意非零常数后，对极约束仍然满足。我们把这件事情称为E在不同尺度下是等价的。这是视觉slam十四讲中的内容。那么我们需要理解其在几何上的意义，以下内容参考这篇文章

如果给本质矩阵E乘以一个常数k，那么就相当于在t乘上一个系数。如上图中，由 $O_1$ 到 $O_2$ 和由 $O_1^{\prime }$ 到 $O_2^{\prime }$ 都是相同的R，但是t不同，$t_1,t_2$ 完全平行，且只相差一个常数系数。

因此，本质矩阵E的尺度等价性，指的就是单目相机由于缺乏深度信息，导致其具有尺度不变性

内在性质

根据计算基础矩阵F的原理中，说明了基础矩阵F和本质矩阵E的秩是2

本质矩阵$E=\hat{t}R$ 中，$R$ 是正交矩阵，乘以一个正交矩阵是不会改变矩阵的秩和奇异值的，因此决定本质矩阵的奇异值实际上只与反对称矩阵$\hat{t}$ 有关，而反对称矩阵根据其特有的对称性，两个奇异值一定是相等的，也就是说对本质矩阵E进行SVD分解时：

$$E=U\Sigma V^T其中\Sigma =diag(a,a,0)$$

本质矩阵E的分解

根据上面的两条性质，再根据SVD分解，就可求解R,t。

注意：因为旋转矩阵R是一个行列式为1的正交矩阵，所以在求解得到的R中，需要将行列式为-1的R去相反，即 $R=-R$

三角化求解地图点

三角化就是在已知位姿R（旋转矩阵）和t（位移向量）后，根据匹配的特征点计算出地图点在世界坐标系的坐标（设定第一帧是世界坐标系）

点的坐标

地图点坐标转换：就是相机成像原理中说明的，一个地图点的坐标经过旋转矩阵和位移向量计算后，转换到了相机坐标系中：

$$X_c=R{X}_w+t\Rightarrow \left(\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right)=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\left(\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right)$$

光心点O坐标：就如上面的转换公式，由于光心在相机坐标系中的坐标是 $X_c=(0,0,0)$ ，那么其在世界坐标系中的坐标就是：

$$X_{ow}=-R^{T}t$$

地图点求解

根据相机成像的原理：
$$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\frac{1}{Z}\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)\left[\begin{array}{cc}R& t\end{array}\right]\left(\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right)=\frac{1}{Z}KT\left(\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right)$$

将中间的 $KT$ 使用一个投影矩阵 $P=KT$ 计算，可以表示为：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{Z}\left[\begin{array}{cccc}p1& p2& p3& p4\\ p5& p6& p7& p8\\ p9& p10& p11& p12\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]\Rightarrow \left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{Z}\left[\begin{array}{ccc}& P_0& -\\ & P_1& -\\ & P_2& -\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$$

对等式两边同时乘像素齐次坐标的反对称矩阵，使得等式左边为0，得到了如下等式：

$$\left[\begin{array}{c}v{P}_2-P_1\\ P_0-u{P}_2\\ u{P}_1-v{P}_0\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=0$$

而这只是一个特征点形成的等式，如果一个空间点在两帧图像上的特征点（2个特征点）组合在一起，就形成了 $AX=0$ 的形式，可以求得空间坐标 $X$

$$\left[\begin{array}{c}v{P}_2-P_1\\ P_0-u{P}_2\\ v^{\prime }{P}_2^{\prime }-P_1^{\prime }\\ P_0^{\prime }-u^{\prime }{P}_2^{\prime }\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=0$$

上面公式中，一帧图像上的特征点坐标是 $(u,v)$ ，对应的投影矩阵是 $P$，另一帧图像的匹配特征点坐标是 $(u^{\prime },v^{\prime })$ ，对应的投影矩阵是 $P^{\prime }$ 。使用之前的最小二乘法，对 $A$ 进行SVD分解，得到的右奇异矩阵的最后一列就是最优解。

在ORBSLAM2中使用三角化计算的地图点，还需要经过很多判定条件才能最终认定这个地图点是合格的

ORBSLAM2应用

由于代码中的注释都很详细，这里就并不列举。可以参考这里的流程，查看代码中的逻辑。（此时已经得到了计算的最好的矩阵模型）

单应矩阵H的分解流程 👇

• 1.按照上面的分解原理，将H矩阵分解：
• • 1-1.计算 $A=K^{-1}HK$ ，后对 $A$ 进行SVD分解，使用分解后的奇异矩阵进行计算
  • 1-2.根据 $x_1,x_3$ 的4中组合，按照上面推导的公式，分别得到 $d^{\prime }>0,d^{\prime }<0$ 的各4个 $R^{\prime },t^{\prime }$
  • 1-3.根据 $R=xU{R}^{\prime }{V}^T,t=U{t}^{\prime }$ ，计算得到一共8组 $R,t$
• 2.分别对8组解进行三角化，判断哪个解是三角化最好的：
• • 2-1.分别计算两帧图像的两个投影矩阵$P_1=K\ast [I|0]$（因为是以第一帧图像的坐标系为世界坐标系，所以这里的 $[R|t]=[T|0]$），$P_2=K\ast [R|t]$
  • 2-2.对每一个匹配的特征点进行三角化，并判断生成的空间点是否合法：
  • • 2-2-1.坐标中是否存在无穷大的数值
    • 2-2-2.根据 $a\cdot b=|a||b|cos<a,b>$ 得到该点到两个光心之间的夹角（视差角），判断视差角是否过大，并判断深度值是否为负数
    • 2-2-3.将空间点坐标分别反向投影到两帧图像，判断与特征点坐标的重投影误差是否太大
    • 2-2-4.经过上面的重重检查，最终留下的点就是三角化成功的点，称为good点
  • 2-3.对8组解分别计算上面打的三角化过程，得到8组解中good点最多的解，作为最优解，同时找到次优解，用于后面再次判断
• 3.根据找到的最优解和次优解，再次判断此最优解是否合格：
• • 3-1.good点数最优解要明显大于次优解，一般取0.75
  • 3-2.视差角大于规定阈值（此视差角是计算每组解的三角化点时统计的一个比较合适的视差角）
  • 3-3.good点数要大于规定的最小被三角化的点数量
  • 3-4.good点数要达到所有特征点数量的90%以上
• 4.以上的条件全部达到，认为单应矩阵H的分解成功，否则失败

基础矩阵F的分解流程 👇

• 1.按照上面的分解原理，将F矩阵分解：
• • 1-1.计算 $E=K^{T}FK$ ，得到本质矩阵E，使用上面的公式分解E
  • 1-2.根据分解公式，得到 $R_1=UW{V}^T,R_2=U{W}^T{V}^T,t_1=U_2,t_2=-U_2$ 共组合成4组解，注意，在计算R时，如果R行列式为负数，需要取反 $R=-R$
• 2.分别对4组解进行三角化，判断哪个解是三角化最好的：
• • 2-1.分别计算两帧图像的两个投影矩阵 $ P_1=K*[I|0] $（因为是以第一帧图像的坐标系为世界坐标系，所以这里的 $[R|t]=[T|0]$ ），$P_2=K\ast [R|t]$
  • 2-2.对每一个匹配的特征点进行三角化，并判断生成的空间点是否合法：
  • • 2-2-1.坐标中是否存在无穷大的数值
    • 2-2-2.根据 $a\cdot b=|a||b|cos<a,b>$ 得到该点到两个光心之间的夹角（视差角），判断视差角是否过大，并判断深度值是否为负数
    • 2-2-3.将空间点坐标分别反向投影到两帧图像，判断与特征点坐标的重投影误差是否太大
    • 2-2-4.经过上面的重重检查，最终留下的点就是三角化成功的点，称为good点
  • 2-3.对4组解分别计算上面的三角化过程，得到4组解中good点最多的解
• 3.根据找到的最优解，再次判断此最优解是否合格：
• • 3-1.最优解的good点数量一定要是远远大于其他解的，一般设为是0.7
  • 3-2.最优解的good点数量要大于所要求的最少三角化3D点的数量
  • 3-3.三角化视差角必须大于所设置的最小视差角
• 4.以上的条件全部达到，认为基础矩阵F的分解成功，否则失败