总结|ORB_SLAM2源码中字典使用细节

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

前言

前段时间，主要对ORB-SLAM2中字典的训练与使用进行了些研究，关于字典的训练之前也写过一篇文章：VSLAM|回环检测之词袋字典如何生成？，简单讲解了如何使用我们自己的数据集进行训练字典，ORB-SLAM作者提供的是字典层数为6层，当然我们也可以训练更低层数的字典，以减小程序所占内存。

本篇文章，主要就单目ORB-SLAM2源码中使用字典的一些函数进行简单剖析。当然，笔者也刚入行VSLAM时间不长，如有不到之处，还请多批评指正。

备注：对于下述的代码注释，主要借鉴了泡泡机器人给出的中文注释

粗略统计了下，单目ORB-SLAM2中主要有四个地方涉及到了字典，以下介绍其函数细节。

一 系统初始化时，加载字典bin或者txt文件

在mono_tum.cc的main函数中，对SLAM系统初始时（主要创建了SLAM系统，初始化了各个线程，为能够处理每帧图片做准备）。

ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

在类System()类的构造函数里，进行了字典文件的加载。

mpVocabulary = new ORBVocabulary();
 bool bVocLoad = false; // chose loading method based on file extension
   if (has_suffix(strVocFile, ".txt"))
  bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
else if(has_suffix(strVocFile, ".bin"))
  bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile);
else
  bVocLoad = false;

接下来，我们重点分析下上述的loadFromBinaryFile()函数的实现细节。

为此，我们需要弄清楚ORBvoc.txt文件中的数据保存格式（对于ORBvoc.bin，由于为二进制文件，此处没办法展示）。

 

10 6 0 0 #分别表示上面的树的分支、 树的深度、 相似度、 权重

0 0 252 188 188 242 169 109 85 143 187 191 164 25 222 255 72 27 129 215

237 16 58 111 219 51 219 211 85 127 192 112 134 34 0

...

#0 表示节点的父节点；0 表示是否是叶节点， 是的话为 1， 否则为 0;252-34 表示 orb 特征；最后一位是权重。

那么以上的ORBvoc.txt里的数据是如何保存的呢？这里不得不提一下saveToTextFile()函数。

template<class TDescriptor, class F>
void TemplatedVocabulary<TDescriptor, F>::saveToTextFile(const std::string& filename) const
{
  std::ofstream ofs;
  ofs.open(filename.c_str(), std::ios_base::out);

  if(!ofs)
  {
    throw std::string("Could not open file: ") + filename;
  }
//注意这里的数据类型格式：

//m_k类型为int,m_L类型为int,m_scoring为枚举类型，此处的0表示相似度//计算方式L1_NORM ，m_weighting为枚举类型，此处的0表示权重TF_IDF;

  ofs << m_k << " " << m_L << " " << " " << m_scoring << " " << m_weighting << std::endl;

  for(size_t i = 1; i < m_nodes.size(); ++i)
  {
    const Node& node = m_nodes.at(i);

//第二行开始，第一个数为节点的父节点
    ofs << node.parent << " ";

//第二行的第二个数，表示是否为叶节点，如果是叶节点，则是1，否则为0
    if(node.isLeaf())
    {
      ofs << 1 << " ";
    }
    else
    {
      ofs << 0 << " ";
    }
    //第二行，接下来表示orb特征；最后一位是权重。
    ofs << F::toString(node.descriptor) << " " << static_cast<double>(node.weight) << std::endl;
  }
  ofs.close();
}

而如果ORB-SLAM2系统中你使用的是bin文件，那么我们需要使用以下代码进行数据保存。

template<class TDescriptor, class F>
void TemplatedVocabulary<TDescriptor, F>::saveToBinaryFile(const std::string& filename) const
{
  std::ofstream ofs;
  ofs.open(filename.c_str(), std::ios_base::out | std::ios::binary);

  if(!ofs)
  {
    throw std::string("Could not open file: ") + filename;
  }
  const unsigned int n_nodes = m_nodes.size(); //n_nodes=1082073
  const unsigned int node_size = sizeof(m_nodes.at(0).parent) + F::L * sizeof(char) + sizeof(float) + sizeof(bool);

//sizeof(m_nodes.at(0).parent)==4;

//F::L * sizeof(char)==32;

//sizeof(float)=4;

//sizeof(bool)==1;

//node_size== 41;

  ofs.write((char*)&n_nodes, sizeof(n_nodes));
  ofs.write((char*)&node_size, sizeof(node_size));
  ofs.write((char*)&m_k, sizeof(m_k));
  ofs.write((char*)&m_L, sizeof(m_L));
  ofs.write((char*)&m_scoring, sizeof(m_scoring));
  ofs.write((char*)&m_weighting, sizeof(m_weighting));
  for(size_t i = 1; i < n_nodes; ++i)
  {
    const Node& node = m_nodes.at(i);

    ofs.write((char*)&node.parent, sizeof(node.parent));
    ofs.write((char*)node.descriptor.data, F::L);

    const float weight = node.weight;
    ofs.write((char*)&weight, sizeof(weight));
    const bool is_leaf = node.isLeaf();
    ofs.write((char*)&is_leaf, sizeof(is_leaf));
  }
  ofs.close();
}

而对于ORBvoc.bin文件的读取，我们的函数体为：

template<class TDescriptor, class F>
void TemplatedVocabulary<TDescriptor, F>::loadFromBinaryFile(const std::string& filename)
{
  std::ifstream ifs; //定义文件
  ifs.open(filename.c_str(), std::ios_base::in | std::ios::binary);//打开文件

  if(!ifs)
  {
    throw std::string("Could not open file: ") + filename;
  } //如果文件读取失败
  unsigned int n_nodes, node_size; //n_nodes=1082074 node_size=41;
  ifs.read((char*)&n_nodes, sizeof(n_nodes));
  ifs.read((char*)&node_size, sizeof(node_size));
  ifs.read((char*)&m_k, sizeof(m_k)); //读取第一行树的分支数
  ifs.read((char*)&m_L, sizeof(m_L)); //读取字典的层数
  ifs.read((char*)&m_scoring, sizeof(m_scoring)); //读取第一行相似度
  ifs.read((char*)&m_weighting, sizeof(m_weighting));//读取第一行权重值
  createScoringObject();
  m_words.clear();
  m_words.reerve(std::pow(static_cast<double>(m_k), static_cast<double>(m_L) + 1.0)); //10000000
  m_nodes.clear();
 m_nodes.resize(n_nodes);
  m_nodes.at(0).id = 0;
  char* buf = new char[node_size];
  unsigned int n_id = 1;
  while(!ifs.eof())
  {
    ifs.read(buf, node_size);
    m_nodes.at(n_id).id = n_id;
    const int* ptr = (int*)buf;
    m_nodes.at(n_id).parent = *ptr;
    m_nodes.at(m_nodes.at(n_id).parent).children.push_back(n_id);
    m_nodes.at(n_id).descriptor = cv::Mat(1, F::L, CV_8U);
    memcpy(m_nodes.at(n_id).descriptor.data, buf + 4, F::L); //获取字典每行的描述子
    m_nodes.at(n_id).weight = *reinterpret_cast<float*>(buf + 4 + F::L);//获取字典每行最后一位：单词的权重。
    //F::L== 32
    //sizeof(char)== 1
    //sizeof(unsigned int)== 4
    //sizeof(float)== 4

    if(buf[8 + F::L])
    {
      const int w_id = m_words.size();
      m_words.resize(w_id + 1);
      m_nodes.at(n_id).word_id = w_id;
      m_words.at(w_id) = &m_nodes.at(n_id);
    }
    else
    {
      m_nodes.at(n_id).children.reserve(m_k);
    }

    ++n_id;
    if(n_id == n_nodes) {
      break;
    }
  }

  ifs.close();

  delete[] buf;
}

二 当前帧，计算词袋

函数ComputeBoW()在ORB-SLAM2中多次被调用。

一次是在Tracking::TrackReferenceKeyFrame()里：

/**
 * @brief 对参考关键帧的MapPoints进行跟踪
 *
 * 1. 计算当前帧的词包，将当前帧的特征点分到特定层的nodes上
 * 2. 对属于同一node的描述子进行匹配
 * 3. 根据匹配对估计当前帧的姿态
 * 4. 根据姿态剔除误匹配
 * @return 如果匹配数大于10，返回true
 */
bool Tracking::TrackReferenceKeyFrame()
{
    // Compute Bag of Words vector
    // 步骤1：将当前帧的描述子转化为BoW向量
    mCurrentFrame.ComputeBoW();

    // We perform first an ORB matching with the reference keyframe
    // If enough matches are found we setup a PnP solver
    ORBmatcher matcher(0.7,true);
    vector<MapPoint*> vpMapPointMatches;

    // 步骤2：通过特征点的BoW加快当前帧与参考帧之间的特征点匹配
    // 特征点的匹配关系由MapPoints进行维护
    int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);

   // ignore something unimportant

}

另一次是在Tracking::Relocalization()中调用：

bool Tracking::Relocalization()
{
    // Compute Bag of Words Vector
    // 步骤1：计算当前帧特征点的Bow映射
    mCurrentFrame.ComputeBoW();

    // Relocalization is performed when tracking is lost
    // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
    // 步骤2：找到与当前帧相似的候选关键帧
    vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);

    //没有找到与当前帧相似的候选关键帧？那怎么办呢，只好退出啦。
    if(vpCandidateKFs.empty())
        return false;

    const int nKFs = vpCandidateKFs.size();

    // We perform first an ORB matching with each candidate
    // If enough matches are found we setup a PnP solver
    ORBmatcher matcher(0.75,true);

    //ignore something unimportant

}//重定位

而上述两次调用，都是调用的Frame类里的函数：

/**
 * @brief Bag of Words Representation
 *
 * 计算词包mBowVec和mFeatVec
 * @see CreateInitialMapMonocular() TrackReferenceKeyFrame() Relocalization()
 */
void Frame::ComputeBoW()
{
    //这个函数只有在当前帧的词袋是空的时候才会进行操作。
    if(mBowVec.empty())
    {
        //1、要写入词袋信息，将以OpenCV格式存储的描述子
        // mvpMapPoints为std::vector
        vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
        mpORBvocabulary->transform(vCurrentDesc, //当前的描述子vector
                mBowVec,  //输出，词袋向量
                mFeatVec, //输出，保存有特征点索引的特征，vector
                4);       //获取某一层的节点索引
                //@todo 这里的4表示从叶节点向前数的层数
    }//判断当前帧的词袋是否是空的
}

而ComputeBoW()函数内层，transform()起着核心作用，那么接下来，我们来一起看一下ORB-SLAM2源码中的transform()。

template<class TDescriptor, class F>
void TemplatedVocabulary<TDescriptor,F>::transform(
  const std::vector<TDescriptor>& features,
  BowVector &v, FeatureVector &fv, int levelsup) const
{
  v.clear();
  fv.clear();
  
  if(empty()) // safe for subclasses
  {
    return;
  }
  
  // normalize
  LNorm norm;

  bool must = m_scoring_object->mustNormalize(norm);

  typename vector<TDescriptor>::const_iterator fit;
  
  if(m_weighting == TF || m_weighting == TF_IDF)
  {

    unsigned int i_feature = 0;
    for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature)
    {
      WordId id;
      NodeId nid;
      WordValue w;
      // w is the idf value if TF_IDF, 1 if TF
      //其中id表示单词的id,w表示Value of a word,nid表示Id of nodes in the vocabulary treee
      //levelsup,表示节点层数
      transform(*fit, id, w, &nid, levelsup);
      
      if(w > 0) // not stopped
      {
        v.addWeight(id, w);
        fv.addFeature(nid, i_feature);
      }
    }
    
    if(!v.empty() && !must)
    {
      // unnecessary when normalizing
      const double nd = v.size();
      for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++)
        vit->second /= nd;
    }
  
  }
  else // IDF || BINARY
  {
    unsigned int i_feature = 0;
    for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature)
    {
      WordId id;
      NodeId nid;
      WordValue w;
      // w is idf if IDF, or 1 if BINARY

      transform(*fit, id, w, &nid, levelsup);
      
      if(w > 0) // not stopped
      {
        v.addIfNotExist(id, w);
        fv.addFeature(nid, i_feature);
      }
    }
  } // if m_weighting == ...
  
  if(must) v.normalize(norm);
}

经过测试，对于上述函数中，我们主要进入的函数为transform(*fit, id, w, &nid, levelsup)，具体实现如下：

template<class TDescriptor, class F>
void TemplatedVocabulary<TDescriptor,F>::transform(const TDescriptor &feature,
  WordId &word_id, WordValue &weight, NodeId *nid, int levelsup) const
{
  // propagate the feature down the tree
  vector<NodeId> nodes;
  typename vector<NodeId>::const_iterator nit;

  // level at which the node must be stored in nid, if given
  const int nid_level = m_L - levelsup;
  if(nid_level <= 0 && nid != NULL) *nid = 0; // root

  NodeId final_id = 0; // root
  int current_level = 0;

  do
  {
    ++current_level;
    nodes = m_nodes[final_id].children;
    final_id = nodes[0];
 
    double best_d = F::distance(feature, m_nodes[final_id].descriptor);

    for(nit = nodes.begin() + 1; nit != nodes.end(); ++nit)
    {
      NodeId id = *nit;
      double d = F::distance(feature, m_nodes[id].descriptor);
      if(d < best_d)
      {
        best_d = d;
        final_id = id;
      }
    }
    
    if(nid != NULL && current_level == nid_level)
      *nid = final_id;
    
  } while( !m_nodes[final_id].isLeaf() );

  // turn node id into word id
  word_id = m_nodes[final_id].word_id;
  weight = m_nodes[final_id].weight;
}

这里，简单说明下上述的几个变量格式：

1）对于vector<cv::Mat> vCurrentDesc，这个经过程序测试，其变量格式格式如下：

vCurrentDesc[0]://表示ORB描述子：32x8=256位

[49,158,107,235,182,167,111,255,86,235,255,230,115,227,176,96,127,238,22,188,187,189，109,191,254，239,167,192,189,202,240,185]

vCurrentDesc[1]:

[49,142,107,...,168]

vBowVec==

<6890,0.00112707>,<15246,0.0013725>,<18465,0.00143206> ...

vFeatVec==

<11:[308]>,<12:[143,320]>,<13:[719,721,827,828,830,832]>,<14:[107,216,321,475],<15:[433,434,441,836,837]>,<18:[831]>,<19:[92,144,181]>,...>

备注：这里的11表示节点，[308]表示图片中相似的特征序号。

当然，对于ORB-SLAM2中作者提供的源码，我们可以进一步优化加速其计算每帧图片词袋向量，包括移位操作、修改数据结构（减少内存）等方式，此处不作详细介绍了，欢迎到我们的学术圈探讨。

三 在重定位中找到与该帧相似的关键帧

ORB源码中实现此功能的主要函数为vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F)

/*
 * @brief 在闭环检测中找到与该关键帧可能闭环的关键帧
 * 1. 找出和当前帧具有公共单词的所有关键帧（不包括与当前帧相连的关键帧）
 * 2. 只和具有共同单词较多的关键帧进行相似度计算
 * 3. 将与关键帧相连（权值最高）的前十个关键帧归为一组，计算累计得分
 * 4. 只返回累计得分较高的组中分数最高的关键帧
 * @param pKF      需要闭环的关键帧
 * @param minScore 相似性分数最低要求
 * @return         可能闭环的关键帧
 * @see III-E Bags of Words Place Recognition
 */
vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F)
{
    // 提出所有与该pKF相连的KeyFrame，这些相连Keyframe都是局部相连，在闭环检测的时候将被剔除
    //Map,Set属于标准关联容器，使用了非常高效的平衡检索二叉树：红黑树，他的插入删除效率比其他序列容器高是因为不需要做内存拷贝和内存移动，而直接替换指向节点的指针即可。
    //Set和Vector的区别在于Set不包含重复的数据。Set和Map的区别在于Set只含有Key，而Map有一个Key和Key所对应的Value两个元素。
    list<KeyFrame*> lKFsSharingWords;// 用于保存可能与F形成回环的候选帧（只要有相同的word，且不属于局部相连帧）
    //这里的局部相连帧,就是和当前关键帧具有共视关系的关键帧

    // Search all keyframes that share a word with current keyframes
    // Discard keyframes connected to the query keyframe
    //. 步骤1：找出和当前帧具有公共单词的所有关键帧（不包括与当前帧链接的关键帧）
    {
        unique_lock<mutex> lock(mMutex);

        // words是检测图像是否匹配的枢纽，遍历该pKF的每一个word
        for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++)
        {
            // 提取所有包含该word的KeyFrame
            list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
            // 然后对这些关键帧展开遍历
            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
            {
                KeyFrame* pKFi=*lit;
                if(pKFi->mnRelocQuery!=F->mnId)// 与pKF局部链接的关键帧不进入闭环候选帧
                {
                    pKFi->mnRelocWords=0;
                    pKFi->mnRelocQuery=F->mnId;// pKFi标记为pKF的候选帧，之后直接跳过判断
                    lKFsSharingWords.push_back(pKFi);
                }
                pKFi->mnRelocWords++; // 记录pKFi与pKF具有相同word的个数
            }
        }
    }
    // 如果没有关键帧和这个关键帧具有相同的单词,那么就返回空
    if(lKFsSharingWords.empty())
        return vector<KeyFrame*>();

    // Only compare against those keyframes that share enough words
    // 步骤2：统计所有闭环候选帧中与当前帧F具有共同单词最多的单词数，并以此决定阈值
    int maxCommonWords=0;
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        if((*lit)->mnRelocWords>maxCommonWords)
            maxCommonWords=(*lit)->mnRelocWords;
    }

    int minCommonWords = maxCommonWords*0.8f;

    list<pair<float,KeyFrame*> > lScoreAndMatch;

    int nscores=0;

    // Compute similarity score.
    // 步骤3：遍历所有闭环候选帧，挑选出共有单词数大于阈值minCommonWords且单词匹配度大于minScore存入lScoreAndMatch
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        KeyFrame* pKFi = *lit;

        // 当前帧F只和具有共同单词较多的关键帧进行比较，需要大于minCommonWords
        if(pKFi->mnRelocWords>minCommonWords)
        {
            nscores++;// 这个变量后面没有用到
            float si = mpVoc->score(F->mBowVec,pKFi->mBowVec);
            pKFi->mRelocScore=si;
            lScoreAndMatch.push_back(make_pair(si,pKFi));
        }
    }

    if(lScoreAndMatch.empty())
        return vector<KeyFrame*>();

    list<pair<float,KeyFrame*> > lAccScoreAndMatch;
    float bestAccScore = 0;

    // Lets now accumulate score by covisibility
    // 步骤4：计算候选帧组得分，得到最高组得分bestAccScore，并以此决定阈值minScoreToRetain
    // 单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分
    // 具体而言：lScoreAndMatch中每一个KeyFrame都把与自己共视程度较高的帧归为一组，每一组会计算组得分并记录该组分数最高的KeyFrame，记录于lAccScoreAndMatch
    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
    {
        KeyFrame* pKFi = it->second;
        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);

        float bestScore = it->first; // 该组最高分数
        float accScore = bestScore;  // 该组累计得分
        KeyFrame* pBestKF = pKFi;    // 该组最高分数对应的关键帧
        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKF2 = *vit;
            if(pKF2->mnRelocQuery!=F->mnId)
                continue;

            accScore+=pKF2->mRelocScore;// 只有pKF2也在闭环候选帧中，才能贡献分数
            if(pKF2->mRelocScore>bestScore)// 统计得到组里分数最高的KeyFrame
            {
                pBestKF=pKF2;
                bestScore = pKF2->mRelocScore;
            }

        }
        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
        if(accScore>bestAccScore) // 记录所有组中组得分最高的组
            bestAccScore=accScore; // 得到所有组中最高的累计得分
    }

    // Return all those keyframes with a score higher than 0.75*bestScore
    // 步骤5：得到组得分大于阈值的，组内得分最高的关键帧
    float minScoreToRetain = 0.75f*bestAccScore;
    set<KeyFrame*> spAlreadyAddedKF;
    vector<KeyFrame*> vpRelocCandidates;
    vpRelocCandidates.reserve(lAccScoreAndMatch.size());
    for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
    {
        const float &si = it->first;
        // 只返回累计得分大于minScoreToRetain的组中分数最高的关键帧 0.75*bestScore
        if(si>minScoreToRetain)
        {
            KeyFrame* pKFi = it->second;
            if(!spAlreadyAddedKF.count(pKFi))// 判断该pKFi是否已经在队列中了
            {
                vpRelocCandidates.push_back(pKFi);
                spAlreadyAddedKF.insert(pKFi);
            }
        }
    }

    return vpRelocCandidates;
}

四 通过词包，对关键帧的特征点进行跟踪

对于函数ORBmatcher::SearchByBoW，ORB-SLAM2源码部分如下：

/**
 * @brief 通过词包，对关键帧的特征点进行跟踪
 *
 * 通过bow对pKF和F中的特征点进行快速匹配（不属于同一node的特征点直接跳过匹配） \n
 *
 * 对属于同一node的特征点通过描述子距离进行匹配 \n
 *
 * 根据匹配，用pKF中特征点对应的MapPoint更新F中特征点对应的MapPoints \n
 *
 * 每个特征点都对应一个MapPoint，因此pKF中每个特征点的MapPoint也就是F中对应点的MapPoint \n
 *
 * 通过距离阈值、比例阈值和角度投票进行剔除误匹配 //通过三个判断条件来判断误匹配
 *
 * @param  pKF               KeyFrame
 *
 * @param  F                 Current Frame
 *
 * @param  vpMapPointMatches F中MapPoints对应的匹配，NULL表示未匹配
 *
 * @return                   成功匹配的数量
 */
int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)
{
    const vector<MapPoint*> vpMapPointsKF = pKF->GetMapPointMatches();

    //和普通帧F特征点的索引一致
    vpMapPointMatches = vector<MapPoint*>(F.N,static_cast<MapPoint*>(NULL));

    const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec;

    int nmatches=0;

    //特征点角度旋转差统计用的直方图
    vector<int> rotHist[HISTO_LENGTH];
    for(int i=0;i<HISTO_LENGTH;i++)
        rotHist[i].reserve(500);

    //将0～360的数转换到0～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)
    // 将属于同一节点(特定层)的ORB特征进行匹配
    DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin();
    DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin();
    DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end();
    DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end();

    while(KFit != KFend && Fit != Fend)
    {
        //first 元素就是node
        if(KFit->first == Fit->first) //步骤1：分别取出属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点)
        {
            const vector<unsigned int> vIndicesKF = KFit->second;
            const vector<unsigned int> vIndicesF = Fit->second;

            // 步骤2：遍历KF中属于该node的特征点
            for(size_t iKF=0; iKF            {
                const unsigned int realIdxKF = vIndicesKF[iKF];

                MapPoint* pMP = vpMapPointsKF[realIdxKF]; // 取出KF中该特征对应的MapPoint

                if(!pMP)
                    continue;

                if(pMP->isBad())
                    continue;

                const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF); // 取出KF中该特征对应的描述子

                int bestDist1=256; // 最好的距离（最小距离）
                int bestIdxF =-1 ;
                int bestDist2=256; // 倒数第二好距离（倒数第二小距离）

                // 步骤3：遍历F中属于该node的特征点，找到了最佳匹配点
                for(size_t iF=0; iF                {
                    const unsigned int realIdxF = vIndicesF[iF];

                    if(vpMapPointMatches[realIdxF])// 表明这个点已经被匹配过了，不再匹配，加快速度
                        continue;

                    const cv::Mat &dF = F.mDescriptors.row(realIdxF); // 取出F中该特征对应的描述子

                    const int dist =  DescriptorDistance(dKF,dF); // 求描述子的距离

                    if(dist// dist < bestDist1 < bestDist2，更新bestDist1 bestDist2
                    {
                        bestDist2=bestDist1;
                        bestDist1=dist;
                        bestIdxF=realIdxF;
                    }
                    else if(dist// bestDist1 < dist < bestDist2，更新bestDist2
                    {
                        bestDist2=dist;
                    }
                }

                // 步骤4：根据阈值 和 角度投票剔除误匹配
                if(bestDist1<=TH_LOW) // 匹配距离（误差）小于阈值
                {
                    // trick!
                    // 最佳匹配比次佳匹配明显要好，那么最佳匹配才真正靠谱
                    if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
                    {
                        // 步骤5：更新特征点的MapPoint
                        vpMapPointMatches[bestIdxF]=pMP;

                        //这里的realIdxKF是当前遍历得到的关键帧的特征点id
                        const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF];

                        if(mbCheckOrientation)
                        {
                            // trick!
                            // angle：每个特征点在提取描述子时的旋转主方向角度，如果图像旋转了，这个角度将发生改变
                            // 所有的特征点的角度变化应该是一致的，通过直方图统计得到最准确的角度变化值
                            float rot = kp.angle-F.mvKeys[bestIdxF].angle;// 该特征点的角度变化值
                            if(rot<0.0)
                                rot+=360.0f;
                            int bin = round(rot*factor);// 将rot分配到bin组,四舍五入，其实就是离散到对应的直方图组中
                            if(bin==HISTO_LENGTH)
                                bin=0;
                            assert(bin>=0 && bin                            rotHist[bin].push_back(bestIdxF);// 直方图统计
                        }
                        nmatches++;
                    }
                }

            }

            KFit++;
            Fit++;
        }
        else if(KFit->first < Fit->first)
        {
            // 对齐
            KFit = vFeatVecKF.lower_bound(Fit->first);
        }
        else
        {
            //对齐
            Fit = F.mFeatVec.lower_bound(KFit->first);
        }
    }

    // 根据方向剔除误匹配的点
    if(mbCheckOrientation)
    {
        // index
        int ind1=-1;
        int ind2=-1;
        int ind3=-1;

        // 计算rotHist中最大的三个的index,如果出现了「一枝独秀」的情况，那么说明次优或者第三优的也不是足够好，直接返回-1.
        ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);

        for(int i=0; i<HISTO_LENGTH; i++)
        {
            // 如果特征点的旋转角度变化量属于这三个组，则保留
            if(i==ind1 || i==ind2 || i==ind3)
                continue;

            // 将除了ind1 ind2 ind3以外的匹配点去掉
            for(size_t j=0, jend=rotHist[i].size(); j            {
                vpMapPointMatches[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
                nmatches--;
            }
        }
    }

    return nmatches;
}

思考：对于本函数中，阈值TH_LOW以及mfNNratio如果对于不同的字典，此处是否应该做出调整？

五 跋

最后，ORB-SLAM2系统中对于字典的使用，还有一些细节，比如反向查找等，以及如何训练更小层数的字典，以使得降低系统内存，这些问题，限于篇幅，小凡便不再作过多介绍，欢迎大家多多交流～

上述内容，如有侵犯版权，请联系作者，会自行删文。

交流群

欢迎加入我们公众号读者群一起和同行交流，目前有3D视觉、深度学习、激光SLAM、VSLAM、三维重建、点云后处理、图像处理、手眼标定、自动驾驶、位姿估计等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。

▲长按加群

▲长按关注我们