ORB-SLAM2 ---- ORBmatcher::SearchForInitialization函数

目录

1.函数作用

2.执行流程 

3.函数参数解析 

4.code 

5.函数解析 

5.1 旋转直方图的构建与作用

5.2 遍历帧1中的特征点在帧2中找出候选匹配特征点 

5.3 第一层筛选  -- 阈值、最优/次优比例、重复匹配

5.4  第二层筛选  -- 旋转直方图

5.5 final 将最后通过筛选的匹配好的特征点保存到vbPrevMatched 

---

1.函数作用

        单目初始化中用于参考帧和当前帧的特征点匹配。

2.执行流程 

Step 1 构建旋转直方图
Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 
Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
Step 5 计算匹配点旋转角度差所在的直方图
Step 6 筛除旋转直方图中“非主流”部分
Step 7 将最后通过筛选的匹配好的特征点保存

3.函数参数解析 

@param[in] F1                        初始化参考帧                  
@param[in] F2                        当前帧
@param[in & out] vbPrevMatched       本来存储的是参考帧的所有特征点坐标，该函数更新为匹配好的当前帧的特征点坐标
@param[in & out] vnMatches12         保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
@param[in] windowSize                搜索窗口
@return int                          返回成功匹配的特征点数目

4.code 

int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector &vbPrevMatched, vector &vnMatches12, int windowSize)
{
    int nmatches=0;
    // F1中特征点和F2中匹配关系，注意是按照F1特征点数目分配空间
    vnMatches12 = vector(F1.mvKeysUn.size(),-1);

    // Step 1 构建旋转直方图，HISTO_LENGTH = 30
    vector rotHist[HISTO_LENGTH];
    for(int i=0;i vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);
    // 从帧2到帧1的反向匹配，注意是按照F2特征点数目分配空间
    vector vnMatches21(F2.mvKeysUn.size(),-1);

    // 遍历帧1中的所有特征点
    for(size_t i1=0, iend1=F1.mvKeysUn.size(); i10)
            continue;

        // Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 
        // vbPrevMatched 输入的是参考帧 F1的特征点
        // windowSize = 100，输入最大最小金字塔层级 均为0
        vector vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);

        // 没有候选特征点，跳过
        if(vIndices2.empty())
            continue;

        // 取出参考帧F1中当前遍历特征点对应的描述子
        cv::Mat d1 = F1.mDescriptors.row(i1);

        int bestDist = INT_MAX;     //最佳描述子匹配距离，越小越好
        int bestDist2 = INT_MAX;    //次佳描述子匹配距离
        int bestIdx2 = -1;          //最佳候选特征点在F2中的index

        // Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
        for(vector::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)
        {
            size_t i2 = *vit;
            // 取出候选特征点对应的描述子
            cv::Mat d2 = F2.mDescriptors.row(i2);
            // 计算两个特征点描述子距离
            int dist = DescriptorDistance(d1,d2);

            if(vMatchedDistance[i2]<=dist)
                continue;
            // 如果当前匹配距离更小，更新最佳次佳距离
            if(dist=0)
                {
                    vnMatches12[vnMatches21[bestIdx2]]=-1;
                    nmatches--;
                }
                // 次优的匹配关系，双向建立
                // vnMatches12保存参考帧F1和F2匹配关系，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
                vnMatches12[i1]=bestIdx2;
                vnMatches21[bestIdx2]=i1;
                vMatchedDistance[bestIdx2]=bestDist;
                nmatches++;

                // Step 5 计算匹配点旋转角度差所在的直方图
                if(mbCheckOrientation)
                {
                    // 计算匹配特征点的角度差，这里单位是角度°，不是弧度
                    float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;
                    if(rot<0.0)
                        rot+=360.0f;
                    // 前面factor = HISTO_LENGTH/360.0f 
                    // bin = rot / 360.of * HISTO_LENGTH 表示当前rot被分配在第几个直方图bin  
                    int bin = round(rot*factor);
                    // 如果bin 满了又是一个轮回
                    if(bin==HISTO_LENGTH)
                        bin=0;
                    assert(bin>=0 && bin=0)
                {
                    vnMatches12[idx1]=-1;
                    nmatches--;
                }
            }
        }

    }

    //Update prev matched
    // Step 7 将最后通过筛选的匹配好的特征点保存到vbPrevMatched
    for(size_t i1=0, iend1=vnMatches12.size(); i1=0)
            vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;

    return nmatches;
}

5.函数解析 

5.1 旋转直方图的构建与作用

        一般的直方图如下图所示，频数和区间是描述某一范围的样本数量的图形化表示，比如我们将0-300分为10个区间，那么第一个小块代表的就是0-30分的样本数量。

        在ORBSLAM2中，我们用旋转直方图表达“旋转角度差”。这个是描述下面一个事情：

        我们在提取特征点时，计算了一个特征点的灰度质心角，当我们匹配两帧特征点时，即使两帧间发生了一定的旋转，但是他们的灰度质心角的差应该是相似的，如下图：

        我们构建旋转直方图，创建了HISTO_LENGTH=30个区间，给个区间对应360/30=20°，每个空间预留空间为500。

5.2 遍历帧1中的特征点在帧2中找出候选匹配特征点 

        我们遍历每一个帧一中的每一个特征点：

        同level1变量记录帧一中正在遍历特征点的在图像金字塔中的层数，在单目初始化时我们只使用原始图像上提取的特征点，因此如果level1>0则不对这个特征点进行匹配。

        我们用GetFeaturesInArea函数来求在帧二中与当前特征点待匹配特征点的候选匹配特征点的集合。

ORB-SLAM2 ---- Frame::GetFeaturesInArea函数解析https://blog.csdn.net/qq_41694024/article/details/128227154        到这里，我们对于帧一的某特征点找到了帧二中的候选特征点集合vIndices2。

5.3 第一层筛选  -- 阈值、最优/次优比例、重复匹配

        我们在上一步得到了第二帧中与第一帧的某一特征点候选的匹配特征点的集合vIndices2。

        现在我们计算帧一中的特征点与vIndices2中的每一个待匹配特征点的描述子距离。我们知道，描述子的汉明距离越小匹配度越好，我们遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的特征点匹配结果，它们在vIndices2的索引是bestDist、bestDist2。

        ①阈值判定：我们即使计算出了最优距离和次优距离，但是这个距离越大的话也就意味着这个匹配越不精准，因此我们设置阈值TH_LOW，只有最佳描述子匹配距离小于设定阈值才保证匹配的精准性，若不满足则舍弃这对匹配点。

        ②最优/次优比例：最佳距离比次佳距离要小于设定的比例，这样特征点辨识度更高。

        ③重复匹配：由于我们匹配的特征点有可能存在重复匹配问题，因此这步也是必须的：

        vnMatches21向量存储了帧2到帧1的匹配关系，如果帧二的vnMatches21[bestIdx2]不为空，即第二帧的我们刚刚计算出的次好待匹配特征点的索引不为空，则帧2的bestIdx2号索引的特征点与第一帧中索引号为vnMatches21[bestIdx2]的特征点存在匹配关系，则存在重复匹配的问题，我们将vnMatches12[vnMatches21[bestIdx2]]，即第一帧的索引为vnMatches21[bestIdx2]的帧的匹配关系置为空（-1）（清除匹配关系），成功匹配数量nmatches--。

        ④若①②③通过检查后，我们建立双向索引

vnMatches12[i1]=bestIdx2;
vnMatches21[bestIdx2]=i1;

        存储匹配点最优距离

vMatchedDistance[bestIdx2]=bestDist;

        成功匹配数量nmatches++。     

5.4  第二层筛选  -- 旋转直方图

        计算匹配点旋转角度差所在的直方图，筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引，剔除掉不在前三的匹配对，因为他们不符合“主流旋转方向” 。

5.5 final 将最后通过筛选的匹配好的特征点保存到vbPrevMatched 

        在匹配完帧一与帧二的特征点后，我们看看我们的参数向量还有局部向量里面都是什么

        ①vbPrevMatched：现在存储的是参考帧（第一帧）的所有特征点坐标

        ②vnMatches12：vnMatches12[i]=j，表示第一帧中索引为i的特征点与第二帧中索引为j的特征点相匹配。若vnMatches12[i] = -1，则表示表示第一帧中索引为i的特征点在第二帧中没有待匹配的特征点。

        ③vMatchedDistance，它的大小为第二帧特征点的数量，里面存放的int型向量是与第一帧中某一特征点匹配的最佳描述子距离。

        ④vnMatches21：从帧2到帧1的反向匹配，前面已经解释过。

        ⑤nmatches：成功匹配的特征点数量

        我们在最后要更新vbPrevMatched向量：

for(size_t i1=0, iend1=vnMatches12.size(); i1=0)
            vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;

        对于每一个匹配的特征点对，我们将vbPrevMatched向量中的内容改变为与第一帧中索引i1相匹配的第二帧中的某一特征点的坐标。

        最后我们返回给调用函数成功匹配的特征点对数目nmatches，函数结束。