(01)ORB-SLAM2源码无死角解析-(16) 单目初始化Initializer→八点发求解Homography矩阵

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196   文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证  

一、前言

这篇博客以及下篇博客，会分别讲解根据两张不同视角图特征点求解 Homography(单应性), 以及Fundamental(基本)矩阵，然后再讲解如何利用他们来求解 相机位姿 \color{red}{相机位姿} 相机位姿。 那么我们首先来看看什么是 Homography矩阵。

 

二、Homography 作用

首先我们来说一下 Homography 矩阵的作用，最简单的作用就是投影变换，如使用相机拍摄如下两幅图像： 上图中的红点标明了左右图中对应的点，Homography 就是将一幅图中的点映射到另一幅图中它的对应点的映射。也就是如果我们知道了 左图 到 右图 的 Homography 矩阵那么我们就可以把 左图 的任意像素坐标映射到 右图。

那么反过来，已知两图中对应的坐标位置，如 左图 中 红色标记 ( x 1 , y 1 ) (x_1,y_1) (x1​,y1​) 与 右图 中的 红色标记 ( x 2 , y 2 ) (x_2,y_2) (x2​,y2​) 为一组已知的对应坐标，存在多组对应时，我们是否可以求解出相应的 Homography 矩阵。答案当然是可以的，也就是本博客需要讲解的内容。

对于 H o m o g r a p h y 矩阵的由来 \color{red}{Homography 矩阵的由来} Homography矩阵的由来，大家可以参考一下本人编写的该篇博客:史上最简SLAM零基础解读(4) - 单应性Homography →公式推导与细节理解

三、公式推导

矩阵的一个重要作用是将空间中的点变换到另一个空间中。这个作用在国内的《线性代数》教学中基本没有介绍。要能形像地理解这一作用，比较直观的方法就是图像变换，图像变换的方法很多，单应性变换是其中一种方法，单应性变换会涉及到单应性矩阵。单应性变换的目标是通过给定的几个点（通常是4对点）来得到单应性矩阵。单应性矩阵为： H = [ h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 ] \color{blue} H=\left[\begin{array}{lll} h_{11} & h_{12} & h_{13} \\ h_{21} & h_{22} & h_{23} \\ h_{31} & h_{32} & h_{33} \end{array}\right] H=⎣ ⎡​h11​h21​h31​​h12​h22​h32​​h13​h23​h33​​⎦ ⎤​

上面的矩阵H会将一幅图像上的一个点的坐标 a = ( x , y , 1 ) a=(x,y,1) a=(x,y,1) 映射成另一幅图像上的点的坐标 b = ( x 1 , y 1 , 1 ) b=(x_1,y_1,1) b=(x1​,y1​,1)，但 H H H是未知的。通常需要根据在同一平面上已知的一些点对(比如 a , b a,b a,b)来求 H H H。 假设已知点对 ( a , b ) (a,b) (a,b)，则有下面的公式： b = H a T (1) \tag {1} \color{blue} b=Ha^T b=HaT(1)带入 H H H得： b = H a T = [ h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 ] [ x y 1 ] = ( x 1 , y 1 , 1 ) (2) \tag {2} \color{blue}b=Ha^T=\left[\begin{array}{lll} h_{11} & h_{12} & h_{13} \\ h_{21} & h_{22} & h_{23} \\ h_{31} & h_{32} & h_{33} \end{array}\right]\left[\begin{array}{l} x\\ y \\ 1 \end{array}\right] =(x_1,y_1,1) b=HaT=⎣ ⎡​h11​h21​h31​​h12​h22​h32​​h13​h23​h33​​⎦ ⎤​⎣ ⎡​xy1​⎦ ⎤​=(x1​,y1​,1)(2) 进而可以得到： { x 1 = h 11 x + h 12 y + h 13 y 1 = h 21 x + h 22 y + h 23 1 = h 31 x + h 32 y + h 33 (3) \tag {3} \color{blue}\left\{\begin{aligned} x_{1} &=h_{11} x+h_{12} y+h_{13} \\ y_{1} &=h_{21} x+h_{22} y+h_{23} \\ 1 &=h_{31} x+h_{32} y+h_{33} \end{aligned}\right. ⎩ ⎨ ⎧​x1​y1​1​=h11​x+h12​y+h13​=h21​x+h22​y+h23​=h31​x+h32​y+h33​​(3)由上面这个公式中的 1 = h 31 x + h 32 y + h 33 1=h31x+h32y+h33 1=h31x+h32y+h33 可得到下面两个等式 { x 1 = x 1 1 = h 11 x + h 12 y + h 13 h 31 x + h 32 y + h 33 y 1 = y 1 1 = h 21 x + h 22 y + h 23 h 31 x + h 32 y + h 33 (4) \tag {4} \color{blue} \left\{\begin{aligned} x_{1}=\frac{x_{1}}{1}=\frac{h_{11} x+h_{12} y+h_{13}}{h_{31} x+h_{32} y+h_{33}} \\ y_{1}=\frac{y_{1}}{1}=\frac{h_{21} x+h_{22} y+h_{23}}{h_{31} x+h_{32} y+h_{33}} \end{aligned}\right. ⎩ ⎨ ⎧​x1​=1x1​​=h31​x+h32​y+h33​h11​x+h12​y+h13​​y1​=1y1​​=h31​x+h32​y+h33​h21​x+h22​y+h23​​​(4) ⇒ \Rightarrow ⇒ { h 11 x + h 12 y + h 13 = h 31 x x 1 + h 32 y x 1 + h 33 x 1 h 21 x + h 22 y + h 23 = h 31 x y 1 + h 32 y y 1 + h 33 y 1 (5) \tag {5} \color{blue}\left\{\begin{aligned} h_{11} x+h_{12} y+h_{13}=h 31 x x_{1}+h_{32} y x_{1}+h_{33} x_{1} \\ \\ h_{21} x+h_{22} y+h_{23}=h 31 x y_{1}+h_{32} y y_{1}+h_{33} y_{1} \end{aligned}\right. ⎩ ⎨ ⎧​h11​x+h12​y+h13​=h31xx1​+h32​yx1​+h33​x1​h21​x+h22​y+h23​=h31xy1​+h32​yy1​+h33​y1​​(5) ⇒ \Rightarrow ⇒ { 0 = h 31 x x 1 + h 32 y x 1 + h 33 x 1 − ( h 11 x + h 12 y + h 13 ) 0 = h 31 x y 1 + h 32 y y 1 + h 33 y 1 − ( h 21 x + h 22 y + h 23 ) (6) \tag {6} \color{blue} \left\{\begin{array}{r} 0=h_{31} x x_{1}+h_{32} y x_{1}+h_{33} x_{1}-\left(h_{11} x+h_{12} y+h_{13}\right) \\ \\ 0=h_{31} x y_{1}+h_{32} y y_{1}+h_{33} y_{1}-\left(h_{21} x+h_{22} y+h_{23}\right) \end{array}\right. ⎩ ⎨ ⎧​0=h31​xx1​+h32​yx1​+h33​x1​−(h11​x+h12​y+h13​)0=h31​xy1​+h32​yy1​+h33​y1​−(h21​x+h22​y+h23​)​(6)对于方程(1)可写成一个矩阵与一个向量相乘，即： [ − x − y − 1 0 0 0 x x 1 y x 1 x 1 0 0 0 − x − y − 1 x y 1 y y 1 y 1 ] h = 0 (7) \tag {7} \color{blue} \left[ \begin{array}{ccccccccc} -x & -y & -1 & 0 & 0 & 0 & x x_{1} & y x_{1} & x_{1} \\ \\ 0 & 0 & 0 & -x & -y & -1 & x y_{1} & y y_{1} & y_{1} \end{array}\right ] h=0 ⎣ ⎡​−x0​−y0​−10​0−x​0−y​0−1​xx1​xy1​​yx1​yy1​​x1​y1​​⎦ ⎤​h=0(7)其中， h = [ h 11 , h 12 , h 13 , h 21 , h 22 , h 23 , h 31 , h 32 , h 33 ] T h=[h11,h12,h13,h21,h22,h23,h31,h32,h33]^T h=[h11,h12,h13,h21,h22,h23,h31,h32,h33]T，是一个9维的列向量。若令: A = [ − x − y − 1 0 0 0 x x 1 y x 1 x 1 0 0 0 − x − y − 1 x y 1 y y 1 y 1 ] (8) \tag {8} \color{blue} A=\left[\begin{array}{ccccccccc} -x & -y & -1 & 0 & 0 & 0 & x x_{1} & y x_{1} & x_{1} \\ \\ 0 & 0 & 0 & -x & -y & -1 & x y_{1} & y y_{1} & y_{1} \end{array}\right] A=⎣ ⎡​−x0​−y0​−10​0−x​0−y​0−1​xx1​xy1​​yx1​yy1​​x1​y1​​⎦ ⎤​(8)则(7)可以记为 A h = 0 (9) \tag {9} \color{blue} {Ah=0} Ah=0(9) 这里的 A ∈ R 2 × 9 A∈R^{2×9} A∈R2×9。这只是1对点所得到的矩阵A。究竟要多少点对才能求出 H H H?,由于我们是采用齐次坐标（即(x,y,1)）来表示平面上的点，所以存在一个非零的标量s，使得 b 1 = s H a T b_1=sHa^T b1​=sHaT 与 b = s H a T b=sHa^T b=sHaT都表示同一个点b。若令 s = 1 h 33 s=\frac{1}{h_{33}} s=h33​1​，则 1 h 33 H \frac{1}{h_{33}}H h33​1​H 为: 1 h 33 H = [ h 11 h 33 h 12 h 33 h 13 h 33 h 21 h 33 h 22 h 33 h 23 h 33 h 31 h 33 h 32 h 33 1 ] = [ h 11 h 33 , h 12 h 33 , h 13 h 33 , h 21 h 33 , h 22 h 33 , h 23 h 33 , h 31 h 33 , h 32 h 33 , 1 ] T (10) \tag{10} \color{blue} \frac{1}{h_{33}} H=\left[\begin{array}{ccc} \frac{h_{11}}{h_{33}} & \frac{h_{12}}{h_{33}} & \frac{h_{13}}{h_{33}} \\ \frac{h_{21}}{h_{33}} & \frac{h_{22}}{h_{33}} & \frac{h_{23}}{h_{33}} \\ \frac{h_{31}}{h_{33}} & \frac{h_{32}}{h_{33}} & 1 \end{array}\right]=[ \frac{h11}{h_{33}},\frac{h12}{h_{33}},\frac{h13}{h_{33}},\frac{h21}{h_{33}},\frac{h22}{h_{33}},\frac{h23}{h_{33}},\frac{h31}{h_{33}},\frac{h32}{h_{33}},1]^T h33​1​H=⎣ ⎡​h33​h11​​h33​h21​​h33​h31​​​h33​h12​​h33​h22​​h33​h32​​​h33​h13​​h33​h23​​1​⎦ ⎤​=[h33​h11​,h33​h12​,h33​h13​,h33​h21​,h33​h22​,h33​h23​,h33​h31​,h33​h32​,1]T(10) 从公式(9)可以看出，其实H只有8个变量（8个自由度）。因此，只需要4个点对，然后通过解线性方程组就可以求得 H H H。也可以多于4个点对。m个点对的公式如下(下标表示矩阵大小)： A ( 2 m , 9 ) H ( 9 , 1 ) = 0 (11) \tag {11} \color{blue} A_{(2m,9)}H_{(9,1)}=0 A(2m,9)​H(9,1)​=0(11)如果是八个点对展开类似如下( x , y x,y x,y与 x ′ , y ′ x',y' x′,y′为对应点对)： [ 0 0 0 − x 1 − y 1 − 1 x 1 ∗ y 1 ′ y 1 ∗ y 1 ′ y 1 ′ x 1 y 1 1 0 0 0 − x 1 ∗ x 1 ′ − y 1 ∗ x 1 ′ − x 1 ′ 0 0 0 − x 2 − y 2 − 1 x 2 ∗ y 2 ′ y 2 ∗ y 2 ′ y 2 ′ x 2 y 2 1 0 0 0 − x 2 ∗ x 2 ′ − y 2 ∗ x 2 ′ − x 2 ′ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ ⋯ 0 0 0 − x 7 − y 7 − 1 x 7 ∗ y 7 ′ y 7 ∗ y 7 ′ y 7 ′ x 7 y 7 1 0 0 0 − x 7 ∗ x 7 ′ − y 7 ∗ x 7 ′ − x 7 ′ 0 0 0 − x 8 − y 8 − 1 x 8 ∗ y 8 ′ y 8 ∗ y 8 ′ y 8 ′ x 8 y 8 1 0 0 0 − x 8 ∗ x 8 ′ − y 8 ∗ x 8 ′ − x 8 ′ ] [ h 1 h 1 h 3 h 4 h 5 h 6 h 7 h 8 h 9 ] = 0 (11) \tag {11} \color{blue} \left[\begin{array}{ccccccccc} 0 & 0 & 0 & -x_{1} & -y_{1} & -1 & x_{1} * y'_{1} & y_{1} * y'_{1} & y'_{1} \\ x_{1} & y_{1} & 1 & 0 & 0 & 0 & -x_{1} * x'_{1} & -y_{1} * x'_{1} & -x'_{1}\\ 0 & 0 & 0 & -x_{2} & -y_{2} & -1 & x_{2} * y'_{2} & y_{2} * y'_{2} & y'_{2} \\ x_{2} & y_{2} & 1 & 0 & 0 & 0 & -x_{2} * x'_{2} & -y_{2} * x'_{2} & -x'_{2}\\ \cdots & \cdots & \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \\ \cdots & \cdots & \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \cdots& \\ 0 & 0 & 0 & -x_{7} & -y_{7} & -1 & x_{7} * y'_{7} & y_{7} * y'_{7} & y'_{7} \\ x_{7} & y_{7} & 1 & 0 & 0 & 0 & -x_{7} * x'_{7} & -y_{7} * x'_{7} & -x'_{7}\\ 0 & 0 & 0 & -x_{8} & -y_{8} & -1 & x_{8} * y'_{8} & y_{8} * y'_{8} & y'_{8} \\ x_{8} & y_{8} & 1 & 0 & 0 & 0 & -x_{8} * x'_{8} & -y_{8} * x'_{8} & -x'_{8}\\ \end{array}\right]\left[\begin{array}{l} h_{1} \\ h_{1} \\ h_{3} \\ h_{4} \\ h_{5} \\ h_{6} \\ h_{7} \\ h_{8} \\ h_{9} \end{array}\right]=0 ⎣ ⎡​0x1​0x2​⋯⋯0x7​0x8​​0y1​0y2​⋯⋯0y7​0y8​​0101⋯⋯0101​−x1​0−x2​0⋯⋯−x7​0−x8​0​−y1​0−y2​0⋯⋯−y7​0−y8​0​−10−10⋯⋯−10−10​x1​∗y1′​−x1​∗x1′​x2​∗y2′​−x2​∗x2′​⋯⋯x7​∗y7′​−x7​∗x7′​x8​∗y8′​−x8​∗x8′​​y1​∗y1′​−y1​∗x1′​y2​∗y2′​−y2​∗x2′​⋯⋯y7​∗y7′​−y7​∗x7′​y8​∗y8′​−y8​∗x8′​​y1′​−x1′​y2′​−x2′​⋯⋯y7′​−x7′​y8′​−x8′​​​⎦ ⎤​⎣ ⎡​h1​h1​h3​h4​h5​h6​h7​h8​h9​​⎦ ⎤​=0(11)

 

四、代码流程：

源码位于 src/Initializer.cc文件中

 1 将当前帧和参考帧中的特征点坐标进行归一化
 2 选择8个归一化之后的点对进行迭代
 3 八点法计算单应矩阵矩阵
 4 利用重投影误差为当次RANSAC的结果评分
 5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记

步骤 1 : \color{blue}{步骤1:} 步骤1: 主要是把参考帧特征点变换到当前帧特征点的平移以及尺度变换去除。简单的说，做了归一化，再去求解 Homography 矩阵，该矩阵中只包含了旋转信息。这里的归一化，主要包含了减均值(消除平移变换)，除偏差(消除尺度缩放)。

步骤 2 : \color{blue}{步骤2:} 步骤2: 循环迭代mMaxIterations=200次,每次选取把对匹配的特征点，

步骤 3 : \color{blue}{步骤3:} 步骤3: 八点法计算单应矩阵矩阵，这里主要使用真接对 H H H 进行SVD分解，即 U ∗ Σ ∗ V T U∗Σ∗V^T U∗Σ∗VT。的方式求解。该求解的方式，这篇博客就不进行讲解了，大家可以参考博客:

步骤 4 : \color{blue}{步骤4:} 步骤4: 利用重投影误差为当次RANSAC的结果评分。使用八点法计算出来的 Homography 矩阵， 他适应于这八对特征点的转换，但是并不一定完全适用于特征点。所以我们把当前帧的所有特征点, 使用该矩阵映射到参考帧，然后从参考帧再映回来。重映射回来特征点坐标误差。

步骤 5 : \color{blue}{步骤5:} 步骤5: 根据重映射回来特征点坐标误差进行评分，累计所有特征的评分，作为该 Homography 矩阵的评分。 这里需要注意的一个点，如果其中某些特征点分值太低，我们则认为其特征点为外点，不再线内。也就是这样的特征带点我们认为是一个离群点。同时对特征点进行线内，线外的标记

最后获得 mMaxIterations=200 个 Homography 矩阵，但是我们只选择了其中分值最高的矩阵。特征点线内，线外的标记，也是与该矩阵进行对应。

 

五、源码注释

主调函数为 FindHomography 函数，其内部核心函数为:

Normalize()  归一化操作
ComputeH21() 八点法计算参考帧到当前帧的H矩阵
CheckHomography() 重投影误差评分

C o m p u t e H 21 ( ) 与 C h e c k H o m o g r a p h y ( ) 在后面的博客有详细的讲解 \color{red}{ComputeH21()与} CheckHomography() 在后面的博客有详细的讲解 ComputeH21()与CheckHomography()在后面的博客有详细的讲解，所以这里简单过一下即可。

void Initializer::FindHomography()代码注释如下

/**
 * @brief 归一化特征点到同一尺度，作为后续normalize DLT的输入
 *  [x' y' 1]' = T * [x y 1]' 
 *  归一化后x', y'的均值为0，sum(abs(x_i'-0))=1，sum(abs((y_i'-0))=1
 *
 *  为什么要归一化？
 *  在相似变换之后(点在不同的坐标系下),他们的单应性矩阵是不相同的
 *  如果图像存在噪声,使得点的坐标发生了变化,那么它的单应性矩阵也会发生变化
 *  我们采取的方法是将点的坐标放到同一坐标系下,并将缩放尺度也进行统一 
 *  对同一幅图像的坐标进行相同的变换,不同图像进行不同变换
 *  缩放尺度是为了让噪声对于图像的影响在一个数量级上
 * 
 *  Step 1 计算特征点X,Y坐标的均值 
 *  Step 2 计算特征点X,Y坐标离均值的平均偏离程度
 *  Step 3 将x坐标和y坐标分别进行尺度归一化，使得x坐标和y坐标的一阶绝对矩分别为1 
 *  Step 4 计算归一化矩阵：其实就是前面做的操作用矩阵变换来表示而已
 * 
 * @param[in] vKeys                               待归一化的特征点
 * @param[in & out] vNormalizedPoints             特征点归一化后的坐标
 * @param[in & out] T                             归一化特征点的变换矩阵
 */
void Initializer::Normalize(const vector &vKeys, vector &vNormalizedPoints, cv::Mat &T)                           //将特征点归一化的矩阵
{
    // 归一化的是这些点在x方向和在y方向上的一阶绝对矩（随机变量的期望）。

    // Step 1 计算特征点X,Y坐标的均值 meanX, meanY
    float meanX = 0;
    float meanY = 0;

	//获取特征点的数量
    const int N = vKeys.size();

	//设置用来存储归一后特征点的向量大小，和归一化前保持一致
    vNormalizedPoints.resize(N);

	//开始遍历所有的特征点
    for(int i=0; i th
    //              p1(i), p2(i) are inner points
    //              vbMatchesInliers(i) = true
    //           else 
    //              p1(i), p2(i) are outliers
    //              vbMatchesInliers(i) = false
    //           end
    //        end
    //   output: score, inliers

	// 特点匹配个数
    const int N = mvMatches12.size();

	// Step 1 获取从参考帧到当前帧的单应矩阵的各个元素
    const float h11 = H21.at(0,0);
    const float h12 = H21.at(0,1);
    const float h13 = H21.at(0,2);
    const float h21 = H21.at(1,0);
    const float h22 = H21.at(1,1);
    const float h23 = H21.at(1,2);
    const float h31 = H21.at(2,0);
    const float h32 = H21.at(2,1);
    const float h33 = H21.at(2,2);

	// 获取从当前帧到参考帧的单应矩阵的各个元素
    const float h11inv = H12.at(0,0);
    const float h12inv = H12.at(0,1);
    const float h13inv = H12.at(0,2);
    const float h21inv = H12.at(1,0);
    const float h22inv = H12.at(1,1);
    const float h23inv = H12.at(1,2);
    const float h31inv = H12.at(2,0);
    const float h32inv = H12.at(2,1);
    const float h33inv = H12.at(2,2);

	// 给特征点对的Inliers标记预分配空间
    vbMatchesInliers.resize(N);

	// 初始化score值
    float score = 0;

    // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
	// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值
    const float th = 5.991;

    //信息矩阵，方差平方的倒数
    const float invSigmaSquare = 1.0/(sigma * sigma);

    // Step 2 通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权重投影误差
    // H21 表示从img1 到 img2的变换矩阵
    // H12 表示从img2 到 img1的变换矩阵 
    for(int i = 0; i th)
            bIn = false;    
        else
            // 误差越大，得分越低
            score += th - chiSquare1;

        // 计算从img1 到 img2 的投影变换误差
        // x1in2 = H21*x1
        // 将图像2中的特征点通过单应变换投影到图像1中
        // |u2|   |h11 h12 h13||u1|   |u1in2|
        // |v2| = |h21 h22 h23||v1| = |v1in2| * w1in2inv
        // |1 |   |h31 h32 h33||1 |   |  1  |
		// 计算投影归一化坐标
        const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);
        const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;
        const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;

        // 计算重投影误差 
        const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);
        const float chiSquare2 = squareDist2*invSigmaSquare;
 
        // 用阈值标记离群点，内点的话累加得分
        if(chiSquare2>th)
            bIn = false;
        else
            score += th - chiSquare2;   

        // Step 2.4 如果从img2 到 img1 和 从img1 到img2的重投影误差均满足要求，则说明是Inlier point
        if(bIn)
            vbMatchesInliers[i]=true;
        else
            vbMatchesInliers[i]=false;
    }
    return score;
}



/**
 * @brief 计算单应矩阵，假设场景为平面情况下通过前两帧求取Homography矩阵，并得到该模型的评分
 * 原理参考Multiple view geometry in computer vision  P109 算法4.4
 * Step 1 将当前帧和参考帧中的特征点坐标进行归一化
 * Step 2 选择8个归一化之后的点对进行迭代
 * Step 3 八点法计算单应矩阵矩阵
 * Step 4 利用重投影误差为当次RANSAC的结果评分
 * Step 5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记
 * 
 * @param[in & out] vbMatchesInliers          标记是否是外点
 * @param[in & out] score                     计算单应矩阵的得分
 * @param[in & out] H21                       单应矩阵结果
 */
void Initializer::FindHomography(vector &vbMatchesInliers, float &score, cv::Mat &H21)
{
    // Number of putative matches
	//匹配的特征点对总数
    const int N = mvMatches12.size();

    // Normalize coordinates
    // Step 1 将当前帧和参考帧中的特征点坐标进行归一化，主要是平移和尺度变换
    // 具体来说,就是将mvKeys1和mvKey2归一化到均值为0，一阶绝对矩为1，归一化矩阵分别为T1、T2
    // 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值
    // 归一化矩阵就是把上述归一化的操作用矩阵来表示。这样特征点坐标乘归一化矩阵可以得到归一化后的坐标
   

	//归一化后的参考帧1和当前帧2中的特征点坐标
    vector vPn1, vPn2;
	// 记录各自的归一化矩阵
    cv::Mat T1, T2;
    Normalize(mvKeys1,vPn1, T1);
    Normalize(mvKeys2,vPn2, T2);

	//这里求的逆在后面的代码中要用到，辅助进行原始尺度的恢复
    cv::Mat T2inv = T2.inv();

    // Best Results variables
	// 记录最佳评分
    score = 0.0;
	// 取得历史最佳评分时,特征点对的inliers标记
    vbMatchesInliers = vector(N,false);

    // Iteration variables
	//某次迭代中，参考帧的特征点坐标
    vector vPn1i(8);
	//某次迭代中，当前帧的特征点坐标
    vector vPn2i(8);
	//以及计算出来的单应矩阵、及其逆矩阵
    cv::Mat H21i, H12i;

    // 每次RANSAC记录Inliers与得分
    vector vbCurrentInliers(N,false);
    float currentScore;

    // Perform all RANSAC iterations and save the solution with highest score
	//下面进行每次的RANSAC迭代
    for(int it=0; it