【代码阅读】MSC-VO

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

〇、代码运行

根据MSC-VO的readme直接配置环境即可，因为这个框架本身就是基于ORBSLAM的，所以环境和ORBSLAM基本上是一样。在运行代码的时候，由于使用的是RGBD相机，所以需要利用单独的py文件，给RGB图像和深度图做一个对应，简单来说就是将同一个时刻的深度图和RGB图的文件名做一个匹配，这一步在之前魔改dynaslam的时候也用到过，readme中也直接给出了association.py的下载链接，直接下载后按照指令执行即可。运行时需要设置五个参数，包括ORB的字典、配置文件、数据集的位置、association文件袋位置、是否进行可视化，相关的CLION配置如下图所示：


代码环境配置属于相对比较容易，环境配置成功后，会出现段错误的问题，代码运行过程中遇见了两个段错误，第一个段错误是出现在GrabImageRGBD函数的Optimizer::LineOptStruct(&mCurrentFrame)位置，这里目前还没有解决。如果将这个函数的调用注释掉，有时候会出现另一个段错误，这个错误尝试了注释掉整个项目的-march=native，结果离奇修复了，如果还是出现，可以尝试将已经编译过的g20库和dbow2库清理一下，也就是将thirdparty文件夹下的build文件夹都清理掉，然后重新执行主文件夹下的build.sh，这样可以解决一个段错误的问题，这个问题应该是编译指令集的问题，设置了-march=native之后会让编译的过程很僵硬，具体可以参考链接。
关于第一个段错误，找到解决方法后会再补充解决方法，目前注释掉是可以成功运行的，左边的红蓝绿三个轴对应的就是提取出来的曼哈顿坐标系。

经过漫长的debug过程，另一个段错误终于解决了，这里补充一下解决的过程，定位到bug的位置之后，尝试了很多方法，打断点、加输出等方法，表现的都是一个问题，在LineOptStruct函数结束时，准确来说是在LineOptStruct结束回到主函数的过程中出现的段错误，利用断点一点一点理顺内存也没发现错误，今天鬼使神差，无意中发现LineOptStruct函数和同一个文件夹下的PoseOptimization函数很像，对着大体看了一下，也没发现什么问题，最后就差了个return，抱着试一试的态度加了一下，结果居然跑通了。

谁成想整了四五天，最后错在没有加返回值，后面尝试了将函数返回值类型改为void，也是可行的，并不清楚这个错误是什么导致的，可能是编译器版本的问题，反正不会是论文代码的问题，按道理对于有返回值的函数，就算不写return语句，也会有一个默认返回值，所以绝大多数时候不会注意到这个问题。反正最后代码成功运行，效果还不错。

一、初始化部分

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图像的对齐，一般都是用现成的代码来生成，可以参考链接。

通过SLAM对象调用TrackRGBD函数，将图像与深度图以及一个时间戳传入到函数中。在TrackRGBD函数中，通过调用mpTracker对象（也就是Tracking线程）的GrabImageRGBD函数来计算位姿，进入这个函数后通过cvtColor来进行通道的转换，将RGB图像转换为灰度图。

Frame对象的初始化

转换好的图像用于初始化帧对象，由于使用了线段和曼哈顿坐标系，所以一些写法也发生了改变。

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

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

在默认的执行顺序下，第一帧会送入到下面else的分支中调用三个函数进行特征提取，由于使用的是RGBD图像，所以这里的特征提取也有稍微的不同。

ORB特征点提取 ExtractORBNDepth

首先是特征点的提取，在ORBSLAM2的代码中就单纯是用ORBextractor进行提取，而由于现在有了深度信息，所以我们多了一步将深度与特征对齐。

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

LSD线段提取 ExtractLSD

相对地，线段提取的部分也采用了单独开辟线程的方式，由主函数所在线程等待线段提取线程执行完成再继续进行。线段提取的函数对应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描述子的函数，对所有LSD提取出来的线段进行LBD描述子的计算。对于所有提取出来的线段，这里还使用了一个循环来为每条线计算了一个类似法向量的东西，这个循环相当于先取出了归一化平面上的起点和终点，之后调用叉乘函数并进行单位化，那这样来看的话，_lineVec2d存放的应该是相机光心、线段起点终点组成平面的法向量。

线段的三角化 isLineGood

提取完成后进入到isLineGood函数进行检验，需要传入灰度图、深度图以及相机的内参矩阵。从原作者的注释可以看出，这个方法也是借鉴的ICRA2021的一篇文章的源代码。进入函数后首先进行一些初始化操作，之后遍历每一条提取出来的线段，计算其长度，并根据长度来确定线段上采样点的数量，一般默认取20个采样点，如果线段长度短于20个像素，则取线段上的每个点。

确定好采样点的数量，就需要开始进行采样，采样过程是根据比例进行的，根据线段起点和终点坐标，按比例分配得到一个线段上的2d点作为采样点，由于按比例分配存在取整的问题，像素值是整数，而取整计算时必然会出现精度丢失，所以后续补充了一段判断，利用col和row选择了距离采样点最近的一个位置，之后利用深度图来得到采样点的深度信息，得到了深度信息，就可以利用针孔相机的成像原理计算出相机坐标系下采样点的3d坐标，将所有成功计算出3d坐标的点进行存储。

如果最后成功计算出3d信息的点的数量少于五个，则认为该线段效果不好。对于保留下来的线段，将每个采样点用compPt3dCov函数进行处理。关于这个compPt3dCov函数，理解起来确实比较困难，根据函数的输入值和返回值，我们可以确定的是这个函数负责对提取出来的3d点进行一系列的计算，问题在于过程是如何实现的。

线段采样与采样点的深度信息恢复 compPt3dCov

进入compPt3dCov函数，传入了3d点的坐标、相机的内参矩阵K以及一个时间戳，首先对内参矩阵利用convertTo函数进行了一个格式上的转换，转换后提取了内参矩阵中的三个值，这里的cu和cv根据提取位置来看，应该是高博视觉SLAM十四讲里面对应章节的cx和cy，而f则对应的是fx，可根据注释来看，这里是把fx当做了焦距f，而在相机内参的推导过程中我们可以知道，fx和fy实际上是焦距和缩放比例的结合，这里应该是有些问题的，当然也有一种可能是，这里本身是一个优化的过程，存在一定的误差，而混用fx和f对误差影响不大，从配置文件也可以看出，fx和fy在给定的数据集上差别是很小的，可能是出于这个考虑才写了这样的代码。

按照fx就是f焦距的思路来看，后面的部分是利用焦距，初始化了一个点的协方差矩阵，对协方差矩阵做了奇异值分解。考虑到协方差矩阵本身是一个方阵，所以这里为了方便理解我干脆混用了奇异值分解和特征值分解，那么按照这个思路，分解的结果中U表示的就是特征向量拼凑出来的一个矩阵，每一列都是一个特征向量，结果中的W就是特征值组成的向量，按照线性代数里面的内容，W应该是一个对角矩阵，对角线上的每个值都是一个特征值，在LineExtractor.h里面可以看到这几个量的注释，明确的写了W是一个向量，D则是W对应的对角矩阵，这么看的话也能解释的通。
对得到的特征值取倒数组成了矩阵D，由于特征值的倒数这个东西比较少见，稍微查一查可以查到：矩阵特征值的倒数是其逆矩阵的特征值。所以这里D应该是协方差矩阵的逆矩阵的特征值组成的对角矩阵，而矩阵取逆操作并不改变其特征向量，所以代码里的du应该表示的是特征向量组成矩阵的逆矩阵乘以协方差矩阵的逆矩阵。至于后面的东西有啥用，目前还没有看到用的地方，等用到了再说。

回到isLineGood函数，如果不深究compPt3dCov函数的话，这里作者的目的就是为了用一种更加合理的方式进行线段的三角化，这种鉴别方式本身也是借用了ICRA2021的另一篇文章，根据原论文的叙述，这里考虑到深度图本身的深度也是不稳定的，尤其是在物体边界的时候，所以这里首先采用采样的方式，对深度信息进行了一个检验从而避免边缘造成的影响太大，之后对采样的点尝试恢复深度信息并进行一系列的计算，计算结果将用于RANSAC提取线段的3d表示，如果提取线段比较准确则认为线段的三角化成功。

计算线段3d表达式 extract3dline_mahdist

这里提取线段的3d表示采用的方法是采样点结合RANSAC来实现的，通过调用线段提取器对象的extract3dline_mahdist函数，传入的是上一步中我们调用compPt3dCov函数提取出来的3d采样点对象，返回值则是一条3d线段对象。下面我们展开看一下这个函数，经过一些列的初始化之后，首先会根据内部点的数量计算一个迭代次数，这里是用maxIterNo来表示的，这个量在赋值时用的是点的数目乘以点的数目减一最后除以二，相当于所有点的组合的次数，用这个次数来确定进行拟合的次数。确定好次数之后对所有点进行一个随机排序，排序好后取最前面两个点，以此来表示从中随机取两个点。

两点可以确定一条直线AB，之后遍历这条线上的所有点，计算点到直线AB的距离，如果距离小于阈值则进行记录，最后统计记录下来的点的数量，如果数量足够多，则将点A和点B以及记录下来的点送入verify3dLine函数，这个函数内会检验直线AB是否可以较好地表征记录的点，根据作者写的注释，这个函数将线段AB分为n段，统计这n个小段上有多少段是能够被点投影上去的，如果投影上去的小段太少就会认为线段的效果不好。

如果认为线段AB具有足够好的代表性，就会存储点A和点B，如此重复迭代，最后保留内点数量最多的线段AB，就用这个最好的线段AB进行3d线段的拟合，这里调用computeLine3d_svd函数，传入带有噪声的共线3d点，使用PCA的方法，计算线段的方向向量以及点坐标的均值，并根据计算结果统计距离足够小的点的数目，迭代计算直到线段拟合效果足够好。这里计算距离采用的是一种比较奇怪的方法，调用computeLine3d_svd得到了所有点的坐标均值和方向向量，利用这两个量，可以确定两个点，进而变成一条线，距离的计算就是计算点到这条线的距离，如果足够小再次进行标记。

如此重复计算，最后得到了一个均值和方向向量，相当于基本确定了这条线，提取出线段的端点并进行相关的计算，就可以返回这条线。所以对于MSC-VO采用的线段计算方法，它其实采用的是用3d点拟合的方法，使用RGBD相机可以直接得到点的深度信息，也就是说这里我们提取到线并从上面进行采样，采样点是自带深度的，我们需要做的就是利用3d采样点对线段进行一个拟合，考虑到深度信息具有不准确的问题，这里的拟合采用了很严谨的方法，采用RANSAC的方法多次计算并根据内点数量进行了筛选。

回到isLineGood函数，通过extract3dline_mahdist函数得到了当前这条线的拟合出来的3d表达式，之后函数会检测端点的距离是否足够远，如果足够，则进行一些额外计算并存储。

isLineGood函数也就结束了，这个函数本身不是MSC-VO作者写的，是借鉴的PlanarSLAM里面的内容，这里吐槽一下，函数名和其内部功能差别有点大，看函数名以为是鉴别线是否足够好，但实际上是对线做深度信息的恢复。

提取曼哈顿坐标系 ExtractMainImgPtNormals

这个函数十分的短小，就是采用同样的重载括号的写法，调用了曼哈顿坐标系对象的重载函数，需要传入的参数包括原图、相机的内参矩阵和两个用于存储结果和辅助信息的中间向量。

这里我们直接去看重载函数的内容，其位置在Manhattan.cpp中，函数也不长，内部调用了两个其它的函数。

提取曼哈顿坐标系的内容是作者借鉴的另一篇文章的方法，关于这篇文章使用的提取方法，可以参考链接。

计算uv方向向量 computeNormalsLPVO

该函数传入了两个Mat对象以及两个vector对象，两个Mat分别是深度图和相机的内参矩阵K，进入函数后，首先会用一个双层循环遍历深度图的每个像素，如果深度在合法范围内，就对其进行运算并存储。

这里关键在于运算的实现过程，也就是代码的245-247行，这部分代码中的z是第u列第v行的像素的深度值，mCx和mCy指的是内参矩阵中的cx和cy，mInvFx和mInvFy指的是内参矩阵中fx和fy的倒数，这四个量都是在初始化曼哈顿对象的时候赋值的。这部分的计算其实是在做坐标系的转换，uv我们可以看作是像素坐标系下的坐标，根据相机成像原理的公式，我们可以得到下面的推理：

所以这里做的其实是根据像素坐标和相机成像原理，反过来求出相机坐标系下对应位置的坐标并进行存储，vertexMap里面每个位置存储的，其实就是对应位置上的相机坐标系下的三维坐标。关于赋值的问题，vertexMap在定义的时候，指定的类型是CV_32FC3，这表示是三通道的32位float，所以在给mat对应位置赋值的时候，使用了Vec3f来形成一个三通道的向量，并将xyz传入了三通道，因此赋值操作是没有问题的。

之后遍历像素坐标系下非边界的每个像素，取中央和上下左右一共五个位置的像素，利用上一步计算出来的3d坐标，形成横向和纵向（u和v两个方向）两个向量并存储其内容。

对于得到的三通道的结果uTangeMapSplitted和vTangeMapSplitted，使用cv::integral函数进行积分图的计算，关于积分图，简单来说就是以前算法比赛里面的那个局部和矩阵，详细可以参考链接，积分图的结果会单独存放。

计算完成后，以网格为单位统计网格内部的向量指向情况，这种网格的处理方法主要是为了一定程度上去除噪声，同时减小计算量，这里网格大小通过cell_size来调节，默认大小是10，格子的移动幅度用norm_density来调节，默认值是15。也就是说，格子移动的过程是存在间隔的。

在每个格子内，利用积分图获得格子内合法点的数目，合法与否取决于tangeMask的值，合法为1.0反之为0，同样用积分图，计算u和v两个方向上三个通道内的和并取平均值，这两个结果做叉乘从而得到法向量，利用cv::normalize函数进行归一化后进行存储。计算结果对应的就是当前这个网格内平面法向量。

从函数的最终结果来看，保存了两个值：平面法向量组成的矩阵和相机坐标系下的深度信息，这两个值返回给了调用的函数。

3d直方图筛选方向向量 getMainOrientations

在computeNormalsLPVO函数中，我们利用每个格子内的点产生的uv两个向量，对所有向量取了均值并叉乘得到了当前这个网格的平面向量，也就是说格子数和向量数是一致的，在getMainOrientations函数中，根据作者的备注，应该是对向量利用3d直方图策略进行了过滤。

进入函数后，首先对上一步计算的平面法向量进行了一个遍历，根据其在三个方向上的值，确定应该放在3d直方图的哪个位置。

之后遍历3d直方图中的每个单元，为每个向量找出与之在同一个单元里面的其它向量，这里根据注释，其代码应该是出于这个思路，但是在遍历的过程中，也就是那三层的if分支，这三层表示的意思貌似不是找同一个单元，看起来更像是相邻位置不远的两个向量就可以被放在一起。

暂且不管这些问题，完成分配之后，遍历落入同一个单元里面的所有向量，如果向量的个数超过了传入函数的最小阈值，就将这个单元取出，存储这些向量到selected_vectors里面，同时还要单独存储一份相同网格内的向量到v_same里面，这里看代码更好理解一些，这两个vector其实就差一个元素。这么看的话，最后这个循环就是在过滤一些向量，根据其落入的直方图的位置，去掉一些比较孤零零的向量，从而让剩下的向量更加具有代表性。作者代码的写法是根据平面法向量在向量内部出现的顺序，比如说最早出现的是法向量A，那么就会寻找和A落入同一个网格内的其它法向量，假设找到了BC，那么会将BC用found向量进行一个标记，后续就不对BC进行计算，同时BC会放到存储结果A的位置上，所以这个写法实际上是存在了一个高低之分的，A我们可以看作是领主，BC是小弟。在下面根据数量进行过滤的部分，selected_vectors存储的是全部，而v_same存储的则是小弟。

那么接下来稍微总结一下整个提取曼哈顿坐标系的这个函数，所谓提取曼哈顿坐标系，本来以为是利用线段的方向进行统计的，没想到在这里是用点来实现的，简单来说就是两步：提取和筛选。computeNormalsLPVO是提取的函数，提取过程首先建立像素坐标系上的2d点和相机坐标系下的3d点之间的对应关系，之后遍历像素坐标系下每个非边界像素点，取出该像素上下左右的四个点，左右和上下两组分别找出在相机坐标系下的3d点并相减产生两个向量，也就是u方向和v方向各自产生一个向量，由于是3d点相减产生的向量，所以是一个三维的向量并且是每个点都会产生这么两个向量，对这三个维度分别做一个积分图，方便后面计算局部和。之后再次遍历，不过这次就是以网格为基本单位，按照函数里面设置的初始值，每个网格大小是10×10，每次移动的距离是15，也就是网格之间会有5像素的间隔。对于每个网格，统计内部所有点产生的向量，在三个方向上计算均值作为这个网格的向量值，也就是一个网格产生一个u方向一个v方向上的均值向量，这两个向量叉乘得到一个向量，该向量将作为这个网格的平面法向量进行后续计算。筛选的部分则是利用3d直方图，上一步我们利用网格得到了很多的平面法向量，一个网格都会产生一个平面法向量，对其进行直方图的分配，这里看代码比描述要容易的多，代码在进行直方图分配的过程中，采用的是有主次的策略，比如说按照向量的顺序，第一个落入这个网格的认为是网格的老大，我们找的是和老大在一个网格内的其它向量，并建立老大与其它向量之间的联系。这样网格分配完成，就需要根据网格内向量的数量来筛选，必须要多于阈值才可以保留，保留时代码还特地用了两个数据结构，selected_vectors保留了网格内的所有向量，而represent_vect保留了除了老大以外的向量。

上面三个线程的工作全部进行完成之后，回到Frame的构造函数（没错就是回到刚开始的位置），继续帧的构造的过程。如果成功提取到图像的特征，就可以继续进行帧的构造，在前几次进行帧的初始化时bManhInit设置为false，会进行单独的一段代码，在曼哈顿坐标系趋于稳定和准确之后，bManhInit就会转为true从而跳过这一段代码。这段单独的代码中，对每条线的表达式（其实就是所有线段的3d方向向量）进行了遍历，取出三个参数并进行了单独的存储。

帧初始化函数的最后，除了一些变量的初始化，还有一部分是开辟线程将提取到的点线特征用网格进行注册，开辟的两个线程分别对应AssignFeaturesToGrid和AssignFeaturesToGridForLine两个函数。点的注册就是根据点的位置，放入对应的网格。线的注册也类似，就是将线段经过的网格都增加线段的序号。

帧的初始化函数终于结束了，总结一下帧的初始化过程，相较于最原始的ORBSLAM2，增加了线段和曼哈顿坐标系的部分，线段使用LSD进行检测并用拟合3d点的方法得到其空间表示，曼哈顿坐标系则是使用了点作为基准来进行方向的提取，提取完成后将点线特征进行网格分配方便后续使用。主要的难点在于曼哈顿坐标系提取的位置，目前还没有看到坐标系提取出来用在了哪里，具体还是需要用到的时候再细看。

帧构造完成之后，遍历每一条提取出来的线段的3d表达式，对于合法的线段，调用computeStructConstrains函数进行结构化约束，也就是寻找和其他线段之间平行和垂直的关系。所谓结构化约束，实际上就是对于一条给定的线段，计算其它线段和给定线段的夹角，如果夹角符合一定的范围要求就进行标记。这里在计算夹角时，计算了两个，变量名上分别写了2d和3d。

同样都是用3d坐标写出来代码，但是标有2d的夹角的坐标来自于mvKeyLineFunctions，而标有3d的夹角坐标来自于mvLineEq。关于这两个向量的赋值，需要回到线段提取的部分，在线段提取重载括号的函数时，传入的最后一个参数就是mvKeyLineFunctions，而mvLineEq则是在isLineGood里面进行赋值的。我们先看mvKeyLineFunctions，这个向量是一个存放Eigen::Vector3d的向量，为其赋值时，首先取出归一化平面上的坐标，叉乘后单位化存入了向量，所以这个向量存放的应该是法向量或者垂线方向之类的东西。而mvLineEq则是在计算出线段的3d表达式之后，提取端点AB之后计算出来的，所以就直接是线段的空间单位方向向量。所以这里用两个方向进行鉴别，个人感觉是为了尽可能保证准确，如果两个方向的夹角都很小，就认为是平行，反之如果两个夹角都很大，就认为是垂直。从代码变量的命名也可以看出，当夹角的余弦值小于阈值，给Perp存储了，而垂直的英文为perpendicular，夹角余弦值大于阈值时，存入了Par，刚好与平行parallel的首字母一样。

线段结构化端点优化 LineOptStruct

在帧对象的初始化过程的最后，需要调用LineOptStruct函数对端点进行一个优化，这里其实对应了论文中提到的根据结构化约束进行端点的微调，进入函数后由于调用的是g2o库进行的优化，所以会初始化很多g2o的优化器之后，完成一些参量的初始化之后，遍历每一条线段，如果结构化约束的关系足够多，也就是与当前线段平行或者垂直的线段足够多，那么就认为这条线是可以进行优化的。对于这些可以优化的线段，如果其深度信息合法，则提取方向向量和端点信息，端点将会作为优化器优化的内容之一。

之后遍历所有与当前线段平行和垂直的其他线段，根据g2o库的写法为其添加约束关系，完成后进行优化器的优化，根据代码的注释，一共需要进行两次优化，每次优化之后都会检查内点和外点的数目。对于优化结果，还会进行一次检验，来排除其中效果不是很好的观测结果。

最终的优化结果将会覆盖给原来传入的线段信息中去，从而实现一个基于结构化约束的线段端点优化。回顾一下这个函数，这个函数对应的就是论文原文中的端点的优化，提取出来的所有线段，其实内部都存在平行和垂直的约束关系，但是根据提取的不准确性，这个平行和垂直其实会存在一定的偏差，当好多线都存在这种平行和垂直的关系的时候，就可以利用这个关系，通过调整端点的坐标，来让整体的关系更加准确，也就是利用平行垂直这种结构化的约束，来实现一个提取线段的端点的校正。

二、Tracking线程

跟踪模式 Track

在GrabImageRGBD函数的最后，完成了一系列的初始化和优化之后，函数终于调用Track函数进行了后续的处理。按照时间顺序，此时是对第一帧进行的处理，所以mState的状态应该为NOT_INITIALIZED，进入初始化的分支。由于补充了曼哈顿坐标系的相关内容，所以在初始化的时候，MSC-VO多了一步提取曼哈顿坐标系。无论曼哈顿坐标系提取是否成功，剩余内容的初始化都会放在坐标系的提取之后。

曼哈顿坐标系提取 ExtractCoarseManhAx

坐标系的提取主要依赖于ExtractCoarseManhAx函数，由于是Tracking.cc内部的函数，所以并不需要传递参数。进入这个函数后首先会遍历所有深度合法的线段，将其进行一个单独的保存方便后续的调用。曼哈顿坐标系的提取，依赖于Tracking对象初始化的时候创建的曼哈顿坐标系对象，需要调用曼哈顿坐标系的findCoordAxis函数，代码里面调用了两次，分别是对mCurrentFrame.mRepLines和mCurrentFrame.mRepNormals进行提取，这两个向量也是之前赋值过的，mRepLines存放的是所有提取出来的合法线段的方向向量，是在Frame对象初始化的时候进行的赋值，而mRepNormals存放的是前面在用网格计算向量方向的时候，存放了直方图内同一个bin里面但是不是bin里面的第一个的向量。这里按道理，使用mRepNormals和使用mvPtNormals是没区别的，提取曼哈顿坐标系需要寻找夹角足够大的两组向量做叉乘，这里使用全部的平面法向量是正确的，但是mRepNormals相比于mvPtNormals只是少了一个老大向量，不明白作者这里这样写的目的是什么。

提取主坐标轴 findCoordAxis

在findCoordAxis函数中，首先会遍历传入的所有线段，第二层的遍历其实根据传入的内容有不同的说法，findCoordAxis函数的调用进行了两次，分别是对mRepLines和mRepNormals进行的，前者存放的是所有线段的方向向量，所以在这一层遍历时其实只会执行一次，因为每条线只有一个方向向量，相当于遍历所有线段的方向向量，后者存放的相当于是由深度提供的一部分信息，这部分信息我们可以看作是空间的一个结构化信息，在遍历这部分信息时，数据结构虽然一样，但是其含义已经不一样了，这时的遍历相当于遍历3d直方图中每个网格内的结构化方向向量，由于一个网格内的向量数目并不一定只有一个，所以这里会进行遍历而不是像mRepLines那样只遍历一次。

当传入为mRepLines时，findCoordAxis函数遍历每条空间线段的方向向量，计算出其2d表示并进行存储。关于这个投影的计算过程，最好的理解方法就是画个图，首先rep_lines[i][j]可以确定是空间向量的一条单位方向向量，这里计算的lambda，表示的是该向量在xoy平面上投影的长度，也可以看作是三角形的底边，而竖向上的边则是z的绝对值，这样tan_alfa表示的就是该方向向量与z轴产生夹角的正切值，而alfa表示的就是这个角度的度数，这里使用asin函数时传入的是lambda，是因为这里省略了除以1，方向向量经过了单位化，所以斜边长度是1，lambda除以1表示的就是alfa的正弦。这里的计算过程，个人猜测是在将方向向量转换为单位球上的方向向量，也就是图中的normal vector of the great circles，如果不进行这一步的话，后面叉乘的结果是解释不通的。

对于计算出的2d表达式，利用MeanShift函数进行平均偏移量的计算，虽然代码里面叫做平均偏移量，首先利用cv::norm函数计算其二维范数，并在此基础上计算一个参数k，每个表达式的对应位置都乘以k并进行求和，最后取平均，同时所有的参数k也会进行一次取平均。当传入为mRepNormals时，本身变化不大，只不过对每个平面法向量都进行了一次检验并计算2d表达式，其余的部分并没有区别。此时meanshift函数相当于将一组平面法向量整合为了一个，这个函数传入的是一组向量，只不过当传入的是方向向量时，这一组向量里只有一个，但传入的是平面法向量时，就变成了多对一的关系，相当于对落入同一个3d直方图网格的平面法向量进行了整合。

回到findCoordAxis，MeanShift函数的计算结果分别存放于s_j和density中，利用这两个结果重新映射回3d空间，单独存放校正后的结果，函数的后续都将依赖于校正后的结果。

之后遍历所有线段的校正结果，利用两次循环实现一个线段组合的遍历，当组合的两条线段夹角小于阈值时，取出两条线段的方向向量并进行单位化，之后叉乘计算得到两个单位方向向量的外积，存储后调用cv::SVDecomp函数进行奇异值分解，计算结果进行单独存储。这里并没有看懂是什么意思，组成的s_coord_axis矩阵本身是一个方阵，所以调用奇异值分解的函数应该结果和特征值分解的结果是一样的，所以计算的结果也就是U和VT应该是特征向量组成的矩阵，只不过VT进行了一次转置，二者相乘的结果应该如何解释就成了个问题。

回到提取曼哈顿坐标系的函数，在计算出坐标系的向量之后，用拷贝的方法初始化了一个v_lines_n_normals向量，并在其中加入了参与计算的方向向量和平面法向量，在v_lines_n_normals_cand中放入了上一步计算出来的待检测的坐标系。这里就是将提取坐标系那一步传入的两个量合并到了一个向量中，并将计算结果也进行了合并。

提取坐标系 extractCoarseManhAxes

一切准备工作都完成后，就调用extractCoarseManhAxes进行曼哈顿坐标系的提取，需要传入上一步中计算的和提取的坐标系以及方向向量和平面法向量。进入函数后，如果方向向量和线段的和少于20个，认为无法提取直接返回false。之后遍历所有的计算结果，对于每一个坐标系，进行一个迭代50次的计算，每次计算中，用初始化曼哈顿坐标系对象时赋值的mvAxis进行计算，这个mvAxis本身应该是一个排列顺序，可能是用来确定坐标轴的一个排列顺序。

利用mvAxis中存储的顺序和cv::hconcat函数进行坐标系的一个重新排列，hconcat函数负责水平的拼接，结果存放在r_mat中。

之后将所有的线段的方向向量投影到当前的这个坐标系中，通过projectManhAxis函数进行实现，这个函数不展开说了，主要就是对于每条线和深度向量，计算其在给定坐标系下的投影结果，如果结果的偏差小于阈值，就进行存储，存储的内容是一个二维的向量。当这个存储结果的数量超过阈值时，就利用这些值计算一个平均的偏移，每次计算都会进行一次校正以及次数的统计，对于一轮计算，如果三个顺序中成功计算的次数少于2，也就是说只要有一次计算失败，就认为当前的这个坐标系是不行的，直接跳出循环继续检验下一个坐标系。而当成功进行三次计算时，就将计算的三个坐标系进行一个存储，同时更新坐标系，这样经过50轮的更新，将最后的坐标系进行存储。当所有的坐标系都进行一次计算之后，调用RemoveRedundancyMF2函数进行剔除，剔除的结果再经过clusterMMF的提取，最终从提取结果中选择最优的那个作为提取出来的曼哈顿坐标系。

回到ExtractCoarseManhAx函数，如果成功提取且提取的准确率能够大于95%，就认为在初始化阶段提取到了合适的曼哈顿坐标系，将计算结果进行存储即可返回。

双目RGBD初始化 StereoInitialization

回到Track函数，成功提取曼哈顿坐标系，会将mCoarseManhInit标记为true，之后进入正常的点线特征初始化StereoInitialization，由于是在ORBSLAM的基础上进行的改进，所以这里还保留了单目的初始化函数，但这里并不会去使用。原本ORBSLAM中只使用了点特征，所以要求特征数目N要大于50个，而在引入了线特征之后，初始化的部分要求点特征数量N和线特征数量NL加起来超过100才可以进行初始化。

初始化的过程中会将当前帧设置为关键帧并进行存储，之后遍历当前帧提取到的特征点，如果深度合法，则将其直接作为地图点进行存储。

对于线特征也是一样，遍历时根据起点和深度信息来确定线段的合法性，如果线段合法，则直接在地图中插入这条线段，线段的3d信息提取是在isLineGood函数中实现的，通过对线段进行采样，利用采样点的深度恢复采样点的3d坐标，再利用3d采样点进行ransac拟合，其3d的端点本身就是2d线段提取结果投影得到的深度信息。

完成一些参数的设置后，初始化完成，将mState设置为OK表示正常初始化。当再次传来第二帧时，由于已经完成了初始化，所以mState已经变为OK了，此时进入另一个分支，开始进行帧的跟踪。此时如果曼哈顿坐标系还没有提取成功，会再进行一次提取，如果完成了提取，此时会进行一次坐标系的校正。

此处的mManhInit在track对象初始化的时候默认设置为false，而mCoarseManhInit在成功提取到曼哈顿坐标系的时候会修改为true，所以会进入这个分支，此处调用了optManhInitAvailable函数，这里mAvailableOptManh默认设置为false，只会在计算出曼哈顿坐标系的四帧之后变为true，表示可以进行坐标系的校正。

按照正常的SLAM设置，建图与定位是都需要完成的，所以mbOnlyTracking会设置为false，也就是进入上面的分支，此时按照执行顺序，会进入CheckReplacedInLastFrame函数，该函数会遍历上一帧的所有点线特征，检查特征在局部地图中是否发生了变化，如果变化了，则会对地图点进行一个更新，线特征同理。

更新完发生变化的特征之后，就会按照正常ORBSLAM的跟踪策略进行跟踪，如果能够计算出速度或者刚刚进行完重定位，就利用参考关键帧模型进行跟踪，如果速度能够使用，就利用恒速模型进行跟踪，跟踪失效也就是mState不是OK的时候，就换用重定位。之后扩展跟踪的范围，对局部地图也进行一个跟踪。

三种跟踪模型

恒速模型 TrackWithMotionModel

在恒速模型中，不仅仅是点特征会参与跟踪，线特征同样也会参与跟踪，根据作者留的注释，这里貌似准备了两种线段跟踪的策略，首先会对点和线进行跟踪，当跟踪成功的数量较少时，ORBSLAM采用的方法是扩大搜索半径，在MSC-VO中，采用的策略是当跟踪成功的点的数目和线的数目的和少于20的时候，认为跟踪不够好，点采用扩大搜索半径的方法再次跟踪，线则更换另一种策略进行跟踪。

线段跟踪采用了两种策略，在第一次尝试跟踪时，使用的是完全依赖于描述子的策略，对应的函数是SearchByGeomNApearance。这个函数利用线段的描述子产生一个DMatch的匹配结果，对于描述子的匹配结果，通过旋转角、位置等几何信息进行一次校正，相当于是暴力匹配然后筛选。当成功匹配的数目太少时，就换用第二种策略，对应函数是SearchByProjection，这种跟踪策略会复杂一些，对于上一帧中每一条合法的线段，调用isInFrustum函数，该函数属于帧对象的一个成员函数，在Frame.cc下面，函数会提取传入线段在世界坐标系下的两个端点，投影到相机坐标系下后，计算其在成像平面的像素坐标，如果合法则进行保存。所以在这里其实是计算了地图线在当前帧上的投影，之后根据这个投影的位置，调用GetFeaturesInAreaForLine函数在投影位置的相邻位置进行搜索，搜索时同时还考虑了尺度问题，会在上下两层进行搜索，这里和ORBSLAM的跟踪搜索策略是很像的。

线段跟踪搜索 GetFeaturesInAreaForLine

展开看一下这里的线段搜索函数，函数传入了线段端点的投影位置的坐标、搜索半径、搜索层数范围以及一个阈值。搜索的过程中利用传入的两个端点和线段中点进行搜索，对于这三个点，计算其落入的网格位置，在这个位置的基础上，根据搜索半径得到邻域，利用网格取出经过的线段，对于这些线段，利用单位方向向量的夹角和点到直线的距离来检测，如果小于阈值则认为是跟踪得到的候选匹配。

对于提取出来的候选跟踪线段，会再次进行一次检验，检测的内容包括描述子距离、2d夹角，2d夹角小于阈值且最优次优比例足够大才认为是真正的跟踪匹配。

回到恒速跟踪模型的函数，采用扩大搜索对点线特征进行跟踪之后，如果点跟踪数量少于20且线跟踪数量少于5则认为跟踪失败，对于跟踪成功的情况，调用PoseOptimization函数进行校正，之后利用矫正结果，对点线特征进行一次过滤，如果此时依然符合则返回true。

恒速跟踪模式下的位姿优化 PoseOptimization

优化的这部分使用的是g2o库进行的优化，因为不是很熟悉这个库的写法，所以这里只能看个大概，从注释看的话，优化的这部分函数是将地图点和地图线两部分利用g2o库的语法，添加到求解器中，线在添加时，使用的是起点和终点，并且在跟踪的优化过程中，曼哈顿坐标系的相关内容并没有加入求解，单纯用了点线特征。

参考关键帧模型 TrackReferenceKeyFrame

参考关键帧模型的跟踪本身和恒速模型差别不大，主要是在速度无效或者恒速模型失效的情况下，将相邻帧的跟踪换为当前帧与上一个关键帧之间的跟踪，对于点特征，ORBSLAM使用了词袋模型进行加速，MSC-VO对点的匹配并没有进行太多的修改，依然是利用词袋模型加速点匹配。重点在于线特征的部分，这里函数直接调用了LSDmatcher的match函数，传入两帧图像线的描述子以及一个阈值，函数内转而跳转到matchNNR函数，这个函数中利用OPENCV自带的匹配，对描述子进行了knn匹配，并使用最优次优的检测方法对匹配结果进行检测，如果符合阈值则进行存储。

对于匹配结果，如果深度信息等内容检测合法，则利用位置和角度进行筛选，只有这些内容都符合要求，才会认为真正匹配。对于关键帧的跟踪模型，函数认为如果点匹配小于15或者线匹配小于15就是跟踪失败。对于成功跟踪的情况，调用PoseOptimization进行位姿的优化，同时根据优化结果，丢弃已经成为外点的点特征和线特征。

重定位模型 Relocalization

当恒速模型和参考关键帧模型都失效的时候，就转而进行重定位模型。在重定位的函数中，首先对当前帧计算词袋模型，利用DetectRelocalizationCandidates函数根据词袋模型得到与当前帧相似的候选关键帧，之后遍历候选关键帧，调用SearchByBoW函数进行词袋的特征匹配，如果匹配的点特征多于15个，就利用当前帧的信息来初始化一个PnPsolver。

对于这些匹配足够多的候选关键帧，利用之前初始化好的PnPsolver进行一次位姿的计算，并利用计算出来的位姿进行校正，具体来说，校正的过程只针对于点特征，如果匹配优秀的特征点不够，就再次进行优化，最后必须满足优秀匹配多于50个才可以认为重定位成功。

由于重定位这里并没有使用线特征，所以本质上还是ORBSLAM那一套，并没有什么太大的改动。也就是对于MSC-VO的三种跟踪模型，重定位我们可以认为和ORBSLAM没有区别，仅仅使用了特征点进行重定位；在参考关键帧模型中，在点的基础上加入了基于线段描述子的参考关键帧跟踪，利用基于knn方法进行匹配，同时利用夹角、在图中的位置等信息进行筛选；而在最重要的恒速模型中，线特征的参与则主要体现在线段的跟踪，跟踪过程首先用第一种策略进行跟踪，利用描述子进行暴力匹配，之后再利用旋转和图中的位置进行筛选，当第一种策略得到的跟踪效果不太好时，对于点特征，此时会扩大搜索半径进行第二次跟踪，对于线特征，这里换用了第二种策略，利用地图线端点的投影结果进行邻域的跟踪，通过在两个端点和中点的附近搜索候选匹配线，之后通过旋转角度、描述子距离、最优次优匹配进行筛选。

局部地图跟踪 TrackLocalMapWithLines

利用三种模型进行跟踪之后，还需要对局部地图进行一次跟踪，以此来补充特征信息的减少。这部分的代码主要在TrackLocalMapWithLines中，进入函数后首先开辟两个线程，分别执行SearchLocalPoints和SearchLocalLines，这两个函数负责对局部地图的特征进行匹配。

局部地图点匹配 SearchLocalPoints

该函数首先对地图点进行一个过滤，如果当前的点已经有与之匹配的地图点就标记mnLastFrameSeen为当前帧的id，从而在后续检测中不进行检测。

对于剩下的地图点，利用投影检测其投影点是否会落在相机的成像范围内，如果能够成功落在图像上，则统计这部分点，最后对这部分点调用SearchByProjection函数，将投影点与图像上的特征点尝试进行匹配，利用描述子距离和最优次优策略进行暴力匹配。

局部地图线匹配 SearchLocalLines

局部地图线的匹配基本流程与地图点匹配类似，先过滤已经匹配的地图线，再判断线是否在成像范围内，对于线段的判断，个人感觉不太合理，判断过程是利用端点进行投影，如果端点或者中点投影落到了成像范围外就会返回错误，可问题在于，线段在帧之间会产生延伸，也就是地图线端点虽然投影到了外面，并不代表这条线就投影不到图像上，这里稍微有一点漏洞。

对于候选的匹配地图线，根据描述子距离和最优次优策略对区域内的线进行匹配，这里描述子距离的计算是依赖于当前帧上的线和地图线的描述子，地图线的描述子是根据创建时的2d线段描述子进行设置的。

对局部地图进行特征跟踪之后，终于用到了曼哈顿对象进行处理，这里利用computeStructConstInMap函数，传入当前帧和能够投影到当前帧的地图线的集合，对于当前帧上的每一条线，计算其与每一条地图线的夹角，利用这个夹角来判断线段与地图线是垂直还是平行，将对应的地图线分别存放，用于后续的校正。

完成这三部分之后，在TrackLocalMapWithLines函数中调用PoseOptimization再进行一次优化，并将新的信息进行更新，更新之后如果匹配的特征依然足够多，则返回true，反之返回false，认为跟踪丢失转而进行重定位。

关键帧判断与生成

局部地图跟踪成功之后，回到Track函数之后进行更新并判断是否需要插入新的关键帧，在调用NeedNewKeyFrame函数之前，MSC-VO还进行了一系列其它操作。当已经成功提取到曼哈顿坐标系时，首先是将当前帧的位姿中的旋转矩阵提取出来，调用LineManhAxisCorresp函数，该函数遍历当前帧线段的方向向量，计算其与曼哈顿坐标系三个轴的夹角，当符合阈值时为线段分配一个轴，这里猜测可能是为了后面优化的时候，用线段和坐标轴的夹角建立一个约束。

新关键帧的判断 NeedNewKeyFrame

更新恒速模型的速度信息并清空当前帧的观测信息，之后才调用NeedNewKeyFrame来检测是不是需要新建一个关键帧，这里在检测时也参考了线特征，首先分别统计跟踪失败的点和线的比例，二者比例都大于0.6时，将bNeedToInsertClosePtsLine标记为true，这个条件会被整合入第二组条件进行检验。

c1a表示很长时间没有插入新的关键帧，c1b表示满足插入关键帧的最小间隔，c1c表示在传感器为RBGD或者双目相机的情况下，当前帧与地图点线特征匹配数目非常少且跟踪失败的点线很多，c2表示与参考关键帧相比当前跟踪到的点线数量太少同时跟踪成功的内点也不能太少。如果符合条件，则认为可以作为新的关键帧进行插入。

新关键帧的生成 CreateNewKeyFrame

当判断需要生成一个新的关键帧时，进入CreateNewKeyFrame函数，首先会利用当前帧初始化一个关键帧对象，对于特征点，根据深度顺序筛选不超过100个地图点，对于线特征，则是根据端点的深度二选一，根据二者中的较大者作为这条线的深度，采用同样的道理筛选出不超过100条地图线。

Tracking线程总结

进入tracking线程之后，准确来说是进入到Tracking.cc文件的GrabImageRGBD函数，我们认为这算是tracking线程的开始，首先初始化一个帧对象，在这个初始化的过程中进行三步操作：提取特征点（ExtractORBDepth）、提取线段（ExtractLSD）、提取平面法向量（ExtractMainImgPtNormals）。提取点线特征的过程主要是在2d图像上提取以及与深度图的对齐，而平面法向量则属于提取曼哈顿坐标系的第一步，这里会计算平面法向量并使用3d直方图进行去噪。
完成初始化后，进入到track函数正式进行跟踪，在没有初始化的时候，会用第一帧先进行初始化，包括点和线的初始化以及曼哈顿坐标系的提取。提取曼哈顿坐标系的过程首先需要用当前帧线段的方向向量和平面法向量，这些向量经过mean shift校正和叉乘求解消失方向（VD），从而组合成待检测的曼哈顿坐标系，之后对这些坐标系进行检验，根据投影来选择最优的曼哈顿坐标系。点线的初始化则是将已经重建好的3d坐标添加到地图中。
完成初始化之后，则会在过后几帧进行曼哈顿坐标系的更新，准确的曼哈顿坐标系被提取出来后，会被赋值给地图对象。之后利用三种模型进行跟踪。恒速模型对线有两种跟踪方法，第一次跟踪时使用全局的knn匹配，利用位置和角度进行筛选，当跟踪成功的数量较少时，进行一个补充，利用上一帧线段在地图中的3d线段，向当前帧投影，投影位置的附近寻找邻近线，利用描述子和最优次优进行筛选。参考关键帧模型利用词袋模型对点的匹配进行加速，线的匹配使用了LBD快速匹配，同时使用角度和位置进行筛选。重定位模型则与ORBSLAM没有太大区别，并没有线特征的参与。
跟踪成功的情况下，再进行一次与局部地图的跟踪，通过地图中的点线特征投影，再次进行一个补充性质的匹配。最后根据跟踪情况和关键帧出现情况，判断是否需要插入新的关键帧，如果需要，创建关键帧对象并补充新的地图点线信息，同时统治局部建图线程进行建图的优化。

三、LocalMapping线程

与ORBSLAM相同，MSC-VO也使用LocalMapping线程进行建图，但由于是VO，所以没有回环检测的部分，虽然没有做，但是在初始化的时候还是给回环检测开了一个线程。这里进入局部建图线程是从System对象初始化的时候开始的，这里开辟一个线程运行Run函数，在这个函数中会循环检测，当有新的关键帧到来，也就是经过Tracking线程处理后认为需要添加新的关键帧时，就会进行一次处理。

处理新关键帧 ProcessNewKeyFrame

当有新的关键帧传入的时候，也就是CheckNewKeyFrames函数返回true的时候，函数首先执行ProcessNewKeyFrame函数，该函数先对当前帧进行词袋模型的计算，对于当前关键帧的已经与地图点匹配的，为地图点添加新的观测信息，对于创建关键帧过程中新生成的地图点，存放入mlpRecentAddedMapPoints向量，等待后续的多次出现检验才能够正式被存入。对于不是来自于当前关键帧的特征点，比如来自于局部地图的观测结果，将这些地图点进行补充同时更新描述子。这部分点的处理和ORBSLAM是一样的，而对于线特征，MSC-VO也采用了同样的方法，需要注意的是，在处理新关键帧时不管是点特征还是线特征，都需要一个最优描述子的更新过程，这个过程对应的是ComputeDistinctiveDescriptors函数，该函数总结所有的地图点线的已有的观测结果，对这些结果计算一个最优的描述子，作为当前地图特征的描述子。

对于处理好的新关键帧，调用UpdateConnections函数更新共视图，并向地图向量中插入新的关键帧。这里简单提一下，在ORBSLAM中，共视图完全是依赖于点特征，在增加了线特征之后，共视图也需要体现线特征的共视关系，其写法与点特征的是一样的，对于一个特征，先取出所有的已经存在共视关系的关键帧，对这些关键帧，统计其数目，只有超过15关键帧存在共视关系才能在共视图中添加一条边。

点线特征剔除 MapPointCulling MapLineCulling

这两个函数负责对观测不好的地图点线特征进行剔除，在程序中通过开辟两个线程的方法进行调用。函数的逻辑是相同的，都是对最近新添加的地图特征进行遍历，如果跟踪到该地图特征的帧数相比预计可观测到该地图点的帧数的比例小于25%，从地图中删除这个特征信息，如果该地图信息从建立到当前帧已经过了很久但观测到的帧的数量确不够多，从地图中删除，当从建立该地图信息至今连续三帧能够观测到，认为是好的地图信息，从最近添加的队列中删除，作为稳定的特征信息进行存储。

考虑到点与线的一些几何上的区别，在剔除函数中，阈值上会稍有区别，点特征要求从第一次创建至今大于等于2帧且观测少于3认为是不好的点，而线特征要求从第一次创建至今大于等于7帧且被观测小于等于3认为是不好的线。此外在保存时也有区别，点特征连续三帧观测就可以保存，而线特征要连续6帧观测才可以。

地图点线创建 CreateNewMapPoints CreateNewMapLinesConstraint

地图点的创建和ORBSLAM基本没区别，因为之前没看过RGBD相机的地图点创建，这里也简单看一下。进入CreateNewMapPoints函数后，首先调用GetBestCovisibilityKeyFrames函数，根据传感器的类型，获得共视关系比较好的共视关键帧，在RGBD相机的情况下是取共视程度最高的前十个共视关键帧，之后遍历每一个共视关键帧，对于当前关键帧和取出的共视关键帧，检验基线长度与两帧位移上的距离，如果基线长度过短则认为这个共视关键帧不会产生十分准确的地图点，这里有一点疑问，基线这个东西按道理是针对于双目相机的，双目相机的基线会影响深度估计的准确进而影响地图点的准确，但是RGBD的深度估计原理和双目是不一样的，不会受到基线的影响。这里对于满足基线约束的共视关键帧，计算视差角并以此选择哪一个关键帧的深度更加准确，选用更准确的那个深度来构建地图点。

而对于地图线的创建，MSC-VO也采用了共视图的筛选策略，筛选出10个共视关系最强的共视关键帧，之后遍历这些关键帧，利用基线进行一次检验，不同于地图点的创建，对于符合基线约束的关键帧，利用SearchForTriangulation函数进行一次基于描述子的暴力匹配，利用双向检验进行校正，匹配的结果将进行单独的存储。

之后再遍历所有的匹配结果，每次取出当前帧和两个共视关键帧的匹配结果，对于当前帧的线段，如果在这两个共视关键帧上存在匹配关系且与之对应的匹配结果还没有插入到地图中，就需要进行地图线的添加，添加地图线的代码居然有400行，简直离谱，简单来说就是对着两条共视线和一条自身的线，用了大量几何的方法进行筛选，在添加的时候端点是依赖于三条线坐标产生的，这里也看不太懂。

完成新的地图元素插入之后，程序回到run函数的循环中，当曼哈顿坐标系已经成功提取并且距离当前关键帧已经有足够的帧的间隔，就对曼哈顿坐标系进行一次优化，这里的间隔要求当前帧与第一次成功提取曼哈顿坐标系的帧间隔要达到至少四帧，也就是至少四帧后的关键帧将进行优化，通过MultiViewManhInit函数进行优化。

曼哈顿坐标系的优化 MultiViewManhInit

优化曼哈顿坐标系的代码也很长，好在作者留了几句注释，进入函数后，首先会将当前关键帧以及共视关键帧，这些关键帧被统称为局部关键帧（lLocalKeyFrames），遍历局部关键帧，取出局部关键帧上已经添加到地图中的地图线，将其进行单独存储，记为局部地图线（lLocalMapLines）。对于这些局部地图线，根据其观测结果，找出线在局部地图线中但是关键帧不在局部关键帧中的关键帧，这部分关键帧在后面的BA优化中需要进行固定。简单来说，这一步就是在根据共视图去固定一部分关键帧，根据当前帧及其共视帧找出一部分地图线，再顺着地图线找出更多的关键帧，新找出来的关键帧需要保持固定，不参与优化。

之后初始化g2o库的相关内容，因为看不懂g2o的相关内容，这后面的部分就简单写一下。g2o的写法本身就是不断添加约束关系以及待优化量，在优化器完成初始化之后，可以通过搜索addVertex来看向其中添加了什么。顺着函数执行顺序可以看到，局部关键帧被加入了进入，同时顺着地图先找出的关键帧也加入了进去，但是利用setFixed函数进行了固定。

除此之外，曼哈顿坐标系作为主角自然也会被加入进去。

addVertex加入的是图优化中的点，边的添加则是利用addEdge。边的添加则是利用局部地图线，地图线之间的平行垂直关系以及与曼哈顿坐标系的平行垂直关系都加入图优化，这些将作为优化过程的约束。调用优化函数之后将优化后的坐标系写回给传入的曼哈顿坐标系。回到主函数后mAvailableOptManh会被修改为true，从而不会再进行第二次的坐标系优化。

局部地图优化 LocalMapOptimization

再次回到局部建图的函数内，当地图内关键帧已经有至少三个时，就调用LocalMapOptimization函数进行一次包含点线特征以及曼哈顿坐标系的局部BA。优化的过程和优化曼哈顿坐标系有点像，依然是利用共视关系寻找局部关键帧，利用局部关键帧提取出地图上的点线特征，利用局部地图点线再找出需要固定的内容。前面的内容其实和优化曼哈顿坐标系的内容类似，只不过是将线扩展到点和线共同纳入优化的范围。

完成g2o对象的一些初始化之后，就开始向图优化中添加元素。和优化曼哈顿坐标系类似，这里也是先将局部关键帧和延伸的关键帧作为点加入到图优化中，对延伸关键帧设置固定。之后添加地图点和地图线，这里也是简单记录，地图点并没有使用曼哈顿坐标系进行约束，而是像ORBSLAM那样直接添加并进行优化。而对于地图线的优化，则使用了曼哈顿坐标系进行约束，也就是不仅要保持地图线段之间的垂直平行关系，还要保证线段与坐标系之间的平行垂直关系，这里也刚好和论文中使用的优化公式相对应。

关键帧剔除 KeyFrameCulling

对局部地图进行优化之后，返回到主函数按照顺序继续执行，进入到关键帧的剔除部分，这一部分是利用共视关系得到共视关键帧，对于每个共视关键帧，提取出其地图点，对于每个合法地图点进行检测，其中能被其它至少3个关键帧观测到的地图点为冗余地图点，如果该关键帧90%以上的有效地图点被判断为冗余的，则认为该关键帧是冗余的，需要删除该关键帧。这里并没有加入线特征相关的内容，完全就是ORBSLAM的那一套。

执行到这里，局部建图线程基本就算结束了，由于没有回环检测的内容，所以MSC-VO基本上也就在这里结束了。再回看整个局部建图线程，其实关于线段和曼哈顿坐标系的内容并没有想象的多，主要是因为这部分的内容都用在了优化器相关的代码中。每当从跟踪线程产生一个新的关键帧，局部建图线程就会开始工作，首先使用ProcessNewKeyFrame函数进行新插入关键帧的预处理，对新加入的点线特征，开辟两个线程进行地图特征的剔除，之后同样是用两个线程，分别进行新的地图点、线元素的创建，之后尝试对曼哈顿坐标系进行优化，这个优化是只进行一次的，当曼哈顿坐标系优化成功后会用于在局部BA中对线段进行一定的约束，从而优化出更加准确的位姿和地图信息。

四、总结

现在回头看一下整个MSC-VO，简单来说这个框架就是在ORBSLAM的基础上，加了线的部分和曼哈顿坐标系的部分，同时减去了回环检测的部分。线特征使用LSD进行提取，并利用深度图拟合空间直线的方法进行第三维度的重建，由于使用的是RGBD相机，所以初始化只需要一帧即可，后续跟踪则在原来ORBSLAM的基础上进行了线段的跟踪，恒速模型部分采用了两种线段匹配策略，分别对应一次就可以跟踪到足够多数目以及补充跟踪两种情况，参考关键帧模型部分则使用基于LBD描述子的knn匹配，而重定位的过程则没有使用线特征。线特征在曼哈顿坐标系的提取过程也起着很大的用途，这个提取过程依赖于深度图计算出来的平面法向量以及线段的方向向量，利用叉乘和SVD提取出的坐标系经过检验最后提取出来，这个初步的曼哈顿坐标系还会在后面进行一次优化，优化之后才能真正用于局部建图线程，优化后的曼哈顿坐标系也会作为局部BA的一部分参与到优化中，基于优化过程更多的约束。