ORBSLAM代码阅读

一、初始化部分

对于单目相机，查看CMakeList可以看到，其对应的可执行文件是mono_tum.cc，进入到这个文件可以看出，这个程序相当于是单目相机情况下的总控制，负责读取数据集、配置文件等内容，这个文件最重要的一句代码就是下面这句：

这句代码实例化了一个System对象，这个对象叫做SLAM，这个System对象实际上可以看作将ORBSLAM三大线程串在一起的枢纽，在这里初始化这个对象，后续就只需要按时间顺序，将每一帧图像读取之后送到这个对象中就可以了，对于单目相机来说，就是调用TrackMonocular这个函数，传入图像和时间戳，上下两句代码只是为了打印一帧的执行时间：

在进入TrackMonocular函数之前，我们先看一下System对象的构造函数，这个构造函数写的巨大无比，简单点来说就是初始化了整个SLAM过程中用得到的很多个对象，最重要的就是三大线程对象：

除此之外还包括一些SLAM过程中存储关键内容的容器，比如存储关键帧的容器mpKeyFrameDatabase。反正这个函数就当作初始化容器的大函数就好。里面比较有意思的是函数最后的这一段：

这段函数主要是设置进程之间的指针，因为三大线程相当于是同时运行的，在运行过程中，免不了会遇见彼此之间的协作或者说相互通信，这种情况就需要用指针操作，而指针的指向关系就是在这一步里面建立的。

回到原来的TrackMonocular函数，进入到这个函数，相当于开始了ORBSLAM的流程，顺着这个函数，首先会到达System.cc里的TrackMonocular函数，在这里首先是一大堆互斥锁的操作，这堆操作可以划分为两个锁的内容，第一个锁看不太懂，应该是在对线程做简化，第二个锁应该是在检查是否进入了重新初始化的状态，也就是三大策略都跟踪失败时进入的重新初始化状态。
这两个锁的内容之后，是这个函数真正关键的地方，也就是位姿的估计，这里首先是调用了mpTracker对象的GrabImageMonocular函数，用来获得相机位姿的估计结果，之后获取锁，保存当前帧的估计情况：

暂且不展开这里的函数，我们按照这个函数的流程，将计算的相机位姿返回，结果回到的应该是mono_tum里面的main函数，但这里在调用时并没有一个数据量来接收这个位姿，这里写法上可能有些问题。回到main函数之后，主函数剩下的内容其实就没什么可说的了，这说明所谓的三大线程，和主函数的联系其实就是通过Tracking连接的，在剩下的部分中，有点说头的就是加载下一张图像的部分，图像的加载实际上是通过for循环实现的，而为了保证每一帧图像到来的时间间隔的稳定，ORBSLAM在这里使用了下面的写法，让循环等待一会再进入下一次循环，从而保证时间间隔的稳定。

那么目前，主函数的基本执行顺序就理清了，无外乎初始化一个System对象，然后按照一定的时间间隔向里面传送图像。接下来我们就按照顺序开始理顺Tracking线程，回到System.cc的TrackMonocular函数中，最开始的一步是调用mpTracker的GrabImageMonocular函数去进行跟踪，我们就从这里开始。
GrabImageMonocular函数在Tracking.cc里面，传入函数的参数是当前帧的图像和一个时间戳，进入这个函数后首先是对传入图像的处理，也就是将彩色图像转换为灰度图像，之后会将当前帧的内容构造为一个帧，在这里的代码实现上是用了一个分支，判断是不是初始化，无论是否进行初始化，都会构造一个帧对象，区别在于提取特征点数目，初始化初始的帧对象提取的数目要多：

这段代码中ORB特征提取器其实是一个对象，mpIniORBextractor和mpORBextractorLeft只有一个区别，就是ORB特征点提取的数目，这两个对象是在Tracking对象初始化的时候进行的，也就是在System对象的构造函数里，对应就是下面这段代码，其中nFeatures就是提取特征点的数目，可以看见在初始化两个特征提取器对象的时候，第一个参数差了两倍，这一点对应的刚好就是特征点提取的数目。

在初始化两个帧对象之后，就会调用Frame的构造函数，而在GrabImageMonocular函数的后面，就只有一个调用Track进行跟踪，所以关于帧的特征提取以及初始化等内容，应该都是在Frame的构造函数里面实现的，所以这里我们展开构造函数看一下内部实现。
构造Frame的过程中，首先是对一些关键内容的赋值，之后利用传入的内容，计算图像金字塔的一些参数，图像金字塔在之前看EDLine的时候记录过，这里不多介绍了。完成这些小参数的初始化之后，就可以提取ORB特征点，调用同一个类内部的ExtractORB函数，在这个函数中，出现了一个极其反人类的写法，也就是重载了（），不知道作者为什么在这里要用炫技的写法，反正看着很别扭，总之在这里，对于单目相机，就通过mpORBextractorLeft对象进行特征提取，提取的细节在ORBextractor.cc里面，ComputePyramid函数负责构建图像金字塔，ComputeKeyPointsOctTree函数进行特征点的提取以及均匀化，这里提取特征点并没有直接使用OPENCV的ORB提取器，而是用了FAST提取器，关于ORB的实现是自己单独写的。提取好特征点之后计算描述子，具体实现在computeDescriptors函数里，这里用了一个很暴力的写法，因为ORB描述子使用了brief描述子，也就是随机选取一定范围内的像素，将灰度值大小换为01从而构建描述子，所以选取方法需要提前确定，这里的选取方法写在了bit_pattern_31_里面。还有一点是在提取的时候涉及的旋转，因为ORB在提取时会将方向转到梯度方向，由于转像素很麻烦，所以这里采用的识将选取方法旋转，也就是旋转pattern。

回到Frame的构造函数，提取完ORB特征点之后，会利用OpenCV的矫正函数、内参对提取到的特征点进行矫正，矫正之后的三角化并没有在这里进行，主要是缺少三角化的第二帧图像，在这个构造函数中只定义了存储本帧地图点的数据结构，但是并没有向里面加入数据。

最后对于已经提取好的特征点，通过划分网格的方法，将特征点放入了图像网格中，这里使用的是AssignFeaturesToGrid函数，这个函数会遍历每个特征点，用PosInGrid函数来确定特征点落入网格的坐标，之后将特征点的id放入对应网格的数据结构中：

到这里Frame的构造函数执行完毕，在这个构造函数中，首先是完成一些参数的赋值，之后构建图像金字塔，之后对单目图像提取特征点并计算描述子，对特征点矫正之后，放入划分的网格。这个时候我们就得到了第一帧图像的特征点信息、特征点的描述子以及网格分布情况，由于没有第二帧，所以还没有进行三角化恢复3D信息。

再继续往回走，回到GrabImageMonocular函数，这时对于已经初始化好的第一帧，调用Track函数，进入到Track函数内部。track包含两部分：估计运动、跟踪局部地图，进入Track之后会先判断mState，这个量如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态，此时显然是没有运行过的状态，会在下面进入地图初始化的分支：

这里直接进入MonocularInitialization函数，进行单目的初始化。此时由于单目初始化器在Tacking对象初始化时赋值了一个空指针，所以这里进入创建单目初始器的分支，在这里必须保证初始帧的特征点数目多于100，否则会直接返回，在第一帧特征点数目足够多的情况下，会先用第一帧创建初始化的两帧的对象，也就是让第一帧先假装第二帧去进行初始化。

之后记录第一帧的所有关键点，利用第一帧去初始化一个Initializer对象，这里就与前面对上了，进入这个分支是mpInitializer为一个空指针，空指针是在Tracking对象初始化的时候在构造函数里赋了一个空指针，在这里我们得到了第一帧图像，所以可以用这帧图像去初始化构造器，所以在这里才会真正赋一个对象给mpInitializer。构造mpInitializer的过程其实没有很复杂，主要是赋值内参矩阵、特征点等内容，这段代码位于Initializer.cc里面：

之后第一次执行MonocularInitialization就结束了，因为没有第二帧图像，只能初始化第一帧的特征点，需要等待第二帧来才能三角化，回到Track函数，将信息传给绘图部分之后就相当于结束了第一帧的处理，返回到TrackMonocular，将第一帧的跟踪信息保存之后，返回到主函数。

接下来等来第二帧图像，按照前面的执行顺序，会再次来到Track函数里，这次还是走上次的分支，第一帧只会让mState从NO_IMAGES_YET换为NOT_INITIALIZED，只有初始化彻底完成，也就是第二帧图像也得到之后才会变为OK，按顺序第二帧图像进入MonocularInitialization里面，这时mpInitializer已经初始化，进入另一个分支，在这个分支中处理第二帧。对于第二帧，只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程，在这里如果第二帧特征点少于100个，就删除初始化对象，从新开始初始化的内容：

如果第二帧符合条件，就会初始化一个ORBmatcher对象，这个对象用于进行特征点的匹配，需要给的两个值是最佳的和次佳特征点评分的比值阈值以及是否检查特征点的方向，比值阈值主要是用来剔除误匹配，最优次优的差别足够大才可以接受最优匹配，而点的方向则规定了在检验匹配结果的时候，是否要依赖旋转直方图来筛除误匹配。之后利用这个对象搜索匹配结果：

这里的最后一个参数，也就是搜索窗口大小，指的是搜索邻域的范围，由于帧于帧之间的运动偏移不会太大，所以搜索时可以利用这种临近搜索的思路，减小搜索的时间开销。匹配的结果存放在 mvIniMatches里，匹配对的数目为nmatches，只有匹配对数目多余100才会继续初始化，否则按照初始化失败处理删除后重新开始。

如果匹配数量足够，那么就可以利用两帧匹配的图像进行初始化，这里就对应了论文中两种模型初始化的部分，通过H模型或F模型进行单目初始化，这部分的代码实现在Initializer.cc的Initialize函数里，这个函数里面主要是通过RANSAC，每次随机取8组匹配点放到vAvailableIndices里面，开两个线程分别运行FindHomography和FindFundamental，最后计算出两个矩阵H和F：

之后通过论文提到的打分机制，为计算的结果赋分并选择一个最优矩阵，利用矩阵还原第一帧和第二帧之间的旋转和平移，如果计算正确就会返回true。三角化的部分也是在这里实现的，三角化的结果存放在了里面。

在计算正确的情况下，返回到MonocularInitialization函数中的值就是true，表示八点法位姿变换计算成功，之后首先会利用计算结果，删除一些无法进行三角化的误差较大的匹配点对，并且利用计算结果建立相机坐标系初始情况和世界坐标系的联系。
在CreateInitialMapMonocular函数中进行了地图的初始化，首先会将初始化用的两帧全部作为关键帧存放进地图中，虽然这里可能会有些冗余，但是在关键帧删除的部分会优化。之后就会将三角化的点构造为一个地图点MapPoint并且为地图点添加一些属性，最后加入到地图中，这里对应的也就是ORBSLAM中地图点的第一个添加的时机。

全部地图点建立完成之后，还需要更新关键帧之间的连接关系，虽然这里只有两个关键帧，但是依然需要维护关键帧之间的连接关系，这里我们可以看作是在维护共视图、生成树等工具，这里会调用UpdateConnections函数来维护连接关系，之后调用GlobalBundleAdjustemnt函数进行全局BA优化，同时优化所有位姿和三维点。
优化完成之后，利用平均观测深度进行筛选，一个是平均深度要大于0,另外一个是在当前帧中被观测到的地图点的数目应该大于100。之后将两帧之间的变换归一化到平均深度1的尺度下，并将3D点的尺度也归一化到1。最后将关键帧插入局部地图，更新归一化后的位姿、局部地图点。初始化成功，至此，初始化过程完成。
最后对整个初始化过程做一个总结，在所有帧和计算都满足条件的情况下，对于传入的第一帧，提取特征并初始化一个初始化器对象，之后等待第二帧的传入，帧间特征点匹配之后，计算两个模型H和F矩阵，利用打分机制选择一个合适的矩阵并恢复帧间变换R和t并三角化恢复特征点，之后将特征点和两个关键帧插入到初始化的地图中，更新帧之间的连接关系后，对尺度做处理，从而完成整个初始化的过程。