ORB-SLAM2学习（原理）：ORBmatcher.cc

小白tb

已于 2023-02-18 13:23:48 修改

阅读量268

点赞数

分类专栏： slam C++学习文章标签：学习算法 c++

于 2022-10-27 17:40:15 首次发布

本文链接：https://blog.csdn.net/fxmtb/article/details/127534388

版权

slam 同时被 2 个专栏收录

13 篇文章 1 订阅

订阅专栏

C++学习

9 篇文章 0 订阅

订阅专栏

ORB-SLAM2学习（原理）：ORBmatcher.cc

详细中文源码解读：链接：https://pan.baidu.com/s/1LWfowy5wbUdXamEGE1STcA 提取码：t796
PS:该代码从“计算机视觉life”客服处免费获得，感觉确实挺详细的，就标明一下出处，侵权则删。

一、ORBmatcher::SearchForInitialization函数

MonocularInitialization() 中使用了这个函数。
参考链接： ORB-SLAM2 ---- ORBmatcher::SearchForInitialization函数

/**
 * @brief 单目初始化中用于参考帧和当前帧的特征点匹配
 * 步骤
 * Step 1 构建旋转直方图
 * Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 
 * Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
 * Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
 * Step 5 计算匹配点旋转角度差所在的直方图
 * Step 6 筛除旋转直方图中“非主流”部分
 * Step 7 将最后通过筛选的匹配好的特征点保存
 * @param[in] F1                        初始化参考帧                  
 * @param[in] F2                        当前帧
 * @param[in & out] vbPrevMatched       本来存储的是参考帧的所有特征点坐标，该函数更新为匹配好的当前帧的特征点坐标
 * @param[in & out] vnMatches12         保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
 * @param[in] windowSize                搜索窗口
 * @return int                          返回成功匹配的特征点数目
 */
int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize)

函数作用：用于单目初始化中用于参考帧和当前帧的特征点匹配。
难理解参数介绍：

vbPrevMatched作为输出
记录的是匹配成功的点坐标信息。即如果第一帧的第一个特征点成功匹配到第二帧索引的第二十个特征点。vbPrevMatched[0]中储存的值为第二帧第二十个特征点的坐标。

vnMatches12是搜索窗口
大小定为帧1中经过去畸变后得到的特征点的数量。具体含义举个例子，比如vnMatches12[0] = 4的含义就是帧1中的第一个特征点和帧2中1的第5个特征点成功匹配。刚开始我们全都初始化为-1说明匹配关系为空。

windowSize是搜索窗口
这个主要用于后面的函数输入，并且windowSize = 100；
GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);
函数可以参考链接： ORB-SLAM2 ---- Frame::GetFeaturesInArea 函数

第一步，构建旋转直方图

    int nmatches=0;
    // F1中特征点和F2中匹配关系，注意是按照F1特征点数目分配空间
    vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1);

    // Step 1 构建旋转直方图，HISTO_LENGTH = 30
    vector<int> rotHist[HISTO_LENGTH];
    for(int i=0;i<HISTO_LENGTH;i++)
    // 每个bin里预分配500个，因为使用的是vector不够的话可以自动扩展容量
        rotHist[i].reserve(500);   

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

    // 匹配点对距离，注意是按照F2特征点数目分配空间
    vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);
    // 从帧2到帧1的反向匹配，注意是按照F2特征点数目分配空间
    vector<int> vnMatches21(F2.mvKeysUn.size(),-1);

    // 遍历帧1中的所有特征点
    for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1<iend1; i1++)
    {
        cv::KeyPoint kp1 = F1.mvKeysUn[i1];
        int level1 = kp1.octave;
        // 只使用原始图像上提取的特征点
        if(level1>0)
            continue;

这里主要是进行一些初始化设置，现在对一些参数含义进行理解：

vnMatches12和vnMatches21
vnMatches12保存参考帧F1和F2匹配关系，i1(index)保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引，例如下面：
vnMatches12[i1]=bestIdx2;
vnMatches21[bestIdx2]=i1;
其中bestIdx2是最佳候选特征点在F2中的index.
rotHist
vector < int > rotHist[HISTO_LENGTH]，HISTO_LENGTH = 30，表示将360°分成了30个区域，即0°-12°，12°-24°…，统计每个区域所含的特征点的个数即为rotHist[i] ，标准直方图示意图如下。
factor
定义了factor = HISTO_LENGTH/360.0f，这里HISTO_LENGTH是分布直方图的区间数目

第二步，在半径窗口内搜索当前帧F2中所有的候选匹配特征点

// Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 
 // vbPrevMatched 输入的是参考帧 F1的特征点
 // windowSize = 100，输入最大最小金字塔层级 均为0
 vector<size_t> 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

GetFeaturesInArea函数
该函数的作用就是以F1某一特征点作为中心以windowSize=100作为半径的圆，搜索在这个区域内的特征点，返回返回搜索到的候选匹配点id。

即找到参考帧中某个特征点在当前帧这个圆类蓝色的特征点。PS:可以注意一下黄色区域，同一小格vCell内，按照欧式距离计算，a特征点不属于计算范围，b属于。具体请看参考链接： ORB-SLAM2学习（原理）：Frame.cc （7）获得特定区域内的坐标点，Frame::GetFeaturesInArea，里面有关于该函数的具体讲解。

问： cv::Mat d1 = F1.mDescriptors.row(i1);这里为什么是.row(i1)取出呢，
答：因为ORBextractor::operator()重构函数里面对描述子的定义：行为第几个特征点的，列为对应的描述子。
_descriptors.create(nkeypoints, //矩阵的行数，对应为特征点的总个数
32, //矩阵的列数，对应为使用32*8=256位描述子
CV_8U); //矩阵元素的格式

第三步，遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的

  // Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
  for(vector<size_t>::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<bestDist)
      {
          bestDist2=bestDist;
          bestDist=dist;
          bestIdx2=i2;
      }
      else if(dist<bestDist2)
      {
          bestDist2=dist;
      }
  }

DescriptorDistance函数
ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b)作用是计算两个描述子二进制串的汉明距离。
遍历所有圈内特征点，找到最佳描述子匹配距离、次佳描述子匹配距离和最佳候选特征点在F2中的index。

第四步，对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配

// Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
// 即使算出了最佳描述子匹配距离，也不一定保证配对成功。要小于设定阈值
if(bestDist<=TH_LOW)
{
   // 最佳距离比次佳距离要小于设定的比例，这样特征点辨识度更高
   if(bestDist<(float)bestDist2*mfNNratio)
   {
       // 如果找到的候选特征点对应F1中特征点已经匹配过了，说明发生了重复匹配，将原来的匹配也删掉
       if(vnMatches21[bestIdx2]>=0)
       {
           vnMatches12[vnMatches21[bestIdx2]]=-1;
           nmatches--;
       }
       // 次优的匹配关系，双向建立
       // vnMatches12保存参考帧F1和F2匹配关系，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
       vnMatches12[i1]=bestIdx2;
       vnMatches21[bestIdx2]=i1;
       vMatchedDistance[bestIdx2]=bestDist;
       nmatches++;

对上一步获得的最优匹配点还需要进一步筛选，才能确定最终确定匹配成功：

匹配距离小于阈值TH_LOW（TH_LOW = 50）
最佳匹配点的汉明距离/次佳匹配点的汉明距离小于mfNNratio（参数默认值为0.6）
如果找到的候选特征点对应F1中特征点已经匹配过了，说明发生了重复匹配，将原来的匹配也删掉。
如果都没有问题，**”暂时“**认为特征点匹配成功，更新匹配容器vnMatches12、vnMatches21、vMatchedDistance，成功匹配特征点的数量nmatches+1。

vnMatches12和vnMatches21上面有讲解。vnMatches12保存参考帧F1和F2匹配关系，i1(index)保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引，其中bestIdx2是最佳候选特征点在F2中的index。

第五步，计算匹配点旋转角度差所在的直方图

// 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<HISTO_LENGTH);
       rotHist[bin].push_back(i1);
                }

关于特征点的angle属性如何计算及意义？
还记得之前讲 ORB-SLAM2学习（原理）：ORBextractor.cc优化了特征点，将FAST特征点，加上了方向特性，即Oriented FAST，我们计算出两个特征点的angle差rot，如果为负，则加360°，使其在0°-360°之间。
定义了factor = HISTO_LENGTH/360.0f，这里HISTO_LENGTH是分布直方图的区间数目。
int bin = round(rot * factor);
round是四舍五入，rot*factor表示匹配的特征点的角度差落在哪个分布直方图的区间内，则将将帧2中匹配的特征点放入直方图的一个区间内。比如：
如果计算角度差值为5° 则5/360 * 30 = 0.42 放入第0个直方图内。
如果计算角度差值为15° 则15/360 * 30 = 1.26 放入第1个直方图内。
如果计算角度差值为140° 则140/360 * 30 = 11.67 放入第12个直方图内。

bool mbCheckOrientation; ///< 是否检查特征点的方向
若此检查标志位为true，即执行一次第五步，先将特征点按照上诉方法分类放入直方图中。

第六步，筛除旋转直方图中“非主流”部分

// Step 6 筛除旋转直方图中“非主流”部分
if(mbCheckOrientation)
{
    int ind1=-1;
    int ind2=-1;
    int ind3=-1;
    // 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引
    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++)
        {
            int idx1 = rotHist[i][j];
            if(vnMatches12[idx1]>=0)
            {
                vnMatches12[idx1]=-1;
                nmatches--;
            }
        }
    }
}

这一步即根据方向特性angle，挑选出方向区间内数量最多的前三个方向区间的bin的索引，然后去除不在前三的特征点。
ComputeThreeMaxima函数下面讲解

第七步，将最后通过筛选的匹配好的特征点保存到vbPrevMatched

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

 return nmatches;

若帧1到帧2中帧1的某个特征点成功匹配到帧2的某个特征点(vnMatches12[i1]>=0)，则更新vbPrevMatched(本来存储的是参考帧的所有特征点坐标，该函数更新为匹配好的当前帧的特征点坐标)
即如果第一帧的第五个特征点成功匹配到第二帧的第五十个特征点。
vbPrevMatched[4]中储存的值为第二帧第五十个特征点的坐标。
最后返回匹配成功的特征点对数给上层函数