ORB-SLAM2 ---- Frame::ComputeBoW函数（TrackReferenceKeyFrame调用版）

目录

1.函数作用

2.什么是BowVec和FeatVec

3.代码 

3.1 Frame::ComputeBoW解释

3.2  transform主函数：将一幅图像所有的特征点转化为BowVector和FeatureVector

3.3 transform：将描述子转化为Word id， Word weight，节点所属的父节点id

3.4 BowVector::addWeight解析 

3.5  FeatureVector::addFeature解析 

---

1.函数作用

        计算当前帧特征点对应的词袋Bow，主要是mBowVec和 mFeatVec。

2.什么是BowVec和FeatVec

        ORB-SLAM2代码中使用的SearchByBoW用于关键帧跟踪、重定位、闭环检测SIM3计算)，以及局部地图里的SearchForTriangulation，内部实现主要是利用了BoW中的FeatureVector来加速特征匹配。
        那么FeatureVector是如何加快匹配的呢？

        我们对于每一个关键帧都会计算BowVec和FeatVec：

        对新来的一帧图像进行ORB特征提取，得到一定数量（一般几百个）的特征点，描述子维度和 vocabulary tree中的一致。vocabulary tree是官方训练出的词典。

        对于每个特征点的描述子，从离线创建好的vocabulary tree中开始找自己的位置，从根节点开始，用该描述子和每个节点的描述子计算汉明距离，选择汉明距离最小的作为自己所在的节点，一直遍历到叶子节点。如下图：

         即我们计算出一个特征点的描述子之后，我们在计算特征点和level1各个结点的汉明距离，然后取汉明距离最小的那个节点一直重复此过程向下匹配...直到匹配到最后一层找到与我们待匹配节点汉明距离最近的节点作为特征点匹配结果。

        那么如何加速匹配呢？这就要看BowVec和FeatVec的定义了：

std::map<WordId, WordValue>

        这就是BowVec的定义，其中 WordId 和 WordValue 表示Word在所有叶子中距离最近的叶子的id 和权重。

void BowVector::addWeight(WordId id, WordValue v)
{
// 返回指向大于等于id的第一个值的位置
BowVector::iterator vit = this->lower_bound(id);
// http://www.cplusplus.com/reference/map/map/key_comp/
if(vit != this->end() && !(this->key_comp()(id, vit->first)))
{
// 如果id = vit->first, 说明是同一个Word，权重更新
vit->second += v;
}
else
{
// 如果该Word id不在BowVector中，新添加进来
this->insert(vit, BowVector::value_type(id, v));
}
}

std::map<NodeId, std::vector<unsigned int> >

        这就是BowVec的定义。其中Nodeld 并不是该叶子节点直接的父节点id，而是距离叶子节点深度为level up对应的node的id，对应上面vocabulary tree图示里的Word's node id。为什么不直接设置为父节点？因为后面搜索该Word的匹配点的时候是在和它具有同样node id下面所有子节点中的Word进行匹配，搜索区域见图示中的Word's search region。所以搜索范围大小是根据level up来确定的，level up值越大，搜索范围广，速度越慢；level up值越小，搜索范围越小，速度越快，但能够匹配的特征就越少。这样就加速了匹配过程。

        因此，我们在匹配过程中，只要从Word's node id向下进行描述子匹配就能加速对特征点的匹配！

3.代码 

3.1 Frame::ComputeBoW解释

/**
 * @brief 计算当前帧特征点对应的词袋Bow，主要是mBowVec 和 mFeatVec
 * 
 */
void Frame::ComputeBoW()
{
	
    // 判断是否以前已经计算过了，计算过了就跳过
    if(mBowVec.empty())
    {
		// 将描述子mDescriptors转换为DBOW要求的输入格式
        vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
		// 将特征点的描述子转换成词袋向量mBowVec以及特征向量mFeatVec
        mpORBvocabulary->transform(vCurrentDesc,	//当前的描述子vector
								   mBowVec,			//输出，词袋向量，记录的是单词的id及其对应权重TF-IDF值
								   mFeatVec,		//输出，记录node id及其对应的图像 feature对应的索引
								   4);				//4表示从叶节点向前数的层数
    }
}

        我们对于传进来的一帧，先判断它的BowVec和FeatVec是否被计算过，如果没有被计算过，则先将这一帧中的描述子转换成DBoW要求的格式，因为我们要调用transform函数计算该帧的BowVec和FeatVec向量。

        我们传入的参数是当前帧特征点的描述子vCurrentDesc，以及我们上文说的Word's node id，ORBSLAM的距离叶子的深度默认是4。

3.2  transform主函数：将一幅图像所有的特征点转化为BowVector和FeatureVector

// --------------------------------------------------------------------------
/**
 * @brief 将一幅图像所有的特征点转化为BowVector和FeatureVector
 * 
 * @tparam TDescriptor 
 * @tparam F 
 * @param[in] features      图像中所有的描述子
 * @param[in & out] v       BowVector
 * @param[in & out] fv      FeatureVector
 * @param[in] levelsup      距离叶子的深度
 */
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 
  // 根据选择的评分类型来确定是否需要将BowVector 归一化
  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;        // 叶子节点的Word id
      NodeId nid;       // FeatureVector 里的NodeId，用于加速搜索
      WordValue w;      // 叶子节点Word对应的权重

      //  将当前描述子转化为Word id， Word weight，节点所属的父节点id（这里的父节点不是叶子的上一层，它距离叶子深度为levelsup）
      // w is the idf value if TF_IDF, 1 if TF 
      transform(*fit, id, w, &nid, levelsup);
      
      if(w > 0) // not stopped
      { 
        // 如果Word 权重大于0，将其添加到BowVector 和 FeatureVector
        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);
}

        我们先初始化该帧的BoWVec和FeatVec，即将这两个向量进行clear操作。接着根据评分类型来确定是否需要将BowVector归一化。

        遍历该帧中的所有特征点，特征点在该帧的索引我们用i_feature存放，通过transform函数（见本文3.3节）得到了ORB词典中与该帧的一个特征点的描述子feature匹配最优的描述子，它在ORB词典中的权重为w、它在ORB词典中的索引位置为id ，它的父节点（距离索引位置的层数为levelsup）为nid。

        如果权重大于0，将其添加到BowVector 和 FeatureVector。（见本文3.4节）

3.3 transform：将描述子转化为Word id， Word weight，节点所属的父节点id

/**
 * @brief 将描述子转化为Word id， Word weight，节点所属的父节点id（这里的父节点不是叶子的上一层，它距离叶子深度为levelsup）
 * 
 * @tparam TDescriptor            
 * @tparam F 
 * @param[in] feature                 特征描述子
 * @param[in & out] word_id           Word id
 * @param[in & out] weight            Word 权重
 * @param[in & out] nid               记录当前描述子转化为Word后所属的 node id，它距离叶子深度为levelsup
 * @param[in] 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
  // m_L: depth levels, m_L = 6 in ORB-SLAM2
  // nid_level 当前特征点转化为的Word 所属的 node id，方便索引
  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;
    // 取出当前节点所有子节点的id
    nodes = m_nodes[final_id].children;
    // 取子节点中第1个的id，用于后面距离比较的初始值
    final_id = nodes[0];

    // 取当前节点第一个子节点的描述子距离初始化最佳（小）距离
    double best_d = F::distance(feature, m_nodes[final_id].descriptor);
    // 遍历nodes中所有的描述子，找到最小距离对应的描述子
    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;
      }
    }
    
    // 记录当前描述子转化为Word后所属的 node id，它距离叶子深度为levelsup
    if(nid != NULL && current_level == nid_level)
      * nid = final_id;
    
  } while( !m_nodes[final_id].isLeaf() );

  // turn node id into word id
  // 取出 vocabulary tree中node距离当前feature 描述子距离最小的那个node的 Word id 和 weight
  word_id = m_nodes[final_id].word_id;
  weight = m_nodes[final_id].weight;
}

        对于某一特征点的描述子，通过此函数将描述子转化为Word id，Word weight，节点所属的父节点id。

        定义nid_level为当前特征点转化为的 Word 所属的 node id，方便索引：

         如果传入的levelsup参数为3，ORBSLAM2中默认的m_L为6，那么我们以后要是简化特征点匹配的话就是从第三行之后匹配。然后检查nid_level 这个值是否小于0，如果小于0或者我们指定的父节点（以后方便匹配的索引）不为空（可能存在内存泄漏），那么我们将方便索引的节点层数nid_level设为0。

        完成初始化之后，我们开始进入循环：

        我们首先将搜索树的深度+1（初始的时候初始化为0），即向我们之前所说的从树的顶层到底层依次计算描述子距离，从最底层取到汉明距离最小的作为今后加速匹配的近似描述子。在更新完树的深度后，我们取得下一层待匹配的描述子在ORB词典中的字节点IDnodes ，当前节点第一个子节点的描述子距离初始化最佳（小）距离best_d 和最优节点索引final_id ，然后我们遍历这层的所有节点的描述子距离依次和best_d比较，不断更新这个索引最终找到这层最优的节点final_id ，如果这层是父节点的层数则更新nid ，作为我们加速匹配的父节点，然后我们一层层匹配，直到选出最后一层最优的best_d和final_id。

        这个函数最后得到了ORB词典中与该帧的一个特征点的描述子feature匹配最优的描述子，它在ORB词典中的权重为weight、它在ORB词典中的索引位置为word_id ，它的父节点（距离索引位置的层数为levelsup）为nid。

3.4 BowVector::addWeight解析 

/**
 * @brief 更新BowVector中的单词权重
 * 
 * @param[in] id    单词的ID
 * @param[in] v     单词的权重
 */
void BowVector::addWeight(WordId id, WordValue v)
{
  // 返回指向大于等于id的第一个值的位置
  BowVector::iterator vit = this->lower_bound(id);
  
  
  if(vit != this->end() && !(this->key_comp()(id, vit->first)))
  {
    // 如果id = vit->first, 说明是同一个Word，权重更新 
    vit->second += v;
  }
  else
  {
    // 如果该Word id不在BowVector中，新添加进来
    this->insert(vit, BowVector::value_type(id, v));
  }
}

        我们更新该帧中的BowVec向量。

        v.addIfNotExist(id, w); //id是叶子节点在ORB词典中的索引，w是id节点的权重。BowVec是按叶子节点的id递升的，当新到了一个id时，如果它已经在BowVec中，则仅仅将权值加上v，如果不存在这个条目，则新创建一个<id,value>的对组。

        比如，我们一帧中BowVec如下：

<2,64><5,75><8,0><16,61><18,45>

        我们新到一个一个<2,32>那么我们现在的BowVec为：

<2,96><5,75><8,0><16,61><18,45>

        比如我们新到一个<9,7>的那么我们现在的BowVec为：

<2,96><5,75><8,0><9,7><16,61><18,45>

3.5  FeatureVector::addFeature解析 

/**
 * @brief 把node id下所有的特征点的索引值归属到它的向量里
 * 
 * @param[in] id              节点ID，内部包含很多单词
 * @param[in] i_feature       特征点在图像中的索引
 */
void FeatureVector::addFeature(NodeId id, unsigned int i_feature)
{
  // 返回指向大于等于id的第一个值的位置
  FeatureVector::iterator vit = this->lower_bound(id);
  // 将同样node id下的特征点索引值放在一个向量里
  if(vit != this->end() && vit->first == id)
  {
    // 如果这个node id已经创建，可以直接插入特征点索引
    vit->second.push_back(i_feature);
  }
  else
  {
    // 如果这个node id还未创建，创建后再插入特征点索引
    vit = this->insert(vit, FeatureVector::value_type(id, std::vector<unsigned int>() ));
    vit->second.push_back(i_feature);
  }
}

        这里的nodeID是父节点（距离根节点深度为levelsup的结点），i_feature是描述子的索引，即新到一个描述子的索引，将其归类到对应的父节点里面。

        比如<6,<4,6,7,8>>的含义就是这个帧的第4，6，7，8个特征点在ORB词典中的索引为6的节点下。