VINS-Mono中的DBoW2关键代码注释

posegraph.loadVocabulary(vocabulary_file);

这句代码包含的信息量巨大，其实现的功能就是将support_files文件夹下的brief_k10L6.bin文件构造为词袋模型的词典。这个词典的内容以树形结构存储，大意可以用下图表示，节点存储的权重和描述子是字典的核心内容。

转到pose_graph.cpp文件中，

void PoseGraph::loadVocabulary(std::string voc_path)
{
    //构造词典，核心代码在这里
    voc = new BriefVocabulary(voc_path);
    //利用构造的词典，初始化图像数据库，它在计算图像相似度时发挥重要作用
    db.setVocabulary(*voc, false, 0);
}

/// BRIEF Vocabulary
//BriefVocabulary类其实是这个类的另外一个名字
typedef DBoW2::TemplatedVocabulary<DBoW2::FBrief::TDescriptor, DBoW2::FBrief> 
  BriefVocabulary;

/// BRIEF Database
//BriefDatabase类其实是这个类的另外一个名字
typedef DBoW2::TemplatedDatabase<DBoW2::FBrief::TDescriptor, DBoW2::FBrief> 
  BriefDatabase;

上面的代码在DBoW文件夹下的DBoW2.h文件中，下面的代码在TemplatedVocabulary.h文件中，

template<class TDescriptor, class F>
TemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary
  (const std::string &filename): m_scoring_object(NULL)
{
    //从brief_k10L6.bin文件中加载词典
    loadBin(filename);
}

//节点m_nodes包括叶子m_words，即叶子一定是节点,节点不一定是叶子。叶子就是单词
template<class TDescriptor, class F>
void TemplatedVocabulary<TDescriptor,F>::loadBin(const std::string &filename) { 

  m_words.clear();//单词（树形结构的叶子）
  m_nodes.clear();//树形结构的各层节点，包括叶子
  //printf("loop load bin\n");
  std::ifstream ifStream(filename);//得到文件流
  VINSLoop::Vocabulary voc;
  voc.deserialize(ifStream);//从文件流中得到voc，这里不懂具体怎么实现的
  ifStream.close();
  
  m_k = voc.k;//树形结构各节点的子节点数的最大值
  m_L = voc.L;//树形结构的深度,即层数
  m_scoring = (ScoringType)voc.scoringType;//评分的标准选哪一个
  m_weighting = (WeightingType)voc.weightingType;//权重的标准选哪一个
 
  createScoringObject(); //根据评分标准得到m_scoring_object
  //节点数重置
  m_nodes.resize(voc.nNodes + 1); // +1 to include root
  m_nodes[0].id = 0;//根节点id

  for(unsigned int i = 0; i < voc.nNodes; ++i)
  {
    NodeId nid = voc.nodes[i].nodeId;//节点id
    NodeId pid = voc.nodes[i].parentId;//节点的父id
    WordValue weight = voc.nodes[i].weight;//节点权重
    //构造节点m_nodes 
    m_nodes[nid].id = nid;
    m_nodes[nid].parent = pid;
    m_nodes[nid].weight = weight;
    m_nodes[pid].children.push_back(nid);
      
    // Sorry to break template here
    //节点描述子
    m_nodes[nid].descriptor = boost::dynamic_bitset<>(voc.nodes[i].descriptor, voc.nodes[i].descriptor + 4);
    //这段代码可忽略
    if (i < 5) {
      std::string test;
      boost::to_string(m_nodes[nid].descriptor, test);
      //cout << "descriptor[" << i << "] = " << test << endl;
    }
  }
  
  // words
  m_words.resize(voc.nWords);//叶子数重置

  for(unsigned int i = 0; i < voc.nWords; ++i)
  {
    NodeId wid = (int)voc.words[i].wordId;//叶子id
    NodeId nid = (int)voc.words[i].nodeId;//叶子作为节点身份的id
    //节点m_nodes的参数补充， 叶子m_words的构造
    m_nodes[nid].word_id = wid;
    m_words[wid] = &m_nodes[nid];
  }
}

词典构造工作完成。

下面的代码在TemplatedDatabase.h文件中

template<class TDescriptor, class F>
template<class T>
inline void TemplatedDatabase<TDescriptor, F>::setVocabulary
  (const T& voc, bool use_di, int di_levels)
{
  m_use_di = use_di;//false
  m_dilevels = di_levels;//0
  delete m_voc;
  m_voc = new T(voc);//词袋模型的词典
  clear();//一些变量的初始化
}

template<class TDescriptor, class F>
inline void TemplatedDatabase<TDescriptor, F>::clear()
{
  // resize vectors
  m_ifile.resize(0);//这个变量很重要，要特别注意
  m_ifile.resize(m_voc->size());
  m_dfile.resize(0);
  m_dBowfile.resize(0);
  m_nentries = 0;
}

返回pose_graph.cpp文件中，

//添加关键帧的描述子到图像数据库
db.add(keyframe->brief_descriptors);

下面代码在TemplatedDatabase.h文件中

template<class TDescriptor, class F>
EntryId TemplatedDatabase<TDescriptor, F>::add(
  const std::vector<TDescriptor> &features,
  BowVector *bowvec, FeatureVector *fvec)
{
  BowVector aux;
  BowVector& v = (bowvec ? *bowvec : aux);
  
  ......
  else//执行这里的代码
  {
    //这段代码的功能就是把描述子features转为BowVector(叶子id,叶子在此帧的权重)的形式
    m_voc->transform(features, v); // with features
    return add(v);
  }
}

转到TemplatedVocabulary.h文件中

template<class TDescriptor, class F>
void TemplatedVocabulary<TDescriptor,F>::transform(
  const std::vector<TDescriptor>& features, BowVector &v) const
{
  v.clear();//先清空
  
  if(empty())
  {
    return;
  }

  // normalize 
  LNorm norm;
  bool must = m_scoring_object->mustNormalize(norm);

  typename std::vector<TDescriptor>::const_iterator fit;

  if(m_weighting == TF || m_weighting == TF_IDF)
  {
    for(fit = features.begin(); fit < features.end(); ++fit)
    {
      WordId id;
      WordValue w; 
      // w is the idf value if TF_IDF, 1 if TF
      //在词典中得到，与此描述子最为匹配的叶子的id及权重
      transform(*fit, id, w);
      
      // not stopped
      // 累积该叶节点的idf权重，v(id).weight += w
	  // 最后v(id).weight实际上等于M*idf，M为插入该叶节点的特征描述的个数
      if(w > 0) v.addWeight(id, w);
    }
    //这段不执行
    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
  {
    for(fit = features.begin(); fit < features.end(); ++fit)
    {
      WordId id;
      WordValue w;
      // w is idf if IDF, or 1 if BINARY
      
      transform(*fit, id, w);
      
      // not stopped
      if(w > 0) v.addIfNotExist(id, w);
      
    } // if add_features
  } // if m_weighting == ...
  
  if(must) v.normalize(norm);
}

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
  std::vector<NodeId> nodes;//表示的是某个节点的所有子节点
  typename std::vector<NodeId>::const_iterator nit;//节点迭代器

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

  NodeId final_id = 0; // root
  int current_level = 0;
 // 当前描述子feature逐层匹配，直到叶子层
  do
  {
    ++current_level;
    nodes = m_nodes[final_id].children;
    final_id = nodes[0];
    // 计算该描述子与本层节点描述子的距离，选取距离最小的节点，记录下它的id
    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（所有叶子集合内的编号）和权重
  word_id = m_nodes[final_id].word_id;
  weight = m_nodes[final_id].weight;
}

回到TemplatedDatabase.h文件中，

template<class TDescriptor, class F>
EntryId TemplatedDatabase<TDescriptor, F>::add(const BowVector &v,
  const FeatureVector &fv)
{
  EntryId entry_id = m_nentries++;//数据库中图像帧id

  BowVector::const_iterator vit;
  std::vector<unsigned int>::const_iterator iit;

  if(m_use_di)//false
  {
    ......
  }
  
  // update inverted file
  for(vit = v.begin(); vit != v.end(); ++vit)
  {
    const WordId& word_id = vit->first;//叶子id
    const WordValue& word_weight = vit->second;//叶子在该图像中的权重
    //下面两句代码的意思: 叶子word_id在图像entry_id中的权重为word_weight
    IFRow& ifrow = m_ifile[word_id];//获取m_ifile[word_id]的地址
    ifrow.push_back(IFPair(entry_id, word_weight));//添加记录
  }
  
  return entry_id;
}

转到pose_graph.cpp文件中，

//在图像数据库中查找与当前帧相似度最高的4帧图像
db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);

转到TemplatedDatabase.h文件中，

template<class TDescriptor, class F>
void TemplatedDatabase<TDescriptor, F>::query(
  const std::vector<TDescriptor> &features,
  QueryResults &ret, int max_results, int max_id) const
{
  BowVector vec;
  //将当前帧的描述子转为BowVector向量
  m_voc->transform(features, vec);
  //利用当前帧的BowVector向量，在图像数据库中查找与当前帧相似度最高的几帧图像
  query(vec, ret, max_results, max_id);
}

template<class TDescriptor, class F>
void TemplatedDatabase<TDescriptor, F>::query(
  const BowVector &vec, 
  QueryResults &ret, int max_results, int max_id) const
{
  ret.resize(0);
  
  switch(m_voc->getScoringType())
  {
    case L1_NORM:
      queryL1(vec, ret, max_results, max_id);
      break; 
      ......
      break;
  }
}

以queryL1()函数为例，

template<class TDescriptor, class F>
void TemplatedDatabase<TDescriptor, F>::queryL1(const BowVector &vec, 
  QueryResults &ret, int max_results, int max_id) const
{
  BowVector::const_iterator vit;
  typename IFRow::const_iterator rit;
  //pairs中存储着当前图像与数据库中图像的相似度value  
  std::map<EntryId, double> pairs;
  std::map<EntryId, double>::iterator pit;
  //遍历当前图像的所有单词
  for(vit = vec.begin(); vit != vec.end(); ++vit)
  {
    const WordId word_id = vit->first;//叶子(单词)id
    const WordValue& qvalue = vit->second;//单词word_id在当前图像中的权值
    // 注意单词word_id在数据库中各个图像的权值是不同的    
    const IFRow& row = m_ifile[word_id];
    
    // IFRows are sorted in ascending entry_id order
    // 计算当前图像与数据库中各个图像entry_id的相似度value
    for(rit = row.begin(); rit != row.end(); ++rit)
    {
      const EntryId entry_id = rit->entry_id;//图像id
      const WordValue& dvalue = rit->word_weight;//单词word_id在图像entry_id中的权值
      //if判断的第2和第3个条件是什么意思?
      if((int)entry_id < max_id || max_id == -1 || (int)entry_id == m_nentries - 1)
      {
        double value = fabs(qvalue - dvalue) - fabs(qvalue) - fabs(dvalue);
        
        pit = pairs.lower_bound(entry_id);
        if(pit != pairs.end() && !(pairs.key_comp()(entry_id, pit->first)))
        {
          pit->second += value;
        }
        else
        {
          pairs.insert(pit, 
            std::map<EntryId, double>::value_type(entry_id, value));
        }
      }
      
    } // for each inverted row
  } // for each query word
	
  // move to vector
  ret.reserve(pairs.size());
  for(pit = pairs.begin(); pit != pairs.end(); ++pit)
  {
    ret.push_back(Result(pit->first, pit->second));
  }
	
  // 升序排列，前面计算的value是负值，所以value值越小相似度越高
  std::sort(ret.begin(), ret.end());

  if(max_results > 0 && (int)ret.size() > max_results)
    ret.resize(max_results);//取最相似的max_results个结果
  QueryResults::iterator qit;
  for(qit = ret.begin(); qit != ret.end(); qit++) 
    qit->Score = -qit->Score/2.0;
}