orb-slam2代码详解之mono kitti

前言

orb-slam2是基于orb特征点检测的视觉slam系统，继承自orb-slam,并在此基础上拓展为支持单目（MONOCULAR），双目（STEREO），深度相机（RGBD）的SLAM系统。
本文从mono-kitti文件开始，梳理以kitti数据集为基础的单目系统代码原理。

orb-slam的优势

总体基础知识

主体三个线程 Tracking LocalMapping LocalClosing
(其实还有一个viewer线程，用于显示界面)
从tracking这个前端线程开始，接收的就是一帧一帧的图片，也就是Frame，用于ORB特征提取，初始位姿估计。将Frame经过一定的计算后提取出关键帧，也就是KeyFrame传入LocalMapping线程进行建图以及优化。将此时的KeyFrame再经过一定的筛选补充传入LocalClosing进行回环检测和回环矫正。
所以可以看出Frame贯穿了整个系统的运行。

命名规则

mono-kitti文件的主体结构

mono-kitti文件位于examples/monocular中，是单目视觉处理kitti数据集的主函数。

代码详解

int main(int argc, char **argv)

文件的主函数
main函数的参数列表保存了输入参数的信息，第一个参数argc记录了输入参数的个数，
第二个参数argv是字符串数组的，字符串数组的每个单元是char*类型的，指向一个c风格字符串。
argc中的参数个数个argv是对应的。

参数读取

if(argc != 4)
    {
        cerr << endl << "Usage: ./mono_kitti path_to_vocabulary path_to_settings path_to_sequence" << endl;
        return 1;
    }

例如在本程序中，argc就是4。
首先判断输入是否是4个参数，按照正确的输入应该是：./mono_kitti path_to_vocabulary path_to_settings path_to_sequence。这里需要注意的是传入的参数分别对应argv[0],argv[1],argv[2],argv[3]。
重点是argv[0]对应的是程序自身，剩下三个就是我们输入的字符串，分别是，①字典词包的路径，②系统中装有一些如相机参数、viewer窗口的参数的配置文件，格式为YAML，③数据集的路径。数据集的路径根据自己下载时的位置为准，自己测试时记得解压数据集包。

cerr与cout的区别
1、cout经过缓冲后输出，默认情况下是显示器。这是一个被缓冲的输出，是标准输出，并且可以重新定向。
2、cerr不经过缓冲而直接输出，一般用于迅速输出出错信息，是标准错误，默认情况下被关联到标准输出流，但它不被缓冲，也就说错误消息可以直接发送到显示器，而无需等到缓冲区或者新的换行符时，才被显示。一般情况下不被重定向。
cerr主要作用在于当程序栈空间用完之后，仍具有输出功能，可以显示紧急的错误信息。

   // Retrieve paths to images检索图片路径
    vector<string> vstrImageFilenames;
    vector<double> vTimestamps;
    //将单目数据集图片（也就是Frames）load进来，argv[3]是Frame的路径，从第三个参数装载图像和时间戳。
    LoadImages(string(argv[3]), vstrImageFilenames, vTimestamps);

strSequence 图像序列的存放路径
vstrImageFilenames 装有每张图像地址和名字的vector
vTimestamps 图像序列中每张图像的时间戳
，是times.txt里每一行的时间组成的vector。

//得到图片的个数
int nImages = vstrImageFilenames.size();

LoadImages函数

LoadImages是主要读取文件的函数

//采用引用传入三个参数，后两个参数vstrImageFilenames和vTimestamps为空，引用完从函数中获取他们的值，函数执行完前者是装有图片具体位置的vector，位置形式如xxx/xxx/xxx/000xx.png，后者为装有一系列图片时间戳的vector。    
void LoadImages(const string &strPathToSequence, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
    //文件读操作，创建对象
    ifstream fTimes;
    //调用数据集中的时间戳文件
    string strPathTimeFile = strPathToSequence + "/times.txt";
    //c_str是string类的一个函数，可以把string类型变量转换成char*变量
    //open()要求的是一个char*字符串
    //当文件名是string时需要，当文件名是字符数组型时就不需要
    fTimes.open(strPathTimeFile.c_str());

    //循环读入timestamp文件到一个名为vTimestamps的vector
    //判断文件是否读完
    while(!fTimes.eof())
    {
        string s;
        //从流中读取一行到字符串s
        getline(fTimes,s);

        //如果字符串不是空的，就写进流，读成double类型放入vector
        if(!s.empty())
        {
            //流对象，用于流的输入和输出
            stringstream ss;
            ss << s;
            double t;
            ss >> t;
            //容器vector的函数，将元素压到最末端储存
            vTimestamps.push_back(t);
            //得到vTimesstamps储存了所有的时间戳
        }
    }

    //左眼为单目，读取左图像，strPrefixLeft是左图像路径
    //组装mono_kitti数据集中image_0目录的路径
    string strPrefixLeft = strPathToSequence + "/image_0/";

    const long nTimes = vTimestamps.size(); //有多少个时间戳
    vstrImageFilenames.resize(nTimes); //有多少个时间戳就有多少个图像，保持维度的一致性
    //按照时间戳的个数，分配容器空间，同时创建对象，初始默认分配为0

    //vstrImageFilenames是装有图像地址+名字的vector
    for(int i=0; i<nTimes; i++)
    {
        stringstream ss;
        //std::setw ：需要填充多少个字符,默认填充的字符为' '空格
        //std::setfill：设置std::setw将填充什么样的字符，如:std::setfill('*')
        //ss总共为6位，i之外的前边几位用0来填充，得到的结果为000001 000099之类
        ss << setfill('0') << setw(6) << i;
        ss << setfill('0') << setw(6) << i; //填6个0,如果来了个i，则代替0的位置，也就是总共有6位，末尾是i，其他用0填充
        vstrImageFilenames[i] = strPrefixLeft + ss.str() + ".png";
        //组装形成包含图像路径和名字编号的vector
    }
}

实例化SLAM系统

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    //argv[1]为vocfile 里边存储的是词汇
    //argv[2]为settingfile 里边存储摄像机校准和畸变参数和ORB相关参数
    //这里创建了System类型的SLAM对象，SLAM构造函数中初始化了系统所有线程和相关参数，并准备好处理帧，代码留待后边详细分析
     //读入词包路径，读入YAML配置文件，设置SLAM为mono状态,启用viewer的线程简要说明
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
    // Vector for tracking time statistics
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);
    //根据图像个数创建空间以及对象
 
    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;
    cout << "Images in the sequence: " << nImages << endl << endl;

循环Tracking

//cv::Mat在SLAM经常用于存储图像数据以及相机位姿，其兼容的数据类型多种多样，使用此数据结构前要明确搞清楚当前矩阵元素是什么类型的，不然后面进行逐个元素访问或者计算时会经常出错
cv::Mat im;
    // Main loop 主循环代码，循环读取一帧一帧的图片
    for(int ni=0; ni<nImages; ni++)
    {
        // Read image from file 读取图片 并按原样返回加载的图像
        im = cv::imread(vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
        //读取时间戳
        double tframe = vTimestamps[ni];
 		
 		//判断图像是否为空，是否成功加载
        if(im.empty())
        {
            cerr << endl << "Failed to load image at: " << vstrImageFilenames[ni] << endl;
            return 1;
        }

//#ifdef如果宏已经定义，则编译下面代码 
//如果编译器可以编译c++11
#ifdef COMPILEDWITHC11
//std::chrono::steady_clock 为了表示稳定的时间间隔，后一次调用now()得到的时间总是比前一次的值大（这句话的意思其实是，如果中途修改了系统时间，也不影响now()的结果），每次tick都保证过了稳定的时间间隔。
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
//std::chrono::monotonic_clock表示单调递增的时间间隔
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
//条件编译的结束
#endif
 
        // Pass the image to the SLAM system 
        //将图片以及时间戳传给SLAM系统。TrackMonocular讲图片传给Tracking，其实Tracking就是系统的主线程。这里传入的im引起了系统的一系列操作
        SLAM.TrackMonocular(im,tframe);
 
#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif
 //进行实践单位转换，并表示时间段
        double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
 
        vTimesTrack[ni]=ttrack;
 
        // Wait to load the next frame
        // 计算下一帧图片时间戳与当前时间戳的差值T，与追踪所需时间进行比较
        // 如果有必要就将当前线程暂停sleep
        // 主要是为了模拟时间情况，因为追踪结束以后下一帧可能还没来
        double T=0;
        if(ni<nImages-1)
            T = vTimestamps[ni+1]-tframe;
        else if(ni>0)
            T = tframe-vTimestamps[ni-1];
 
        if(ttrack<T)
        //把进程挂起一段时间，单位是秒
            usleep((T-ttrack)*1e6);
    }
 
    // Stop all threads 关闭SLAM系统，也就关闭了系统中的几个线程
    SLAM.Shutdown();
 
    // Tracking time statistics
    //追踪时间排序后相加
    sort(vTimesTrack.begin(),vTimesTrack.end());
    float totaltime = 0;
    for(int ni=0; ni<nImages; ni++)
    {
        totaltime+=vTimesTrack[ni];
    }
    cout << "-------" << endl << endl;
    cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
    cout << "mean tracking time: " << totaltime/nImages << endl;
 
    // Save camera trajectory 
    // 整个SLAM系统运行完后，计算出来的是相机的运行轨迹，系统退出时保存相机轨迹
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");    
 
    return 0;
}

附录

stringstream

定义了三个类：istringstream、ostringstream 和 stringstream，分别用来进行流的输入、输出和输入输出操作。本文以 stringstream 为主，介绍流的输入和输出操作。

主要用来进行数据类型转换，由于 使用 string 对象来代替字符数组（snprintf方式），能避免缓冲区溢出的危险；而且，因为传入参数和目标对象的类型会被自动推导出来，所以不存在错误的格式化符的问题。简单说，相比c库的数据类型转换而言， 更加安全、自动和直接。
代码示例

// swapping ostringstream objects
#include <string>       // std::string
#include <iostream>     // std::cout
#include <sstream>      // std::stringstream

int main () {

  std::stringstream ss;

  ss << 100 << ' ' << 200;

  int foo,bar;
  ss >> foo >> bar;

  std::cout << "foo: " << foo << '\n';
  std::cout << "bar: " << bar << '\n';

  return 0;
}
 Edit & Run

Output:
foo: 100
bar: 200
原文链接：c++ stringstream ss()

vector

vector容器
vector数据结构和数组非常相似，也称为单端数组
vector与普通数组区别：
不同之处在于数组是静态空间，而vector可以动态扩展
动态扩展:
并不是在原空间中后续接新空间，而是找更大的内存空间，然后将原数据拷贝新空间，释放原空间
vector容器的迭代器是支持随机访问的迭代器

vector容量和大小
函数原型：
empty（） 判断容器是否为空
capacity（） 容器的容量
size（） 返回容器中元素的个数
resize（int num） 重新制定容器的长度为num，若容器变长，则以默认值填充新位置，若容器变短，则末尾超出容器长度的元素被删除
resize（int num，elem） 重新指定容器的长度为num，若容器变长，则以elem填充，若变短，则同上

#include<vector>
void test1()
{
	vector<int>v1;
	//为v1赋值，此处省略
	if(v1.empty())
	//返回bool变量，返回真值说明是空的
	{
		cout<<"kong"<<endl;
	}
	else
	{
		cout<<v1.capacity()<<endl;
		//存在动态扩展，所以容量永远会大于当前存储的数
		cout<<v1.size()<<endl;
		//size与capacity的区别，元素个数与容量，容量大于元素个数
		v1.resize(15);
		//多出的五个位置会默认用0顶替
		v1.resize(15,100);
		//多出的位置用100来顶替
	}	
}

vector的插入和删除
函数原型：
push_back（ele）； 尾部插入元素ele
pop_back（）； 删除最后一个元素
insert（const_iterator pos,ele） 迭代器指向位置pos插入元素ele
insert(const_iterator pos,int count,ele) 迭代器指向位置pos插入count个元素ele
erase(const_iterator pos) 删除迭代器指向的元素
erase(const_iterator start,const_iterator end) 删除迭代器从start到end之间的元素
clear() 删除容器中的所有元素

#include<vector>
void test1()
{
	vector<int>v1;
	v1.push_back(10);
	v1.push_back(20);
	v1.push_back(30);
	v1.push_back(40);
	v1.push_back(50);
	//尾插
	v1.pop_back();
	//尾删
	v1.insert(v1.begin(),100);
	//在指定的开头位置插入100
	v1.insert(v1.begin(),2,100);
	v1.erase(v1.begin());
	v1.erase(v1.begin(),v1.end());
	//相当于清空
	v1.clear();
}

原文链接vector的相关函数用法

chrono库

要使用chrono库，需要#include，其所有实现均在std::chrono namespace下。注意标准库里面的每个命名空间代表了一个独立的概念。所以下文中的概念均以命名空间的名字表示！ chrono是一个模版库，使用简单，功能强大，只需要理解三个概念：duration、time_point、clock。
原文链接c++ std::chrono库详解

预处理指令

预处理指令是以#号开头的代码行。#号必须是该行除了任何空白字符外的第一个字符。#后是指令关键字，在关键字和#号之间允许存在任意个数的空白字符。整行语句构成了一条预处理指令，该指令将在编译器进行编译之前对源代码做某些转换。

,预处理指令是在编译器进行编译之前进行的操作.预处理过程扫描源代码，对其进行初步的转换，产生新的源代码提供给编译器。可见预处理过程先于编译器对源代码进行处理。在很多编程语言中，并没有任何内在的机制来完成如下一些功能：在编译时包含其他源文件、定义宏、根据条件决定编译时是否包含某些代码(防止重复包含某些文件)。要完成这些工作，就需要使用预处理程序。尽管在目前绝大多数编译器都包含了预处理程序，但通常认为它们是独立于编译器的。预处理过程读入源代码，检查包含预处理指令的语句和宏定义，并对源代码进行响应的转换。预处理过程还会删除程序中的注释和多余的空白字符。
原文链接c/c++中的预处理指令