【代码阅读】MSC-VO

MSC-VO是ICRA2022的一篇点线视觉SLAM论文，本身是在ORBSLAM2的基础上改进的，改进的部分在于为SLAM系统引入了线段，并且使用了曼哈顿坐标系与结构化约束进行优化，之前看过的论文记录可以参考链接，年前把线段匹配和均匀化的内容试着做了一下，现在计划看一下这篇文章的代码看一下后续的代码应该如何实现。

一、初始化部分

MSC-VO本身是基于ORBSLAM2的改进，所以基本的代码框架还是ORBSLAM2的那些东西，一个比较大的区别在于MSC-VO更换了数据的内容，将图像换为了深度图，也就是RGBD图像，所以在MSC-VO的代码里面，如果输入内容不是RGBD图像，就会报错。

整个MSC-VO的主函数在Examples文件夹下的RGB-D下的rgbd-tum.cc文件，该文件内部写有整个框架的主函数，通过初始化SLAM对象来调用其余的内容，初始化的部分和ORBSLAM2几乎没有差别，不同之处在于，由于MSC-VO使用的线段约束以及曼哈顿坐标系，在初始化system对象的时候需要补充这两个新增的部分。体现在代码上，system对象在初始化时会初始化一个Tracking对象，而这个对象在初始化时会将线段提取器和曼哈顿坐标系两个对象进行初始化。

这两个对象属于是MSC-VO中新增的，我们先简单看一下这两个对象的初始化内容。

线段提取器对象

线段提取器对象主要是负责线相关的操作，我们可以看作是线段版的orbextractor对象，该对象在初始化的时候需要指定四个参数：

四个参数分别为提取线段时的金字塔层数、金字塔的尺度因子、需要提取的线段数量以及线段的最短长度，这些参数来自于RGB-D文件夹下的yaml配置文件，需要我们在控制台运行主函数时给出这个配置文件的位置。根据源代码里面提供的几个配置文件，层数一般设置为1，尺度因子设置为1.2，线段数设置为200，在tum数据集上的最短长度设置为0，而在实际场景数据集中设置为了5。

曼哈顿坐标系对象

曼哈顿坐标系相关的内容在Manhattan.cpp中，需要传入一个opencv的mat对象，该对象在Tracking对象的构造函数内部被创建，初始化为一个3×3的单位矩阵，之后再四个位置填入了相机内参，也就是说在初始化时需要给曼哈顿坐标系对象相机的内参矩阵，这个内参矩阵内部的四个参数来自于配置文件。

传入的内参矩阵会进行提取并单独存储为对象中的一个属性，除此之外，曼哈顿坐标系对象初始化的过程还将三个坐标轴的相关信息也进行了处理，这一点应该对应于原论文里面初始化曼哈顿坐标系的部分，使用了一个二维的向量进行了处理，粗略初始化了坐标轴的三个方向。

最后还初始化了一个24维的向量，向量内部每个元素是一个3×3的mat对象，根据注释来看是用于用于去除曼哈顿坐标系线提取中的冗余，但是具体干什么用还没有见到，在后面看到的时候在具体分析。

对象初始化完成之后，根据文件名读取RGBD图像，调用SLAM对象的TrackRGBD函数进行后续的处理。这里稍微补充一下，RGBD的数据集其实是两组图组合出来的，这里拿ORBSLAM使用的那个桌子的数据集来说，下载好的数据集内部其实包括这么几个部分：

其中rgb文件夹下的图像和我们平时用的单目相机图像是一样的，就是带颜色的相机图像，而depth文件夹则存放了深度图像，这里的深度图实际上是单通道的灰度图，用对应位置上的像素值来表示传感器感知到的深度值，深度图和RGB图像之间用提前存好的关系来进行对齐，从而实现深度的感知。另外之前在魔改Dynaslam的时候用到的也是RGBD图像，当初在处理数据集的时候记得这类RGBD数据集需要一个association文件，这个文件用来实现深度图和RGBD图像的对齐，一般都是用现成的代码来生成，可以参考链接。

二、Tracking线程

通过SLAM对象调用TrackRGBD函数，将图像与深度图以及一个时间戳传入到函数中。在TrackRGBD函数中，通过调用mpTracker对象（也就是Tracking线程）的GrabImageRGBD函数来计算位姿，进入这个函数后通过cvtColor来进行通道的转换，将RGB图像转换为灰度图。转换好的图像用于初始化帧对象，由于使用了线段和曼哈顿坐标系，所以一些写法也发生了改变。

下图为ORBSLAM2代码中对应位置的写法：

可以看出，构造函数中增加了线段的提取器和曼哈顿距离的相关处理的内容，这几个对象都是在构造Tracking对象的时候进行初始化的，位置在Tracking对象的构造函数中，传入的对象其实就是给Frame对象中的相关属性进行了赋值，并将这些对象中的一些只取出来赋值给Frame对象。赋值结束后，就需要进行特征的提取，不同于ORBSLAM2代码顺序提取的方法，MSC-VO换用了多线程的提取方法，而不再是ORBSLAM2里面顺序提取的方法。这里使用c++里面的线程函数，初始化线程分别指向特征点提取、线段提取以及曼哈顿坐标系的处理，使用join函数来等待指向的函数执行完毕，关于join函数的作用可以参考链接。

在默认的执行顺序下，第一帧会送入到下面else的分支中调用三个函数进行特征提取，由于使用的是RGBD图像，所以这里的特征提取也有稍微的不同。首先是特征点的提取，在ORBSLAM2的代码中就单纯是用ORBextractor进行提取，而由于现在有了深度信息，所以我们多了一步将深度与特征对齐。

可以看见377行利用重载的括号，调用了提取特征点的函数，具体提取的部分可以参考ORBextractor.cc中的ORBextractor::operator()函数。提取结果需要经过两个函数的处理，UndistortKeyPoints函数负责畸变校正，ComputeStereoFromRGBD则负责将深度图中对应位置的深度提取出来并和特征点对齐，相当于给特征点填上深度，从而节省了三角化的时间。

相对地，线段提取的部分也采用了单独开辟线程的方式，由主函数所在线程等待线段提取线程执行完成再继续进行。线段提取的函数对应Frame.cc文件中的ExtractLSD函数，需要传入灰度图和深度图。进入函数之后可以看到为了保证写法上的类似，线段提取器的写法和ORB提取器的写法是一样的，也采用了重载括号的方法，提取好的线段经过isLineGood函数进行检测。

这里我们展开看一下线段提取和检查的函数，提取线段是利用重载括号调用LineEextractor.cpp文件里的LINEextractor::operator()函数，该函数需要传入五个内容：灰度图、掩码、两个存放结果的向量以及存放结果描述子的向量，这里在调用的时候，掩码传入的实际上是一个空的mat矩阵，可以忽略不计。这里有一个小细节，重载函数的传入参数，对灰度图和掩码的类型定义，使用的是cv::InputArray，而在调用括号重载函数时，我们传入的却是两个cv::Mat类型的，并且第四个参量也是这种情况，传入的是一个cv::Mat而定义时则使用了cv::OutputArray，这里应该是考虑传入内容的多样性，opencv为了让接口的传递更加灵活，就设计了这种机制，让InputArray和OutputArray来表征多种可能的参数类型，从而让接口更加具有代表性，这样就不用为了每个类型的输入输出都单独重载一次函数，具体可以参考添加链接描述。

由于使用了InputArray和OutputArray作为参数，所以代码首先进行了一下类型的转换，将其转换回Mat类型，之后初始化了一个线段提取对象，调用内部的detect函数进行线段的提取，提取完成后，如果线段的数量过多，就只选择一部分线段保留。完成后调用计算描述子的函数，对所有LSD提取出来的线段进行LBD描述子的计算。对于所有提取出来的线段，这里还使用了一个循环来为每条线计算了一个类似法向量的东西，这个循环相当于先取出了归一化平面上的起点和终点，之后调用叉乘函数并进行单位化，那这样来看的话，_lineVec2d存放的应该是相机光心、线段起点终点组成平面的法向量。