ORBSLAM2系列-矩阵模型H和F的计算

矩阵模型计算原理

对极约束

对极约束在视觉slam十四讲中讲解的比较详细，在这里就简单的介绍一些流程：

• 1.对于一个空间坐标$P(X,Y,Z)$，根据针孔相机模型可以得到两个像素点$p_1,p_2$在两个相机坐标系下的位置：
  $$s_1{p}_1=KP,s_2{p}_2=K(RP+t)参考：Z\left[\begin{array}{c}u\\ v\\ 1\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)\stackrel{\mathrm{def}}{=}KP$$

• 2.现考虑齐次坐标，那么$s_1{p}_1$和$p_1$就是尺度意义下的相等，上面投影关系就可以写成如下，其中$x_1,x_2$是归一化平面的坐标：

$$p_1\simeq KP,p_2\simeq K(RP+t)取：x_1=K^{-1}{p}_1,x_2=K^{-1}{p}_2$$

• 3.将上面的公式进行结合后，经过一系列的数学运算：

$$x_2\simeq R{x}_1+t\stackrel{\mathrm{左乘}\hat{\mathrm{t}}}{\Rightarrow }\hat{t}{x}_2\simeq \hat{t}R{x}_1\stackrel{\mathrm{左乘}{\mathrm{x}}_2^{\mathrm{T}}}{\Rightarrow }{x}_2^T\hat{t}{x}_2\simeq x_2^T\hat{t}R{x}_1$$

• 4.上面最后一个式子的左侧，$\hat{t}{x}_2$是一个与$t$和$x_2$都垂直的向量，那它再和$x_2$做内积时（点乘），将得到0。由此就得到了重点的对极约束：

$$\{\begin{array}{l}{x}_2^T\hat{t}R{x}_1=0\\ \\ p_2^T{K}^{-T}\hat{t}R{K}^{-1}{p}_1=0\end{array}$$

• • 基础知识：
  • 内积：就是$a\cdot b=a^{T}b=|a||b|cos(a,b)$，描述的是向量之间的投影关系，得到的是一个数值
  • 外积：就是$a\times b=\hat{a}b$，指的是用 $a$ 的反对称矩阵与 $b$ 相乘，得到的是一个向量，这个向量的方向与 $a,b$ 都垂直，大小是$|a||b|sin(a,b)$ ，是这两个向量张成的四边形的面积

• 5.得到了对极约束，就可以得到了基础矩阵F和本质矩阵E：

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

基础矩阵F

• 1.基础矩阵F就是根据上面的对极约束计算，描述的是特征点对 $p_1,p_2$ 的变化关系：

$$p_2^{T}F{p}_1=0\Rightarrow \left[\begin{array}{ccc}{u}_2& v_2& 1\end{array}\right]\left[\begin{array}{ccc}{f}_1& f_2& f_3\\ f_4& f_5& f_6\\ f_7& f_8& f_9\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=0$$

• 2.将上面矩阵形式展开，就转化为：

$$\left[\begin{array}{ccccccccc}{u}_1\ast u_2& v_1\ast u_2& u_2& u_1\ast v_2& v_1\ast v_2& v_2& u_1& v_1& 1\end{array}\right]\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\\ f_4\\ f_5\\ f_6\\ f_7\\ f_8\\ f_9\end{array}\right]=0$$

• 3.等式左边分别使用 $A,f$ 表示：

$$Af=0$$

一对点提供了一个约束方程，F共有9个元素，再加上尺度等价性（$Af=0$ 代表着这等式两边乘以任意数都成立，因此可以将最后一个 $f_9$ 设置为1）和秩为2，因此基础矩阵F共有7个自由度，所以8对点提供8个约束方程能够求解，这就是经典的8点法

• 为什么基础矩阵F的秩为2？
• 基础矩阵F在定义中是：$F=K^{-T}\hat{t}R{K}^{-1}$ 这其中所有的矩阵都是三阶的， $t$ 向量的反对称矩阵的秩为2，其他矩阵的秩都为3，根据线性代数中的定理：$r(A)+r(B)-n\le r(AB)$ 和 $r(AB)\ge min(r(A),r(B))$ 那么$r(\hat{t}R)=2$ 也就是本质矩阵E的秩为2，同理，基础矩阵F的秩为2

单应矩阵H

单应矩阵H是用来描述处于共同平面上的一些点在两张图像之间的变换关系，因此就需要一些平面的参数参与计算，单应矩阵的计算如下：

• 1.3D空间点 $P$ 所在平面在相机系下的平面参数为：$n,d$ ，其中 $n$ 为平面法向量，$d$ 是相机系远点到平面距离，空间点 $P$ 满足方程：

$$n^T\cdot P+d=0\Rightarrow -\frac{n^{T}P}{d}=1$$

• 2.将上面的对极约束中的公式进行计算：

$$\begin{array}{rl}{p}_2& \simeq K(RP+t)\\ & \simeq K(RP+t(-\frac{n^{T}P}{d}))\\ & \simeq K(R-\frac{t{n}^T}{d})P\\ & \simeq K(R-\frac{t{n}^T}{d})K^{-1}{p}_1\end{array}$$

• 3.得到了同一平面上的空间点在两张图像之间的变换关系：

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

• 4.对上面式子同时左乘 $\widehat{p_2}$ ，也就是左乘 $p_2$ 的反对称矩阵（这里的 $p_1,p_2$ 都是齐次坐标），使得上面式子左侧为0：

$$p_2\times H\ast p_1=0\Rightarrow \left[\begin{array}{ccc}0& -1& v_2\\ 1& 0& -u_2\\ -v_2& u_2& 0\end{array}\right]\left[\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=0$$

• 5.跟计算基础矩阵F一样，将上面矩阵形式展开：

$$\left[\begin{array}{ccccccccc}0& 0& 0& -u_1& -v_1& -1& u_1\ast v_2& v_1\ast v_2& v_2\\ u_1& v_1& 1& 0& 0& 0& -u_1\ast u_2& -v_1\ast u_2& -u_2\end{array}\right]\left[\begin{array}{c}{h}_1\\ h_2\\ h_3\\ h_4\\ h_5\\ h_6\\ h_7\\ h_8\\ h_9\end{array}\right]=0$$

• 6.同样使用 $A,h$ 表示：

$$Ah=0$$

一对点提供了两个约束，单应矩阵H共有9个元素，8个自由度（尺度等价性），因此需要4对点提供8个约束方程就可以求解

ORBSALM2中应用

在实际的求解上述基础矩阵F和单应矩阵H时，由于噪声、数值的误差和错误的匹配点的影响，仅仅使用上述的线性方法求解是非常不稳定的，因此在8点法的基础上有很多改进。

归一化

利用8点法求解矩阵的关键在于由图像像素坐标组成的系统矩阵$A$，所以首先需要改进的就是将这些坐标进行归一化处理，将两张图像的坐标同向变换到一个标准的坐标系中，并让噪声对图像影响在同一个数量级上。

归一化的好处：

• 提高运算结果的精度
• 利用归一化处理后的图像坐标，对任何尺度缩放和原点的选择是不变的。归一化步骤预先为图像坐标选择了一个标准的坐标系中，消除了坐标变换对结果的影响

归一化流程（其实就是做了平移和缩放）：

• 平移：对于 $N$ 个特征点坐标$(u_i,v_i)$ ，先求解其形心坐标（坐标均值），然后平移这些特征点将形心移动到$(0,0)$ 点，每个特征点平移后的坐标点$(u_i^{\prime },v_i^{\prime })$：

$$\{\begin{array}{l}meanX={\sum }_{i=0}^N{u}_i/N\\ meanY={\sum }_{i=0}^N{v}_i/N\end{array}\Rightarrow \{\begin{array}{l}{u}_i^{\prime }=u_i-meanX\\ v_i^{\prime }=v_i-meanY\end{array}$$

• 计算缩放因子：用这些平移后的特征点坐标偏离横纵坐标均值的程度作为缩放因子：

$$\{\begin{array}{l}sX=\frac{N}{{\sum }_{i=0}^N|u_i^{\prime }|}\\ sY=\frac{N}{{\sum }_{i=0}^N|v_i^{\prime }|}\end{array}$$

• 对每一个平移后的坐标值进行缩放，就得到了归一化的坐标$(x,y)$：

$$\{\begin{array}{l}x=u_i^{\prime }\ast sX\\ y=v_i^{\prime }\ast sY\end{array}$$

• 最后计算归一化变换的矩阵，这个矩阵就是上面流程的一个表示而已，可以仔细思考：
  $$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{ccc}sX& 0& -meanX\ast sX\\ 0& sY& -meanY\ast sY\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$

之所以要计算这个变换矩阵，是因为坐标转换之后，计算了基础矩阵F或者单应矩阵H后，还需要对这个计算出的矩阵进行解除归一化，面对不同的矩阵计算原理，需要使用不同的公式解除归一化：

• 基础矩阵F：假设两张图像匹配的特征点是$p_1,p_2$ ，对应归一化变换矩阵分别是$T_1,T_2$ ，使用归一化坐标计算的基础矩阵$\hat{F}$ ：

$$p_2^{T}F{p}_1=0\Rightarrow (T_2\ast p_2{)}^T\hat{F}(T_1\ast p_1)=0\Rightarrow p_2^T(T_2^T\hat{F}{T}_1)p_1=0$$

• • 因此，基础矩阵F的解除归一化的公式为：$F=T_2^T\hat{F}{T}_1$
• 单应矩阵H：假设两张图像匹配的特征点是$p_1,p_2$ ，对应归一化变换矩阵分别是$T_1,T_2$ ，使用归一化坐标计算的基础矩阵$\hat{H}$ ：

$$p_2=H{p}_1\Rightarrow (T_2\ast p_2)=\hat{H}(T_1\ast p_1)\Rightarrow p_2=(T_2^{-1}\hat{H}{T}_1)p_1$$

• • 因此，单应矩阵H的解除归一化的公式为：$H=T_2^{-1}\hat{H}{T}_1$

SVD分解

整理两个概念：

• 特征值分解：$A=Q\Sigma Q^{-1}$ ，其中 $Q$ 就是特征向量组成的矩阵，$\Sigma$ 是由特征值由大到小排列组成。这代表的意义是对于一个矩阵 $A$ ，一个基础矩阵变化到 $A$ ，那么特征向量表示这个变化的方向，特征值代表这个变换有多重要。因此我们可以用N个这种特征变化近似表示这个矩阵 $A$ 。
• 奇异值分解：$A=U\Sigma V^T$ ，其中 $U$ 是左奇异向量，$V$ 是右奇异向量，$\Sigma$ 是由奇异值由大到小排列组成。奇异值与特征值很像，只不过奇异值面对的是所有矩阵，而特征值只能是方阵。奇异值在$\Sigma$ 中减少的特别快，在很多情况下，前10%甚至1%的奇异值的和就占了全部的奇异值之和的99%以上了，也就是说，可以对$\Sigma$ 矩阵近似看成方阵。
• 从存储角度看，奇异值分解的优点：分解后的这三个矩阵的面积之和（在存储观点来说，矩阵面积越小，存储量就越小）要远远小于原始的矩阵$A$ ，我们如果想要压缩空间来表示原矩阵$A$，我们存下这里的三个矩阵：$U$、$\Sigma$、$V$就好了。

SVD分解方法如下图中表示：

得到的$M^{T}MV=VL,M{M}^{T}U=UL$，就可以变换成特征值和特征向量的形式：

由此就可以得到：

• $V$ 是$M^{T}M$ 的特征向量组成
• $U$ 是$M{M}^T$ 的特征向量组成
• $\Sigma$ 是$M^{T}M$ 和$M{M}^T$ 的特征值开方构成的对角阵（特征值相同）

最小二乘求解

八点法中，需要使用8对匹配的点计算矩阵，但是一般情况下两张图片的匹配点有很多，因此可以使用更多的点求解（超定方程）。

可以将上面原理中提到的基础矩阵F和单应矩阵H的公式 $AX=0$ ，变成求如下问题的最小二乘解：
$$f(X)=min\frac{1}{2}{\Vert AX\Vert }^2=min\frac{1}{2}(AX{)}^T(AX)=min\frac{1}{2}{X}^T{A}^{T}AX$$

其中 $A$ 是由匹配点对组成的系数矩阵，可以大于8对，$X$ 是基础矩阵F或者单应矩阵H。那么问题就是求解上面的式子，最优解就是导数为0的解：

$$\frac{df}{dX}=0\Rightarrow A^{T}AX=0$$

此时问题就变为求解$A^{T}A$ 的最小特征向量的问题，而在上面提到的SVD分解中：$V$ 是$M^{T}M$ 的特征向量组成，而且$\Sigma$ 是根据特征值从大到小排列的，因此$V$ 的最后一列就是最优解（其实其他列也是解，只不过不是最优解）

随机采样一致性 RANSAC

以上对8点法进行了很多改进，但是依然无法解决误匹配点对的影响，因此在计算了很多基础矩阵F或者单应矩阵H后，需要对每个结果进行结果评分，也就是使用了RANSAC

RANSAC(Random Sample Consensus)随机采样一致性从一组含有外点的数据集中，通过迭代的方式估计出符合该数据集的数学模型的参数。因此，它也可以用来检测出数据集中的外点。总体思想就是通过数据集中的一部分点建立符合内点的数学模型，然后计算其他点与该模型之间的距离，超过阈值就认为是外点，重复迭代k次取内点多的那一次结果

ORBSLAM2就是使用了差不多的想法，总体思想就是设定分数：对于一个求解的基础矩阵或者单应矩阵H，求解每个特征点的重投影误差的最小二乘结果与设定阈值的差值的和（其中的$\Sigma$ 是在初始化时候设定的，这里是1）

注意的点：

• 重投影在基础矩阵F和单应矩阵H有不同的投影方式，重投影误差的计算方式也就不同
• 阈值是根据卡方检验（后面会讲解）自由度为2设定的，可以认为是有一个像素的误差

$$score=\sum _{i=0}^N[th-e(i{)}^T{\Sigma }^{-1}e(i)]$$

基础矩阵F：

需要知道的概念：

	直线方程为$ax+by+c=0$ 可以表示为$l=(a,b,c)$ 对于左图对极几何的图片中，基础矩阵F满足如下等式： $$\begin{gathered} p_2^{T}F{p}_1=0 \\ {p}_1^T{F}^T{p}_2=0 \end{gathered}$$对于左图像：$p_1$ 在极线 $l_1$ 上，那么 $p_1^T{l}_1=0$ 根据上面等式可知：$l_1=F^T{p}_2$ 这里是因为对直线方程无影响，因此可以直接相等 对于右图像：$p_2$ 在极线 $l_2$ 上，那么 $p_2^T{l}_2=0$ 根据上面等式可知：$l_2=F{p}_1$ 这里是因为对直线方程无影响，因此可以直接相等

在对基础矩阵F进行结果评分时，重投影误差使用的是点与极线之间的距离，这个距离本应该是0（点在极线上），但是由于匹配点对错误或者图像噪声等原因，往往不是0，因此我们就设定这个距离为误差：

• 对于右图像，先根据 $p_1$ 求解极线 $l_2$ ，再判断 $p_2(u_2,v_2)$ 点到极线 $l_2$ 的距离，将其设定为重投影误差 $e(i)$ ：

$$\{\begin{array}{l}{l}_2(i)=F{p}_1(i)=(a_i,b_i,c_i{)}^T\\ d(i)=\frac{|a_i\ast u_2(i)+b_i\ast v_2(i)+c_i|}{\sqrt{a_i^2+b_i^2}}=e(i)\end{array}$$

• 设定分数，将每个特征点的重投影误差累加，并将超过阈值的设定为外点：

$$\begin{array}{rl}& if[e(i{)}^T{\Sigma }^{-1}e(i)]>th\\ & \\ & 将这个点设定为outliners,并不计入分数score\\ & score=\sum _{i=0}^N[th-e(i{)}^T{\Sigma }^{-1}e(i)]\end{array}$$

• 对于左图像，只不过使用的是 $l_1=F^T{p}_2$ 求解极线，之后也是一样的方式，将重投影误差累加，得到最终的分数
• 上面的过程需要将每个匹配的特征点对都计算一个分数，然后累加，就得到这个基础矩阵F的分数，取这个分数最大时的基础矩阵F，认为此时是最好的基础矩阵F

单应矩阵H：

在对单应矩阵H进行结果评分时，重投影误差使用的是点与点之间的距离，这个距离本应该是0（投影点应该是一样），但是由于匹配点对错误或者图像噪声等原因，往往不是0，因此我们就设定这个距离为误差：

• 根据单应矩阵H的推导公式，可以知道H就是两个投影点之间的变换关系，也就是说，知道了一个点 $p_1$ 可以使用单应矩阵H求解 $p_2$ ，反之亦然，因此这个重投影误差可以设定为投影点与匹配点之间的距离：

$$\begin{array}{r}e(i)={\Vert p_2(i)-H{p}_1(i)\Vert }^2\Rightarrow 从第一帧投影到第二帧（左图像到右图像）\\ e(i)={\Vert p_1(i)-H^{-1}{p}_2(i)\Vert }^2\Rightarrow 从第二帧投影到第一帧（右图像到左图像）\end{array}$$

• 之后的步骤与计算基础矩阵F相同，分别计算双向投影的分数，然后累加分数
• 依然取分数最大时的单应矩阵H，认为此时是最好的单应矩阵H

ORBSLAM2代码

由于代码中的注释都很详细，这里就并不列举。可以参考这里的流程，查看代码中的逻辑。

单应矩阵H的计算流程👇

• 1.将两帧图像的所有特征点分别进行归一化，求解归一化矩阵（用于后面解除归一化）
• 2.根据之前整理的每8对点的数组，分别计算单应矩阵H：
• • 2-1.按照上面单应矩阵H的原理，列方程得到$Ah=0$
  • 2-2.对 $A$ 进行SVD分解，根据上面的最小二乘求解，可得 $h$ 的最优解是右奇异矩阵的最后一列
• 3.对求解的 $h$ 解除归一化：$H=T_2^{-1}h{T}_1$ 求得该8对点的单应矩阵 $H$
• 4.使用上面的RANSAC评比，对每个计算的单应矩阵 $H$ 进行打分，记录最高分数的单应矩阵作为最好的
• • 4-1.RANSAC策略是对每个特征点进行重投影误差得到（上面有讲解），并根据误差计算分数
  • 4-2.对每个误差小于阈值的特征点设置为内点，否则为外点

基础矩阵F的计算流程👇

• 1.将两帧图像的所有特征点分别进行归一化，求解归一化矩阵（用于后面解除归一化）
• 2.根据之前整理的每8对点的数组，分别计算基础矩阵F：
• • 2-1.按照上面基础矩阵F的原理，列方程得到$Af=0$
  • 2-2.对 $A$ 进行SVD分解，根据上面的最小二乘求解，可得 $f^{\prime }$ 的最优解是右奇异矩阵的最后一列
  • 2-3.为了满足基础矩阵F的秩为2，强制设置其最后一个奇异值为0：对上一步求解的 $f^{\prime }$ 再次进行SVD分解，将 $\Sigma$ 的最后一位设置为0得到 ${\Sigma }^{\prime }$，最后再重新计算 $f=U{\Sigma }^{\prime }{V}^T$
• 3.对求解的 $f$ 解除归一化：$F=T_2^{T}f{T}_1$ 求得该8对点的基础矩阵 $F$
• 4.使用上面的RANSAC评比，对每个计算的基础矩阵 $F$ 进行打分，记录最高分数的基础矩阵作为最好的
• • 4-1.RANSAC策略是对每个特征点进行重投影误差得到（上面有讲解），并根据误差计算分数
  • 4-2.对每个误差小于阈值的特征点设置为内点，否则为外点