ROS-3DSLAM（十三）lvi-sam源代码阅读11 —— visual_loop阅读4 + ORB-SLAM初探

2021SC@SDUSC（十三）lvi-sam源代码阅读11 —— visual_loop阅读4 + ORB-SLAM初探visual_loopDBoW文件夹DBoW内的内容主要跟词袋模型的部分相关，是回环检测中用于匹配回环点的重要步骤。项目中DBoW节点，应用的一个SLAM的模型——ORB-SLAM。所以决定先学习一下ORB-SLAM。对本项目中该部分的分析，考虑后决定按照算法流程来进行分析。楼

handsome_hhh

3438人浏览 · 2021-12-12 18:49:33

handsome_hhh · 2021-12-12 18:49:33 发布

2021SC@SDUSC

（十三）lvi-sam源代码阅读11 —— visual_loop阅读4 + ORB-SLAM初探

visual_loop

DBoW文件夹

DBoW内的内容主要跟词袋模型的部分相关，是回环检测中用于匹配回环点的重要步骤。

项目中DBoW节点，应用的一个SLAM的模型——ORB-SLAM。所以决定先学习一下ORB-SLAM。

对本项目中该部分的分析，考虑后决定按照算法流程来进行分析。
楼下是本次博客中分析的内容：

在这里插入图片描述

在loop_detection_node.cpp中，有读取一个二进制文件的操作：

// initialize vocabulary
/*
    vocabulary_file: "/config/brief_k10L6.bin"
    brief_pattern_file: "/config/brief_pattern.yml"
*/
string vocabulary_file;
fsSettings["vocabulary_file"] >> vocabulary_file;  
vocabulary_file = pkg_path + vocabulary_file;

该二进制文件是ORB-SLAM文件作者提供的词典库，在ORB-SLAM系统的初始化时将该词典库加载到系统之中。ORBSLAM默认读取txt格式的ORBvoc.txt字典，速度非常慢，而读取二进制的ORBvoc.bin可以大大加快此过程。

加载字典集：

loopDetector.loadVocabulary(vocabulary_file);

LoopDetector

回到LoopDetector类，此时对类中函数loadVocabulary的用途已经知道了–在系统中加载预置的字典集。

loadVocabulary函数：

void LoopDetector::loadVocabulary(std::string voc_path) //字典集的路径
{
    voc = new BriefVocabulary(voc_path);
    db.setVocabulary(*voc, false, 0);
}

BriefVocabulary：在DBoW2.h中定义：

/// BRIEF Vocabulary
typedef DBoW2::TemplatedVocabulary<DBoW2::FBrief::TDescriptor, DBoW2::FBrief> 
  BriefVocabulary;

为类TemplatedVocabulary申请了别名BriefVocabulary

TemplatedVocabulary

类中的模板：

/// @param TDescriptor class of descriptor
/// @param F class of descriptor functions
template<class TDescriptor, class F>
class TemplatedVocabulary

成员变量：

//成员变量
	//考虑BoW中的k-means算法
/// Branching factor
int m_k;
/// Depth levels 
int m_L;
/// Weighting method
WeightingType m_weighting;
/// Scoring method
ScoringType m_scoring;
/// Object for computing scores
GeneralScoring* m_scoring_object;
/// Tree nodes
std::vector<Node> m_nodes;
/// Words of the vocabulary (tree leaves)
/// this condition holds: m_words[wid]->word_id == wid
std::vector<Node*> m_words;

Node：（get到一个结构体struct的新用法）

//类中定义的一个结构体类型
  /// Tree node
  struct Node 
  {
    /// Node id
    NodeId id;
    /// Weight if the node is a word 权重
    WordValue weight;
    /// Children 
    std::vector<NodeId> children;
    /// Parent node (undefined in case of root)
    NodeId parent;
    /// Node descriptor
    TDescriptor descriptor;

    /// Word id if the node is a word
    WordId word_id;

    /**
     * Empty constructor
     */
    Node(): id(0), weight(0), parent(0), word_id(0){}
    
    /**
     * Constructor
     * @param _id node id
     */
    Node(NodeId _id): id(_id), weight(0), parent(0), word_id(0){}

    /**
     * Returns whether the node is a leaf node
     * @return true iff the node is a leaf
     */
    inline bool isLeaf() const { return children.empty(); }
  };

NodeId（在BowVector中定义）：

/// Id of nodes in the vocabulary treee
typedef unsigned int NodeId;

WordValue（在BowVector中定义）：

/// Value of a word
typedef double WordValue;

WeightingTpye和ScoringType（在BowVector中定义）：

/// Weighting type
enum WeightingType
{ //TF_IDF：译频率 - 逆文档频率
  TF_IDF,
  TF,
  IDF,
  BINARY
};

/// Scoring type
enum ScoringType
{
  L1_NORM,
  L2_NORM,
  CHI_SQUARE,
  KL,
  BHATTACHARYYA,
  DOT_PRODUCT
};

构造函数（参数为字典集的路径）：

//构造函数
/**
* Creates the vocabulary by loading a file
* @param filename
*/
TemplatedVocabulary(const std::string &filename);
{loadBin(filename);/*函数中的内容*/}

//loadBin 1512

loadBin：

// Added by VINS [[[
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;
  /*
  	VINSLoop::Vocabulary  在VocabularyBinary.hpp的VINSLoop命名空间中定义的一个结构体
  	理解成将字典读入到Vocabulary中
  */
  voc.deserialize(ifStream);
  ifStream.close();
  
  m_k = voc.k; //字典的中心点K的个数
  m_L = voc.L; //字典的层数
  m_scoring = (ScoringType)voc.scoringType; //评分方式
  m_weighting = (WeightingType)voc.weightingType; //权重
  
  createScoringObject();

  m_nodes.resize(voc.nNodes + 1); // +1 to include root 加入一个虚根节点
  m_nodes[0].id = 0;

  for(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[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(int i = 0; i < voc.nWords; ++i)
  {
    NodeId wid = (int)voc.words[i].wordId;
    NodeId nid = (int)voc.words[i].nodeId;
    
    m_nodes[nid].word_id = wid;
    m_words[wid] = &m_nodes[nid];
  }
}

VINSLoop命名空间

namespace VINSLoop {
    
struct Node {
    int32_t nodeId;
    int32_t parentId;
    double weight;
    uint64_t descriptor[4];
};

struct Word {
    int32_t nodeId;
    int32_t wordId;
};

struct Vocabulary {
    int32_t k;
    int32_t L;
    int32_t scoringType;
    int32_t weightingType;
    
    int32_t nNodes;
    int32_t nWords;
    
    Node* nodes;
    Word* words;
    
    Vocabulary();
    ~Vocabulary();
    
    void serialize(std::ofstream& stream); //序列化
    void deserialize(std::ifstream& stream); //反序列化（将读入的二进制文件进行解析，将字典写入到nodes和words中）
    
    inline static size_t staticDataSize() {
        return sizeof(Vocabulary) - sizeof(Node*) - sizeof(Word*);
    }
};

}

ORB-SLAM：使用二进制文件ORBvoc.bin加速词典读取 https://blog.csdn.net/qinqinxiansheng/article/details/116855868

VSLAM|回环检测之词袋字典如何生成？ https://blog.csdn.net/Yong_Qi2015/article/details/100997415

ORB-SLAM

ORB-SLAM是一个完整的 SLAM 系统，包括视觉里程计、跟踪、回环检测，是一种完全基于稀疏特征点的单目 SLAM 系统，同时还有单目、双目、RGBD 相机的接口。其核心是使用 ORB (Orinted FAST and BRIEF) 作为整个视觉 SLAM 中的核心特征。

ORB-SLAM框架的基本特征可以概括为以下四点：

ORB-SLAM 选用了 ORB 特征，基于 ORB 描述量的特征匹配和重定位，都比 PTAM 具有更好的视角不变性。此外，新增三维点的特征匹配效率更高，因此能更及时地扩展场景。扩展场景及时与否决定了后续帧是否能稳定跟踪。
ORBSLAM 加入了循环回路的检测和闭合机制，以消除误差累积。系统采用与重定位相同的方法来检测回路(匹配回路两侧关键帧上的公共点)，通过方位图 (Pose Graph) 优化来闭合回路。
PTAM 需要用户指定 2 帧来初始化系统，2 帧间既要有足够的公共点，又要有足够的平移量.平移运动为这些公共点提供视差 (Parallax)，只有足够的视差，才能三角化出精确的三维位置。ORB-SLAM 通过检测视差来自动选择初始化的 2 帧。
PTAM 扩展场景时也要求新加入的关键帧提供足够的视差，导致场景往往难以扩展。ORB-SLAM 采用一种更鲁棒的关键帧和三维点的选择机制——先用宽松的判断条件尽可能及时地加入新的关键帧和三维点，以保证后续帧的鲁棒跟踪; 再用严格的判断条件删除冗余的关键帧和不稳定的三维点，以保证 BA 的效率和精度。

ORB-SLAM 它是由三大块、三个流程同时运行的。第一块是跟踪，第二块是建图，第三块是闭环检测（本次学习关注的重点）。