ORB-SLAM2 ---- ORBmatcher::SearchForTriangulation函数

最新推荐文章于 2023-06-12 09:39:12 发布

APS2023

最新推荐文章于 2023-06-12 09:39:12 发布

阅读量1.2k

点赞数

分类专栏：视觉SLAM - ORBSLAM2 原理解析文章标签： c++ 图像处理计算机视觉 slam orb-slam

本文链接：https://blog.csdn.net/qq_41694024/article/details/126332164

版权

视觉SLAM - ORBSLAM2 原理解析专栏收录该内容

66 篇文章 46 订阅

订阅专栏

2.3 计算KF1的相机中心在KF2图像平面的二维像素坐标

2.4 利用BoW加速匹配：只对属于同一节点(特定层)的ORB特征进行匹配

1.推荐先读博客

为了更好的理解本节内容，推荐看懂以下内容再看本节：

ORB-SLAM2 ---- ORBmatcher::SearchByBoW函数_Courage2022的博客-CSDN博客https://blog.csdn.net/qq_41694024/article/details/126322962 ORB-SLAM2 ---- Frame::ComputeBoW函数_Courage2022的博客-CSDN博客https://blog.csdn.net/qq_41694024/article/details/126328833

2.源码解析

2.1 源代码

/*
 * @brief 利用基础矩阵F12极线约束，用BoW加速匹配两个关键帧的未匹配的特征点，产生新的匹配点对
 * 具体来说，pKF1图像的每个特征点与pKF2图像同一node节点的所有特征点依次匹配，判断是否满足对极几何约束，满足约束就是匹配的特征点
 * @param pKF1          关键帧1
 * @param pKF2          关键帧2
 * @param F12           从2到1的基础矩阵
 * @param vMatchedPairs 存储匹配特征点对，特征点用其在关键帧中的索引表示
 * @param bOnlyStereo   在双目和rgbd情况下，是否要求特征点在右图存在匹配
 * @return              成功匹配的数量
 */
int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
                                       vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo)
{
    const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
    const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;

    // Compute epipole in second image
    // Step 1 计算KF1的相机中心在KF2图像平面的二维像素坐标
    // KF1相机光心在世界坐标系坐标Cw
    cv::Mat Cw = pKF1->GetCameraCenter(); 
    // KF2相机位姿R2w,t2w,是世界坐标系到相机坐标系
    cv::Mat R2w = pKF2->GetRotation();    
    cv::Mat t2w = pKF2->GetTranslation(); 
    // KF1的相机光心转化到KF2坐标系中的坐标
    cv::Mat C2 = R2w*Cw+t2w; 
    const float invz = 1.0f/C2.at<float>(2);
    // 得到KF1的相机光心在KF2中的坐标，也叫极点，这里是像素坐标
    const float ex =pKF2->fx*C2.at<float>(0)*invz+pKF2->cx;
    const float ey =pKF2->fy*C2.at<float>(1)*invz+pKF2->cy;

    // Find matches between not tracked keypoints
    // Matching speed-up by ORB Vocabulary
    // Compare only ORB that share the same node

    int nmatches=0;
    // 记录匹配是否成功，避免重复匹配
    vector<bool> vbMatched2(pKF2->N,false);        
    vector<int> vMatches12(pKF1->N,-1);
    // 用于统计匹配点对旋转差的直方图
    vector<int> rotHist[HISTO_LENGTH];
    for(int i=0;i<HISTO_LENGTH;i++)
        rotHist[i].reserve(500);

     //! 原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的，更改为下面代码   
    const float factor = HISTO_LENGTH/360.0f;

    // We perform the matching over ORB that belong to the same vocabulary node (at a certain level)
    // Step 2 利用BoW加速匹配：只对属于同一节点(特定层)的ORB特征进行匹配
    // FeatureVector其实就是一个map类，那就可以直接获取它的迭代器进行遍历
    // FeatureVector的数据结构类似于：{(node1,feature_vector1) (node2,feature_vector2)...}
    // f1it->first对应node编号，f1it->second对应属于该node的所有特特征点编号
    DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
    DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
    DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
    DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();

    // Step 2.1：遍历pKF1和pKF2中的node节点
    while(f1it!=f1end && f2it!=f2end)
    {
        // 如果f1it和f2it属于同一个node节点才会进行匹配，这就是BoW加速匹配原理
        if(f1it->first == f2it->first)
        {
            // Step 2.2：遍历属于同一node节点(id：f1it->first)下的所有特征点
            for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++)
            {
                // 获取帧1即pKF1中属于该node节点的描述子索引 0 --- 数量-1
                const size_t idx1 = f1it->second[i1];
                
                // Step 2.3：通过特征点索引idx1在pKF1中取出对应的MapPoint
                MapPoint* pMP1 = pKF1->GetMapPoint(idx1);
                
                // If there is already a MapPoint skip
                // 由于寻找的是未匹配的特征点，所以pMP1应该为NULL
                if(pMP1)
                    continue;

                // 如果mvuRight中的值大于0，表示是双目，且该特征点有深度值
                const bool bStereo1 = pKF1->mvuRight[idx1]>=0;

                if(bOnlyStereo)
                    if(!bStereo1)
                        continue;
                
                // Step 2.4：通过特征点索引idx1在pKF1中取出对应的特征点
                const cv::KeyPoint &kp1 = pKF1->mvKeysUn[idx1];
                
                // 通过特征点索引idx1在pKF1中取出对应的特征点的描述子
                const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);
                
                int bestDist = TH_LOW;
                int bestIdx2 = -1;
                
                // Step 2.5：遍历该node节点下(f2it->first)对应KF2中的所有特征点
                for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++)
                {
                    // 获取pKF2中属于该node节点的所有特征点索引
                    size_t idx2 = f2it->second[i2];
                    
                    // 通过特征点索引idx2在pKF2中取出对应的MapPoint
                    MapPoint* pMP2 = pKF2->GetMapPoint(idx2);
                    
                    // If we have already matched or there is a MapPoint skip
                    // 如果pKF2当前特征点索引idx2已经被匹配过或者对应的3d点非空，那么跳过这个索引idx2
                    if(vbMatched2[idx2] || pMP2)
                        continue;

                    const bool bStereo2 = pKF2->mvuRight[idx2]>=0;

                    if(bOnlyStereo)
                        if(!bStereo2)
                            continue;
                    
                    // 通过特征点索引idx2在pKF2中取出对应的特征点的描述子
                    const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);
                    
                    // Step 2.6 计算idx1与idx2在两个关键帧中对应特征点的描述子距离
                    const int dist = DescriptorDistance(d1,d2);
                    
                    if(dist>TH_LOW || dist>bestDist)
                        continue;

                    // 通过特征点索引idx2在pKF2中取出对应的特征点
                    const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];

                    //? 为什么双目就不需要判断像素点到极点的距离的判断？
                    // 因为双目模式下可以左右互匹配恢复三维点
                    if(!bStereo1 && !bStereo2)
                    {
                        const float distex = ex-kp2.pt.x;
                        const float distey = ey-kp2.pt.y;
                        // Step 2.7 极点e2到kp2的像素距离如果小于阈值th,认为kp2对应的MapPoint距离pKF1相机太近，跳过该匹配点对
                        // 作者根据kp2金字塔尺度因子(scale^n，scale=1.2，n为层数)定义阈值th
                        // 金字塔层数从0到7，对应距离 sqrt(100*pKF2->mvScaleFactors[kp2.octave]) 是10-20个像素
                        //? 对这个阈值的有效性持怀疑态度
                        if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave])
                            continue;
                    }

                    // Step 2.8 计算特征点kp2到kp1对应极线的距离是否小于阈值
                    if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2))
                    {
                        // bestIdx2，bestDist 是 kp1 对应 KF2中的最佳匹配点 index及匹配距离
                        bestIdx2 = idx2;
                        bestDist = dist;
                    }
                }

                
                if(bestIdx2>=0)
                {
                    const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];
                    // 记录匹配结果
                    vMatches12[idx1]=bestIdx2;      
                    
                    vbMatched2[bestIdx2]=true;  // !记录已经匹配，避免重复匹配。原作者漏掉！
                    nmatches++;

                    // 记录旋转差直方图信息
                    if(mbCheckOrientation)
                    {
                        // angle：角度，表示匹配点对的方向差。
                        float rot = kp1.angle-kp2.angle;
                        if(rot<0.0)
                            rot+=360.0f;
                        int bin = round(rot*factor);
                        if(bin==HISTO_LENGTH)
                            bin=0;
                        assert(bin>=0 && bin<HISTO_LENGTH);   
                        rotHist[bin].push_back(idx1);
                    }
                }
            }

            f1it++;
            f2it++;
        }
        else if(f1it->first < f2it->first)
        {
            f1it = vFeatVec1.lower_bound(f2it->first);
        }
        else
        {
            f2it = vFeatVec2.lower_bound(f1it->first);
        }
    }

    // Step 3 用旋转差直方图来筛掉错误匹配对
    if(mbCheckOrientation)
    {
        int ind1=-1;
        int ind2=-1;
        int ind3=-1;

        ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);

        for(int i=0; i<HISTO_LENGTH; i++)
        {
            if(i==ind1 || i==ind2 || i==ind3)
                continue;
            for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
            {              
                vbMatched2[vMatches12[rotHist[i][j]]] = false;  // !清除匹配关系。原作者漏掉！
                vMatches12[rotHist[i][j]]=-1;
                nmatches--;
            }
        }

    }

    // Step 4 存储匹配关系，下标是关键帧1的特征点id，存储的是关键帧2的特征点id
    vMatchedPairs.clear();
    vMatchedPairs.reserve(nmatches);

    for(size_t i=0, iend=vMatches12.size(); i<iend; i++)
    {
        if(vMatches12[i]<0)
            continue;
        vMatchedPairs.push_back(make_pair(i,vMatches12[i]));
    }

    return nmatches;
}

2.2 关于基础矩阵

        在第一帧的坐标系下，设 $P$ 的空间位置为 $P=[X,Y,Z]^{T}$

        根据针孔相机模型，两个像素点 $p_{1},p_{2}$ 的像素位置为： $s_{1}p_{1}=KP \ ,s_{2}p_{2}=K(R_{21}P+t_{21})$ ，这里 $K$ 为相机内参矩阵， $R,t$ 为两个坐标系的相机运动。

        (注意：为什么是 $R_{21}$ ，因为 $P$ 是世界坐标即在 $O_{1}$ 参考系下看见的坐标，但不知道第二个坐标系看到的 $P$ 是什么样子，于是乘以 $R_{21}$ ，转化为第二个坐标系即 $O_{2}$ 看到的坐标值，那 $s_{2}$ 是什么， $s_{2}$ 是 $O_{2}p$ ， $p_{2}$ 是在 $O_{2}$ 参考系下的像素矩阵)

        具体来说，这里计算的是 $R_{21}$ 和 $t_{21}$ ，因为它们把第一个坐标系下的坐标转换到第二个坐标系下。也可以把它们写成李代数形式。
        有时，我们会使用齐次坐标表示像素点。在使用齐次坐标时，一个向量将等于它自身乘上任意的非零常数。这通常用于表达一个投影关系。例如， $s_{1}p_{1}$ 和 $p_{1}$ 成投影关系，它们在齐次坐标的意义下是相等的。我们称这种相等关系为尺度意义下相等(equal up to a scale)，记作： $sp\simeq p$

        那么，上述两个投影关系可写为：   $p_{1}\simeq KP \ ,p_{2}\simeq K(RP+t)$

现在取 $x_{1}=K^{-1}p_{1},x_{2}=K^{-1}p_{2}$

        这里的 $x_{1},x_{2}$ 是两个像素点的归一化平面上的坐标。代入上式，得 $x_{2}\simeq Rx_{1}+t$

        两边同时左乘 $\hat{t}$ 。回忆^的定义，这相当于两侧同时与 $t$ 做外积： $t^{\wedge }x_{2}\simeq t^{\wedge }Rx_{1}$

        两侧同时左乘 $x_{2}^{T}$ ： $x_{2}^{T}t^{\wedge }x_{2}\simeq x_{2}^{T}t^{\wedge }Rx_{1}$

        等式左侧， $t^{\wedge }x_{2}$ 是一个与 $t^{\wedge }$ 和 $x_{2}$ 都垂直的向量。它再和 $x_{2}$ 做内积时，将得到0。

        由于等式左侧严格为零，乘以任意非零常数之后也为零，于是我们可以把 $\simeq$ 写成通常的等号。因此，我们就得到： $x_{2}^{T}t^{\wedge }Rx_{1} = 0$

重新带入 $p_{1},p_{2}$ 有 $p_{2}^{T}K^{-T}t^{\wedge }RK^{-1}p_{1} = 0$

        这两个式子都称为对极约束。它的几何意义是 $O_{1},P,O_{2}$ 三者共面。

        对极约束中同时包含了平移和旋转。我们把中间部分记作两个矩阵：基础矩阵(Fundamental Matrix)F和本质矩阵(Essential Matrix)E，于是可以进一步简化对极约束：



$E=t^{\wedge }R,\ F=K^{-T}EK^{-1},\ x_{2}Ex_{1}=p_{2}^{T}Fp_{1}=0$

3.总结

对极约束简洁地给出了两个匹配点的空间位置关系。于是，相机位姿估计问题变为以下两步：

1.根据配对点的像素位置求出 $E$ 或者 $F$ 。
2.根据 $E$ 或者 $F$ 求出 $R,t$ 。
由于 $E$ 和 $F$ 只相差了相机内参，而内参在SLAM中通常是已知的，所以实践中往往使用形式更简单的 $E$ 。我们以 $E$ 为例，介绍上面两个问题如何求解。

4.本质矩阵的特性

        根据定义，本质矩阵 $E=t^{\wedge }R$ 。它是一个3×3的矩阵，内有9个未知数。那么，是不是任意一个3×3的矩阵都可以被当成本质矩阵呢？
        本质矩阵是由对极约束定义的。由于对极约束是等式为零的约束，所以对E乘以任意非零常数后，对极约束依然满足。我们把这件事情称为E在不同尺度下是等价的。
        根据 $E=t^{\wedge }R$ ，可以证明，本质矩阵 $E$ 的奇异值必定是 $[\sigma ,\sigma ,0]^{T}$ 的形式。这称为本质矩阵的内在性质。
        另外，由于平移和旋转各有3个自由度，故 $t^{\wedge }R$ 共有6个自由度。但由于尺度等价性，故 $E$ 实际上有5个自由度。

5.八点法求本质矩阵

   $E$ 具有5个自由度的事实，表明我们最少可以用5对点来求解 $E$ 。但是， $E$ 的内在性质是一种非线性性质，在估计时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用8对点来估计 $E$ ——这就是经典的八点法（Eight-point-algorithm ) 。八点法只利用了 $E$ 的线性性质，因此可以在线性代数框架下求解。下面我们来看八点法是如何工作的：
        考虑一对匹配点，它们的归一化坐标为 $x_{1}=[u_{1},v_{1},1]^{T},x_{2}=[u_{2},v_{2},1]^{T}$ 。根据对极约束，有

                                         $(u_{2},v_{2},1)\begin{pmatrix} e_{1} & e_{2}&e_{3} \\ e_{4} &e_{5} & e_{6}\\ e_{7} & e_{8}& e_{9} \end{pmatrix} \begin{pmatrix} u_{1}\\v_{1} \\ 1 \end{pmatrix}=0$

        我们把矩阵 $E$ 展开，写成向量的形式： $e=(e_{1},e_{2},e_{3},e_{4},e_{5},e_{6},e_{7},e_{8},e_{9})^{T}$

        那么，对极约束可以写成与 $e$ 有关的线性形式：

$[u_{2}u_{1},u_{2}v_{1},u_{2},v_{2}u_{1},v_{2}v_{1},v_{2},u_{1},v_{1},1]\cdot e=0$

        同理，对于其他点对也有相同的表示。我们把所有点都放到一个方程中，变成线性方程组( $u^{i},v^{i}$ 表示第 $i$ 个特征点,依此类推)：

        这8个方程构成了一个线性方程组。它的系数矩阵由特征点位置构成，大小为8×9。

         $e$ 位于该矩阵的零空间中。如果系数矩阵是满秩的(即秩为8)，那么它的零空间维数为1，也就是 $e$ 构成一条线。这与 $e$ 的尺度等价性是一致的。如果8对匹配点组成的矩阵满足秩为8的条件，那么 $E$ 的各元素就可由上述方程解得。

6.根据本质矩阵估计相机运用参数R,t

         接下来的问题是如何根据已经估得的本质矩阵E,恢复出相机的运动 $R,t$ 。这个过程是由奇异值分解（SVD）得到的。设E的SVD为： $E=U\Sigma V^{T}$

         其中 $U,V$ 为正交阵， $\Sigma$ 为奇异值矩阵。根据 $E$ 的内在性质，我们知道 $\Sigma =diag(\sigma ,\sigma ,0)$ 。在SVD分解中，对于任意一个 $E$ ，存在两个可能的 $t,R$ 与它对应：



         其中， $R_{Z}(\frac{\pi }{2})$ 表示沿 $Z$ 轴旋转90°得到旋转矩阵。同时，由于 $-E$ 和 $E$ 等价，所以对任意一个 $t$ 取负号，也会得到同样的结果。因此，从 $E$ 分解到 $t,R$ 时，一共存在4个可能的解。
        下图展示了分解本质矩阵得到的4个解。我们已知空间点在相机(蓝色线)上的投影(红色点)，想要求解相机的运动。在保持红色点不变的情况下，可以画出4种可能的情况:

        只有第一种解中P在两个相机中都具有正的深度。因此，只要把任意一点代入4种解中，检测该点在两个相机下的深度，就可以确定哪个解是正确的了。

         如果利用E的内在性质，那么它只有5个自由度。所以最少可以通过5对点来求解相机运动。然而这种做法形式复杂，从工程实现角度考虑，由于平时通常会有几十对乃至上百对的匹配点，从8对减至5对意义并不明显。为保持简单，我们这里就只介绍基本的八点法。
        剩下的一个问题是：根据线性方程解出的 $E$ ，可能不满足 $E$ 的内在性质——它的奇异值不一定为 $\sigma ,\sigma ,0$ 的形式。这时，我们会刻意地把矩阵调整成上面的样子。通常的做法是，对八点法求得的 $E$ 进行SVD，会得到奇异值矩阵 $\Sigma =diag(\sigma_{1} ,\sigma_{2} ,\sigma _{3})$ ，不妨设 $\sigma_{1}\geqslant \sigma_{2}\geqslant \sigma _{3}$ 。取:



         这相当于是把求出来的矩阵投影到了 $E$ 所在的流形上。当然，更简单的做法是将奇异值矩阵取成 $diag(1,1,0)$ ，因为 $E$ 具有尺度等价性，所以这样做也是合理的。

2.3 计算KF1的相机中心在KF2图像平面的二维像素坐标

    const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
    const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;

    // Compute epipole in second image
    // Step 1 计算KF1的相机中心在KF2图像平面的二维像素坐标
    // KF1相机光心在世界坐标系坐标Cw
    cv::Mat Cw = pKF1->GetCameraCenter(); 
    // KF2相机位姿R2w,t2w,是世界坐标系到相机坐标系
    cv::Mat R2w = pKF2->GetRotation();    
    cv::Mat t2w = pKF2->GetTranslation(); 
    // KF1的相机光心转化到KF2坐标系中的坐标
    cv::Mat C2 = R2w*Cw+t2w; 
    const float invz = 1.0f/C2.at<float>(2);
    // 得到KF1的相机光心在KF2中的坐标，也叫极点，这里是像素坐标
    const float ex =pKF2->fx*C2.at<float>(0)*invz+pKF2->cx;
    const float ey =pKF2->fy*C2.at<float>(1)*invz+pKF2->cy;

    // Find matches between not tracked keypoints
    // Matching speed-up by ORB Vocabulary
    // Compare only ORB that share the same node

    int nmatches=0;
    // 记录匹配是否成功，避免重复匹配
    vector<bool> vbMatched2(pKF2->N,false);        
    vector<int> vMatches12(pKF1->N,-1);
    // 用于统计匹配点对旋转差的直方图
    vector<int> rotHist[HISTO_LENGTH];
    for(int i=0;i<HISTO_LENGTH;i++)
        rotHist[i].reserve(500);

     //! 原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的，更改为下面代码   
    const float factor = HISTO_LENGTH/360.0f;

建立指向帧1和帧2的FeatureVector迭代器vFeatVec1 、vFeatVec2。

我们计算KF1的相机中心在KF2图像平面的二维像素坐标即e2。步骤如下：

1.获取KF1相机光心在世界坐标系坐标Cw。

2.获取KF2相机位姿R2w,t2w,是世界坐标系到相机坐标系。

3.计算KF1的相机光心转化到KF2坐标系中的坐标C2 。

4.计算极点坐标ex，ey。

2.4 利用BoW加速匹配：只对属于同一节点(特定层)的ORB特征进行匹配

// Step 2 利用BoW加速匹配：只对属于同一节点(特定层)的ORB特征进行匹配
    // FeatureVector其实就是一个map类，那就可以直接获取它的迭代器进行遍历
    // FeatureVector的数据结构类似于：{(node1,feature_vector1) (node2,feature_vector2)...}
    // f1it->first对应node编号，f1it->second对应属于该node的所有特特征点编号
    DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
    DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
    DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
    DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();

    // Step 2.1：遍历pKF1和pKF2中的node节点
    while(f1it!=f1end && f2it!=f2end)
    {
        // 如果f1it和f2it属于同一个node节点才会进行匹配，这就是BoW加速匹配原理
        if(f1it->first == f2it->first)
        {
            // Step 2.2：遍历属于同一node节点(id：f1it->first)下的所有特征点
            for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++)
            {
                // 获取帧1即pKF1中属于该node节点的描述子索引 0 --- 数量-1
                const size_t idx1 = f1it->second[i1];
                
                // Step 2.3：通过特征点索引idx1在pKF1中取出对应的MapPoint
                MapPoint* pMP1 = pKF1->GetMapPoint(idx1);
                
                // If there is already a MapPoint skip
                // 由于寻找的是未匹配的特征点，所以pMP1应该为NULL
                if(pMP1)
                    continue;

                // 如果mvuRight中的值大于0，表示是双目，且该特征点有深度值
                const bool bStereo1 = pKF1->mvuRight[idx1]>=0;

                if(bOnlyStereo)
                    if(!bStereo1)
                        continue;
                
                // Step 2.4：通过特征点索引idx1在pKF1中取出对应的特征点
                const cv::KeyPoint &kp1 = pKF1->mvKeysUn[idx1];
                
                // 通过特征点索引idx1在pKF1中取出对应的特征点的描述子
                const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);
                
                int bestDist = TH_LOW;
                int bestIdx2 = -1;
                
                // Step 2.5：遍历该node节点下(f2it->first)对应KF2中的所有特征点
                for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++)
                {
                    // 获取pKF2中属于该node节点的所有特征点索引
                    size_t idx2 = f2it->second[i2];
                    
                    // 通过特征点索引idx2在pKF2中取出对应的MapPoint
                    MapPoint* pMP2 = pKF2->GetMapPoint(idx2);
                    
                    // If we have already matched or there is a MapPoint skip
                    // 如果pKF2当前特征点索引idx2已经被匹配过或者对应的3d点非空，那么跳过这个索引idx2
                    if(vbMatched2[idx2] || pMP2)
                        continue;

                    const bool bStereo2 = pKF2->mvuRight[idx2]>=0;

                    if(bOnlyStereo)
                        if(!bStereo2)
                            continue;
                    
                    // 通过特征点索引idx2在pKF2中取出对应的特征点的描述子
                    const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);
                    
                    // Step 2.6 计算idx1与idx2在两个关键帧中对应特征点的描述子距离
                    const int dist = DescriptorDistance(d1,d2);
                    
                    if(dist>TH_LOW || dist>bestDist)
                        continue;

                    // 通过特征点索引idx2在pKF2中取出对应的特征点
                    const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];

                    //? 为什么双目就不需要判断像素点到极点的距离的判断？
                    // 因为双目模式下可以左右互匹配恢复三维点
                    if(!bStereo1 && !bStereo2)
                    {
                        const float distex = ex-kp2.pt.x;
                        const float distey = ey-kp2.pt.y;
                        // Step 2.7 极点e2到kp2的像素距离如果小于阈值th,认为kp2对应的MapPoint距离pKF1相机太近，跳过该匹配点对
                        // 作者根据kp2金字塔尺度因子(scale^n，scale=1.2，n为层数)定义阈值th
                        // 金字塔层数从0到7，对应距离 sqrt(100*pKF2->mvScaleFactors[kp2.octave]) 是10-20个像素
                        //? 对这个阈值的有效性持怀疑态度
                        if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave])
                            continue;
                    }

                    // Step 2.8 计算特征点kp2到kp1对应极线的距离是否小于阈值
                    if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2))
                    {
                        // bestIdx2，bestDist 是 kp1 对应 KF2中的最佳匹配点 index及匹配距离
                        bestIdx2 = idx2;
                        bestDist = dist;
                    }
                }

                
                if(bestIdx2>=0)
                {
                    const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];
                    // 记录匹配结果
                    vMatches12[idx1]=bestIdx2;      
                    
                    vbMatched2[bestIdx2]=true;  // !记录已经匹配，避免重复匹配。原作者漏掉！
                    nmatches++;

                    // 记录旋转差直方图信息
                    if(mbCheckOrientation)
                    {
                        // angle：角度，表示匹配点对的方向差。
                        float rot = kp1.angle-kp2.angle;
                        if(rot<0.0)
                            rot+=360.0f;
                        int bin = round(rot*factor);
                        if(bin==HISTO_LENGTH)
                            bin=0;
                        assert(bin>=0 && bin<HISTO_LENGTH);   
                        rotHist[bin].push_back(idx1);
                    }
                }
            }

            f1it++;
            f2it++;
        }
        else if(f1it->first < f2it->first)
        {
            f1it = vFeatVec1.lower_bound(f2it->first);
        }
        else
        {
            f2it = vFeatVec2.lower_bound(f1it->first);
        }
    }