基于词袋模型的回环检测实现(c++)

1. 相关链接

源码地址: https://github.com/dorian3d/DLoopDetector
源码原理: https://blog.csdn.net/weixin_37835423/article/details/112890634

2. demo测试

2.1 源码安装DBow2库

git clone https://github.com/dorian3d/DBoW2.git
cd DBoW2
mkdir build & cd build
cmake ..
make
sudo make install

2.2 下载编译DLoopDetector

git clone https://github.com/dorian3d/DLoopDetector.git
cd DLoopDetector
mkdir build & cd build
cmake ..
make

2.3 下载测试数据集
数据集地址: https://drive.google.com/uc?export=download&id=1MpZwPjXDAUxKfSTpeCjG0PAUpaeWuo7D
然后解压到目录 “DLoopDetector/build/” 下．

2.4 运行demo

cd DLoopDetector/build
./demo_brief

2.5 结果

从结果中可以看出，重复走过的地方都有效的检测出了回环，可验证该算法进行回环检测的有效性．

3. 源码解析

注: 下面注释中的"博客"指的都是我的上一篇博客

template
bool TemplatedLoopDetector::detectLoop(
  const std::vector &keys, 
  const std::vector &descriptors,
  DetectionResult &match)
{
  EntryId entry_id = m_database->size();
  match.query = entry_id;
  
  BowVector bowvec;
  FeatureVector featvec;
  
  // 通过词袋模型转换描述子为词袋向量，并更新Direct index查找表，加速后面几何一致性验证过程
  if(m_params.geom_check == GEOM_DI)
    m_database->getVocabulary()->transform(descriptors, bowvec, featvec,
      m_params.di_levels);
  else
    m_database->getVocabulary()->transform(descriptors, bowvec);

  if((int)entry_id <= m_params.dislocal)
  {
    // only add the entry to the database and finish
    m_database->add(bowvec, featvec);
    match.status = CLOSE_MATCHES_ONLY;
  }
  else
  {
    int max_id = (int)entry_id - m_params.dislocal;
    
    // 对应博客5.1节中的数据库候选者检索，检索出可能的候选者
    QueryResults qret;
    m_database->query(bowvec, qret, m_params.max_db_results, max_id);

    // update database
    m_database->add(bowvec, featvec); // returns entry_id
    
    if(!qret.empty())
    {
      // factor to compute normalized similarity score, if necessary
      double ns_factor = 1.0;
      
      // 对应博客5.1节 计算期待的最好得分s(v_t, v_t_dt)
      if(m_params.use_nss)
      {
        ns_factor = m_database->getVocabulary()->score(bowvec, m_last_bowvec);
      }
      
      if(!m_params.use_nss || ns_factor >= m_params.min_nss_factor)
      {
        // 对应博客5.1节最后通过阈值alpha移除归一化匹配得分较小的候选者
        removeLowScores(qret, m_params.alpha * ns_factor);
        
        if(!qret.empty())
        {
          // the best candidate is the one with highest score by now
          match.match = qret[0].Id;
          
          // 对应博客5.2节中对候选者进行分组
          // compute islands
          vector islands;
          computeIslands(qret, islands); 
          // this modifies qret and changes the score order
          
          // get best island
          if(!islands.empty())
          {
            // 对应博客5.2节中获取匹配组(组内候选者归一化得分之和最大组)
            const tIsland& island = 
              *std::max_element(islands.begin(), islands.end());
            
            // 对应博客5.3节中进行时间一致性检查
            // check temporal consistency of this island
            updateTemporalWindow(island, entry_id);
            
            // 对应博客5.3节中，在进行完时间一致性检查后，获取匹配组内的最优候选者(归一化得分最高者)作为闭环候选者
            // get the best candidate (maybe match)
            match.match = island.best_entry;
            
            if(getConsistentEntries() > m_params.k)
            {
              // candidate loop detected
              // check geometry
              bool detection;

              // 对应博客5.4节，进行几何一致性检测
              if(m_params.geom_check == GEOM_DI)
              {
                // all the DI stuff is implicit in the database
                detection = isGeometricallyConsistent_DI(island.best_entry, 
                  keys, descriptors, featvec);
              }
              else if(m_params.geom_check == GEOM_FLANN)
              {
                cv::FlannBasedMatcher flann_structure;
                getFlannStructure(descriptors, flann_structure);
                            
                detection = isGeometricallyConsistent_Flann(island.best_entry, 
                  keys, descriptors, flann_structure);
              }
              else if(m_params.geom_check == GEOM_EXHAUSTIVE)
              { 
                detection = isGeometricallyConsistent_Exhaustive(
                  m_image_keys[island.best_entry], 
                  m_image_descriptors[island.best_entry],
                  keys, descriptors);            
              }
              else // GEOM_NONE, accept the match
              {
                detection = true;
              }
              
              // 更新回环检测状态
              if(detection)
              {
                match.status = LOOP_DETECTED;
              }
              else
              {
                match.status = NO_GEOMETRICAL_CONSISTENCY;
              }
              
            } // if enough temporal matches
            else
            {
              match.status = NO_TEMPORAL_CONSISTENCY;
            }
            
          } // if there is some island
          else
          {
            match.status = NO_GROUPS;
          }
        } // if !qret empty after removing low scores
        else
        {
          match.status = LOW_SCORES;
        }
      } // if (ns_factor > min normal score)
      else
      {
        match.status = LOW_NSS_FACTOR;
      }
    } // if(!qret.empty())
    else
    {
      match.status = NO_DB_RESULTS;
    }
  }

  // update record
  // m_image_keys and m_image_descriptors have the same length
  if(m_image_keys.size() == entry_id)
  {
    m_image_keys.push_back(keys);
    m_image_descriptors.push_back(descriptors);
  }
  else
  {
    m_image_keys[entry_id] = keys;
    m_image_descriptors[entry_id] = descriptors;
  }
  
  // store this bowvec if we are going to use it in next iteratons
  if(m_params.use_nss && (int)entry_id + 1 > m_params.dislocal)
  {
    m_last_bowvec = bowvec;
  }

  return match.detection();
}