orb-slam2代码详解之system

最新推荐文章于 2023-08-30 16:16:28 发布

it waits

最新推荐文章于 2023-08-30 16:16:28 发布

阅读量1k

点赞数 5

本文链接：https://blog.csdn.net/itwaits/article/details/113029296

版权

system在orb-slam2中的作用

1）读取ORB字典，为后期的回环检测做准备；

2）创建关键帧数据库KeyFrameDatabase，用于存放关键帧相关数据；

3）初始化Tracking线程。其实Tracking线程是在main线程中运行的，可以简单的认为main线程就是Tracking线程；

4）初始化并启动LocalMapping线程；

5）初始化并启动LoopClosing线程；

6）初始化并启动窗口显示线程mptViewer；

7）在各个线程之间分配资源，方便线程彼此之间的数据交互。

system程序基本结构

在这里插入图片描述

system类的基本介绍

system.h

System.h中包含了七个类，分别是Viewer，FrameDrawer，Map，Tracking，LocalMapping，LoopClosing的声明，和System类的定义，就像描述的一样，这些类组成了一个系统。

class Viewer;
class FrameDrawer;
class Map;
class Tracking;
class LocalMapping;
class LoopClosing;
class System;

system类

System 类中的public成员包含一个sensor枚举和一些成员函数，主要有：
① sensor枚举

// Input sensor 定义eSensor类型的sensor变量
 enum eSensor{
     MONOCULAR=0,
     STEREO=1,
     RGBD=2
 };

0,1,2分别代表传感器的类型。
枚举数据类型是一种由程序员定义的数据类型，其合法值是与它们关联的一组命名整数常量。
之所以被称为枚举类型，就是因为命名常量是作为数据类型定义的一部分而枚举或列出的，以下是枚举类型声明的示例：

enum Roster {Tom, Sharon, Bill, Teresa, John};

该语句将创建一个名为 Roster 的数据类型。因为单词 enum 是 C++ 关键字，所以它必须小写
和 Roster 数据类型关联的命名整数常量被称为枚举量，Roster 数据类型的变量可能只是关联到这些枚举量的值之一。
默认情况下，编译器设置第一个枚举量为 0，下一个为 1，以此类推。
重要的是要意识到，enum 语句示例实际上并没有创建任何变量，只是定义数据类型。当以后创建这个数据类型的变量时，它们看起来就是整数，并且这些整数的值被限制在与枚举集合中的符号名称相关联的整数上。

② System构造函数

System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true);

可以看到构造函数中，构建SLAM系统需要输入词包路径、YAML配置文件、设置传感器类型，本示例中设置为mono，启用viewer线程。

③ Tracking函数

//双目视觉传入左眼视觉图像，右眼视觉图像，以及时间戳
cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp);
//RGB-D传入彩色图和深度图以及时间戳
cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp);
cv::Mat TrackMonocular(const cv::Mat &im, const double &timestamp);

针对不同传感器不同的Tracking。输入图像可以使rgb的也可以是grayscale的（最终读进去都会转化为grayscale的），函数返回值为camera的位姿pose。

④ 定位模式函数

void ActivateLocalizationMode();
void DeactivateLocalizationMode();

调用前者终止mapping线程，开启定位模式，调用后者重启mapping线程。

⑤ 重启与终止函数

void Reset();
void Shutdown();

前者可以清空map，后者可以终止所有线程，在保存相机轨迹之前需要调用此函数。

⑥ SaveTrajectory 函数

void SaveTrajectoryTUM(const string &filename);
//二者相似
void SaveKeyFrameTrajectoryTUM(const string &filename);
void SaveTrajectoryKITTI(const string &filename);

把相机轨迹存成相应数据集的格式，调用此函数时先shutdown SLAM系统，mono_kittti中save函数用的是SaveKeyFrameTrajectoryTUM，这个函数看起来像是只能用于TUM数据集，但三种传感器均适合。

system构造函数

传入参数

/**
 * strVocFile 为词典文件
 * strSettingsFile 为设置配置文件
 * sensor sensor类型，单目、双目和RGBD
 * bUseViewer 是否进行相机位姿和图片帧显示
*/
//首先将参数传递到成员变量中，msensor是private的成员变量和sensor数据类型相同，此时传入为mono
//mpViewer是System类的隐含成员变量，Viewer类指针，这里赋空。
System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
               const bool bUseViewer):mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false),mbActivateLocalizationMode(false),
        mbDeactivateLocalizationMode(false)

读取传进来的文件路径

 // Output welcome message 显示程序信息，以及此时设定的传感器
    cout << endl <<
    "ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza." << endl <<
    "This program comes with ABSOLUTELY NO WARRANTY;" << endl  <<
    "This is free software, and you are welcome to redistribute it" << endl <<
    "under certain conditions. See LICENSE.txt." << endl << endl;
 
    cout << "Input sensor was set to: ";
 
    if(mSensor==MONOCULAR)
        cout << "Monocular" << endl;
    else if(mSensor==STEREO)
        cout << "Stereo" << endl;
    else if(mSensor==RGBD)
        cout << "RGB-D" << endl;

    //Check settings file 检查配置文件是否打开
    //将YAML文件打开，进行读操作
    cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
       cerr << "Failed to open settings file at: " << strSettingsFile << endl;
       exit(-1);
    }


    //Load ORB Vocabulary 加载ORB词包
    //ORB中vocabulary的作用是特征空间的划分和Bag-of-Words Vector的计算。
    cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
	
	//使用默认的构造函数，使用无参化构造方法构造一个对象
	//使用mpVocabulary指针指向在堆区存储该对象的位置
	//ORBVocabulary类是 typedef 成的，来源于 DBoW2 的TemplatedVocabulary类 也就是子类继承
	//DBoW2能够快速稳定地计算两幅图像之间的相似程度。
    mpVocabulary = new ORBVocabulary();
    //使用指针调用类内函数加载词包，并返回布尔变量表示是否成功加载
    bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
    //判断是否成功
    if(!bVocLoad)
    {
        cerr << "Wrong path to vocabulary. " << endl;
        cerr << "Falied to open at: " << strVocFile << endl;
        exit(-1);
    }
    cout << "Vocabulary loaded!" << endl << endl;

初始化各种线程和数据库

①用词袋初始化关键帧数据库（用于重定位和回环检测）

②初始化一个Map类，该类用于存储指向所有关键帧和地图点的指针

③初始化画图工具，用于可视化。

④初始化 Tracking线程，主线程，使用this指针（只初始化不启动，启动在 main 函数里 TrackMonocular() 启动）

⑤初始化 Local Mapping线程并启动（这里mSensor传入MONOCULAR）

⑥初始化 Loop Closing线程并启动（这里mSensor传入的不是MONOCULAR）

⑦初始化 Viewer线程并启动，也使用了this指针；给 Tracking线程设置Viewer

⑧三个线程每两个线程之间设置指针

    //Create KeyFrame Database 创建关键帧数据库
    //有两个重要功能，一是计算在回环检测时计算候选关键帧集合，二是在重定位时计算候选关键帧
    mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
 
    //Create the Map 创建地图对象
    //增删关键帧和地图点
    mpMap = new Map();
 
    //Create Drawers. These are used by the Viewer 创建两个显示窗口
    mpFrameDrawer = new FrameDrawer(mpMap);
    mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);
 
    //Initialize the Tracking thread
    //(it will live in the main thread of execution, the one that called this constructor)
    //这里初始化的Tracking线程是在main线程中运行的(其实也就是main线程)，所以不需要调用new thread来创建
    mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                             mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
 
    //Initialize the Local Mapping thread and launch
    //初始化LocalMapping对象，并开启LocalMapping线程
    mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
    //子线程在创建时启动,使用功能std::thread类创建线程对象
    //std::thread类的构造函数是使用可变参数模板实现的，也就是说，可以传递任意个参数，第一个参数是线程的入口函数，而后面的若干个参数是该函数的参数。
    //run 调用loopclosing里面函数
    mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
 
    //Initialize the Loop Closing thread and launch 
    //初始化LooClosing对象，并开启LoopClosing线程
    mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
 
    //Initialize the Viewer thread and launch
    //是否显示图像帧和相机位姿的窗口
    if(bUseViewer)
    {
        //初始化显示窗口对象，并启动线程用于显示图像和地图点
        mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
        mptViewer = new thread(&Viewer::Run, mpViewer);
        mpTracker->SetViewer(mpViewer);
    }
 
    //Set pointers between threads
    //给各个线程分配资源
    //以下变量设置是为了在各个线程中可以访问其他线程中的数据，方便线程之间的数据交互
    mpTracker->SetLocalMapper(mpLocalMapper);
    mpTracker->SetLoopClosing(mpLoopCloser);
 
    mpLocalMapper->SetTracker(mpTracker);
    mpLocalMapper->SetLoopCloser(mpLoopCloser);
 
    mpLoopCloser->SetTracker(mpTracker);
    mpLoopCloser->SetLocalMapper(mpLocalMapper);

附录

FileStorage类

FileStorage类将各种OpenCV数据结构的数据存储为XML 或 YAML格式。同时，也可以将其他类型的数值数据存储为这两种格式。
构造函数
FileStorage类的构造函数为：

cv::FileStorage(const string& source, int flags， const string& encoding=string());

参数：

source –存储或读取数据的文件名（字符串），其扩展名(.xml 或 .yml/.yaml)决定文件格式。

flags – 操作模式，包括：

FileStorage::READ 打开文件进行读操作
FileStorage::WRITE 打开文件进行写操作
FileStorage::APPEND打开文件进行附加操作
FileStorage::MEMORY 从source读数据，或向内部缓存写入数据（由FileStorage::release返回）
encoding – 文件编码方式。目前不支持UTF-16 XML 编码，应使用 8-bit 编码。

原文链接：opencv学习笔记——FileStorage类的数据存取操作

orb vocabulary

为了减少累积误差，目前Visual SLAM都加入回环检测了，并通过后端优化来纠正整个回环中的姿态估计误差。要实现回环检测，SLAM系统需要回答一个问题：这个场景之前我是不是见过？而且是在可能出现平移+绽放+旋转的情形下，判断当前帧与之前记录过的帧是不是来源于同一个场景。这个问题落入到基于内容的图像检索范畴。ORB-SLAM采用的是Bag-of-Words的方式将图像中的多个局部描述符表示成一个定长的全局特征，而Vocabulary Tree是局部描述符量化和索引的一种高效数据结构。

原文链接：为什么要加载orb vocabulary

OrbVocabulary部分

DBoW2是一个开源C++库，可以将图像转换为bag-of-word的表示（词袋模型）。它使用特征提取算法（可选取）提取图像的特征描述子，然后根据一定的策略将这些特征描述子聚类为“单词”，并使用树的形式组织，得到一个vocabulary tree。之后将利用vocabulary tree将图像转换为{单词，权值}的向量表示，通过计算向量间距离的方式计算图像之间的相似性。

FORB
类FORB派生自FClass，简单查看下FClass，它是一个虚类，定义了一些对描述子操作的函数，重要的包括（1）meanValue()：用于计算描述子集合的中值；（2）distance()：计算两个描述子之间的距离。

OrbVocabulary类
查看代码，发现 OrbVocabulary是这样定义的

typedef DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB>  OrbVocabulary;

TemplatedVocabulary是一个字典模板类，具体定义见TemplatedVocabulary.h，它定义了字典类的通用操作：字典的构造、保存、读取、将图像/特征转换为单词表示等。 OrbVocabulary中传入的类是DBoW2::FORB::TDescriptor和DBoW2::FORB，前者是描述子类型的定义，查看代码后发现就是cv::Mat（opencv中ORB得到的描述子使用cv::Mat保存）；后者前面已经介绍了。
原文链接：ORBSLAM2学习（四）：DBoW2源码分析（OrbVocabulary部分）

KeyFrameDatabase

这个类内容较少，有两个重要功能，一是计算在回环检测时计算候选关键帧集合，二是在重定位时计算候选关键帧

变量：

std::vector<list<KeyFrame*> > mvInvertedFile

是一个向量，里面存放的是一个个单词，每个单词对应一个list链表，每个链表里存放的是拥有该单词的关键帧。

关键函数：

vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)

原文链接：KeyFrameDatabase类

线程类构造函数

构造函数的参数
std::thread类的构造函数是使用可变参数模板实现的，也就是说，可以传递任意个参数，第一个参数是线程的入口函数，而后面的若干个参数是该函数的参数。

第一参数的类型并不是c语言中的函数指针（c语言传递函数都是使用函数指针），在c++11中，增加了可调用对象(Callable Objects)的概念，总的来说，可调用对象可以是以下几种情况：

1.函数指针
2.重载了operator()运算符的类对象，即仿函数
3.lambda表达式（匿名函数）
4.std::function
原文链接：[c++11]多线程编程(二)——理解线程类的构造函数

run成员函数

run 调用loopclosing里面函数
CheckNewKeyFrames（），判断是否插入关键帧，如果有，继续

DetectLoop()，布尔型，判断是否存在闭环，如果有，继续

l 每个候选帧将与自己相连的关键帧构成一个“子候选组spCandidateGroup”，vpCandidateKFs–>spCandidateGroup

l 检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在nCurrentConsistency++，则将该“子候选组”放入“当前连续组vCurrentConsistentGroups”

l 如果nCurrentConsistency大于等于3，那么该”子候选组“代表的候选帧过关，进入mvpEnoughConsistentCandidates

ComputeSim3() 计算sim3

CorrectLoop() 闭环校正

ResetIfRequested() 检测是否结束
在这里插入图片描述

线程间数据的传递

更新其他线程的数据途径：私有成员变量（指针操作，所有Tracking线程数据更新，实际更新的是其他两个线程的数据）

//Other Thread Pointers
///局部建图器句柄
LocalMapping* mpLocalMapper;
///回环检测器句柄
LoopClosing* mpLoopClosing;

获取函数：私有成员函数

//设置局部建图器
void Tracking::SetLocalMapper(LocalMapping *pLocalMapper)
{
    mpLocalMapper=pLocalMapper;
}

//设置回环检测器
void Tracking::SetLoopClosing(LoopClosing *pLoopClosing)
{
    mpLoopClosing=pLoopClosing;
}