ORBSLAM2

主进程system：system类的构造函数输入参数为：1、词典文件路径2、配置文件路径3、传感器类型4、是否使用可视化界面。

构造函数做的事情：1、输出传感器类型

2、检查配置文件是否有效

3、加载词典文件

4、使用词典文件创建关键帧数据库(用于回环检测和重定位)

5、创建地图

6、创建帧绘制器和地图绘制器，viewer会使用这两个绘制器

7、初始化tracking、localmapping、loopclosing线程，后两者初始化后需要调用各自的run函数

8、根据第四个参数判断是否创建viewer，即是否需要可视化：

9、分别给这三个线程设置指向另外两个线程的指针，方便线程通信

 

system类里的其他函数：双目输入时的追踪器接口：cv::Mat System::TrackStereo：确定传感器为双目后：

1、判断当前是否为定位模式，如果是，意味着不需要建图，即会将localmapping线程停止，并将这件事情告诉追踪器

如果取消定位模式，则上述过程反过来

if条件里的变量在sysytem头文件中定义，通过system.cc文件中的ActivateLocalizationMode、ActivateLocalizationMode函数来修改

2、检查是否要求追踪器复位：

mbReset变量与上面的两个检查变量类似，通过下述函数修改

3、保存估计的相机位姿，并作为返回值。

cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp);

4、获取其他变量：运动追踪状态、当前帧追踪到的地图点向量指针、和当前帧追踪到的特征点向量指针。

mTrackingState = mpTracker->mState;

mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;

mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;

对于RGBD传感器和单目传感器，追踪器接口函数参数不一样，但函数内部实现的功能一样：

cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)

system.cc中其他一些函数：

bool System::MapChanged()：判断地图是否有较大的改变，通过调用map类的GetLastBigChangeIdx()函数，如果该函数返回值大于0，MapChanged函数返回true，否则为false

void System::Shutdown()：关闭整个系统，内部就是执行各个线程对象的RequestFinish()函数

void System::SaveTrajectoryTUM(const string &filename):按照TUM格式保存相机运行轨迹并保存到指定的文件中

void System::SaveKeyFrameTrajectoryTUM(const string &filename)按照TUM格式保存关键帧轨迹并保存到指定的文件中

void System::SaveTrajectoryKITTI(const string &filename)：按照KITTI数据集的格式将相机的运动轨迹保存到文件中

下面三个函数是获取一些变量的值，对应的变量在追踪器的接口函数中最后几行赋值：

 Tracking.cc:

在system类中的构造函数中通过

 调用tracking类：tracking的构造函数：

 1、首先从配置文件中读入相机内参、矫正系数等参数并赋值。

 2、配置文件中还有提取ORB特征点相关的一些参数，如每一帧提取的特征点数、金字塔的层数、fast特征点的阈值，读入这些参数才可以创建特征点提取器：

 如果是双目传感器，还会创建一个 mpORBextractorRight，与left完全一样；如果是单目，初始化时使用mpIniORBextractor作为提取器，它每帧提取的特征点数量是两倍的nfeatures，其他参数完全一样

 构造函数的最后是对 mThDepth和 mDepthMapFactor的赋值

tracking.cc中的其他函数：

 

 上面这三个函数是为了方便线程通信，在上面的system.cc中调用

下面这个函数根据双目输入计算当前帧的位姿，首先转为灰度图，然后构造当前帧，执行track函数，返回计算的位姿。对于RGBD和单目的位姿计算分别在cv::Mat Tracking::GrabImageRGBD和cv::Mat Tracking::GrabImageMonocular函数中。

 

 构造当前帧，后面会对其各种属性进行赋值。

track()函数：首先判断mState的值，它表示当前tracking的状态，初始值为NO_IMAGES_YET，表示当前系统没有输入，如果这时调用track函数，那么首先是要进行初始化，修改mState为NOT_INITIALIZED。双目和RGBD相机的初始化函数均为StereoInitialization()，单目为MonocularInitialization()均在tracking.cc中有定义。初始化函数的最后会将mState置为OK。双目和RGBD的初始化过程较简单：首先要保证当前帧的特征点数量大于500，设置初始位姿为单位旋转和0平移，将当前帧设置为关键帧，向地图中加入这个关键帧：mpMap->AddKeyFrame(pKFini)；为每个特征点构造地图点，在当前帧中使用mvDepth这个数组来保存所有特征点的深度，只有深度>0的特征点才会构造世界点。MapPoint类会记录自己被哪个关键帧的哪个特征点观测到，所有可以观测到该地图点的特征点中区分度最高的描述子以及平均观测方向和观测距离地图点加入地图，也会加入初始关键帧(哪个特征点可以观测到)，mvpMapPoints数组记录了当前帧特征点与地图点的对应关系。

 最后将当前帧设为上一帧，获取地图中的地图点(此时应该是初始化时生成的那些地图点)，设置初始关键帧为参考关键帧，初始化的地图点设置为地图中的参考地图点(画图时用)，设置相机当前pose(单位旋转，0平移)

单目初始化：

需要连续两帧的特征点数量都>100，通过ORBmatcher中的searchforinitialization函数进行特征匹配，返回匹配成功的数量，如果小于100，同样需要重新初始化。Initializer.cc中的Initialize函数同时计算基础矩阵和单应矩阵选择最佳的得到两帧间的相对姿态，初始化成功后，首先删除无法三角化的匹配点，将初始化第一帧作为世界坐标系，计算第二帧的位姿。

 最后将三角化得到的3D点打包成地图点(需要设置一些属性值)：CreateInitialMapMonocular()

这个函数同时也会将mState置为OK

初始化完成后，会通过mbOnlyTracking为0或为1判断当前是处于SLAM模式(定位+建图)还是处于仅定位模式。如果是SLAM模式，如果当前正常跟踪，首先通过CheckReplacedInLastFrame()函数检查局部建图线程更新的地图点，然后判断使用参考帧跟踪TrackReferenceKeyFrame()还是使用运动模型跟踪TrackWithMotionModel()。前者条件是运动模型为空(mVelocity.empty()为1 )或者当前帧是重定位帧的下一帧(mCurrentFrame.mnId<mnLastRelocFrameId+2==1),说明需要重定位。后者条件即上面两个条件都不满足，此时可以使用运动模型跟踪，在使用恒速模型跟踪是也可能出现失败的情况，此时只能回到参考帧跟踪

 如果跟踪不成功(mState!=OK),则需要重定位bOK = Relocalization()

如果是仅定位模式，如果mState==LOST，说明此时跟丢了，则需要重定位,否则，会根据当前帧匹配地图点的数量是否大于10选择跟踪策略：如果>10，且满足运动模型，则TrackWithMotionModel()，不满足运动模型则TrackReferenceKeyFrame()；如果<10，这时既要跟踪同时因为匹配的地图点太少又要重定位，会计算两个位姿：运动模型估计的位姿和重定位估计的位姿，如果重定位成功，则使用重定位的位姿，否则

Tracking过程中使用的几个关键函数：

TrackReferenceKeyFrame():

在恒速模型失效时使用，首先将当前帧与参考关键帧进行特征匹配，匹配过程中使用了BOW进行加速，匹配成功后即可得到参考关键帧中的3D点以及它们在当前帧的投影坐标，以上一帧的位姿作为初值进行BA(仅优化位姿)，使用的是Optimizer中的PoseOptimization函数，优化后会剔除匹配中的外点，如果剔除之后成功匹配的数量>10则认为跟踪成功。

TrackWithMotionModel()：

使用恒速模型跟踪，这里的恒速是上一帧相对上上帧的相对变换，用这个变换乘上上一帧的位姿得到当前帧的初始位姿，使用这个位姿将上一帧的地图点投影到当前帧，根据设置的搜索半径进行匹配，得到20个以上的匹配后再进行3D-2D位姿优化，函数跟上面的一样。然后在剔除外点，如果剔除之后成功匹配的数量>10则认为跟踪成功。

TrackLocalMap():

前面都是利用了之前一帧的信息，这个函数会根据局部地图(局部关键帧和局部地图点)来得到更多的匹配关系，利用这些匹配关系再进行位姿优化

Relocalization()：

重定位函数，当特征点匹配关系不足导致跟踪失败时使用

1、计算当前帧的词袋向量

2、使用词袋向量匹配得到一些候选关键帧，这里使用的函数是KeyFrameDatabase里的DetectRelocalizationCandidates，是第一次筛选，条件较宽松。

3、遍历所有候选关键帧，使用词袋匹配结果(这里的词袋匹配是ORBmatcher里的SearchByBoW)初始化PnPSolver(这个源文件里使用的是EPnP，精度高速度快)，如果匹配数量<15，放弃该关键帧，否则用匹配结果初始化一个PnPSolver

 4、上一步淘汰掉了匹配数量小于15的候选关键帧，对于剩下的关键帧，使用相应的PnPSolver估计姿态，这个PnPSolver在解决问题是用到了RANSAC，可以简单的理解为处理数据中的外点。如果对于某个关键帧的PnP过程中超过了RANSAC的最大迭代次数，说明数据太差，将该关键帧舍弃但这里并没有跳出本次循环，也就是还在考虑当前关键帧

d

 使用EPnP计算出的位姿作为当前帧的初值以及RANSAC后的内点进行位姿优化，优化之后内点数量<10则跳出本次循环，否则继续:首先在当前帧删除优化过程中标记的外点，也就是当前帧看不到的地图点，如果内点<50，通过投影的方式将关键帧中未匹配的地图点投影到当前帧得到新的匹配关系(之前是词袋匹配)，如果这时>50，使用新的匹配进行位姿优化，得到新的内点数，这个新的内点数如果减小到了30和50之间，再投影匹配，但这一次会用更小的窗口和更严格的阈值，这次投影匹配后得到的特征点加上之前的新内点数>50，最后再位姿优化一下即得到了重定位的位姿，更新当前帧的地图点。如果不需要投影匹配内点已经>50,则认为该关键帧与当前帧的匹配关系足够好，当前帧的位姿即可确定，重定位成功。记录重定位帧的id为当前帧的id，防止短时间多次重定位。如果遍历完所有候选关键帧仍未匹配上，则重定位失败

这个重定位函数是通过和之前关键帧进行搜索匹配不断BA优化直到内点的数量>一定的阈值，但是BA优化需要比较好的初始值，初始时如果地图点的数量不太够可能得到一个较差的初值，即使优化成功，也可能跟真实值有偏差，这里我认为可以有一些改进工作；如果不增加其他传感器，可以通过建立一些语义信息使重定位函数有更多可以利用的信息。

localmapping线程：该线程负责生成一些地图点，而且是质量高的地图点，可以减少计算量提高追踪效率。地图点全部来自关键帧中的特征点。是一些离散的点，对于特征点法来说构建的地图是稀疏的。Run函数是该线程的主函数，里面就是执行一个while循环来处理Tracking线程产生的关键帧，进入while循环后，首先告诉Tracking线程停止产生关键帧，然后检查当前关键帧队列，如果不为空，执行ProcessNewKeyFrame()函数处理队头关键帧：包括计算该关键帧的词袋向量，然后遍历改关键帧的地图点，只要它不是坏点，就为它设置一些属性，如果这个地图点的观测还不包括当前关键帧，也就是说它是局部地图中的地图点，在当前这个新来的关键帧中观测得到，则为它增加一个观测(当前关键帧)，更新该点的平均观测方向和描述子等信息，如果不需要设置这些属性值，只有在双目或者RGBD时才会出现这种情况，因为这两种传感器近平一帧就可以创建地图点，但是我们并不知道它的质量的好坏，将它放到mlpRecentAddedMapPoints队列中等待检验。遍历完该关键帧的地图点后，需要更新共视图并将该关键帧放入局部地图。

然后执行MapPointCulling 检查队列中的地图点，筛选出质量高的，选取准则依次为：坏点首先删除；跟踪到该地图点的帧数相比预计可观测到该地图点的帧数比例小于0.25删除并SetBadFlag；(这个地方没看懂，前者是看到地图点的帧数，包括普通帧，后者说是地图点应该被看到的帧数，这两个值为什么不相等)；从建立该点的第一个关键帧到当前关键帧如果不小于两个关键帧但是观测到该地图点的帧数小于一个阈值则删除并SetBadFlag，这个阈值跟传感器的类型有关；这个点从开始建立到现在经历了>=3个关键帧则删除，但不SetBadFlag，否则不删除(没有任何操作)，执行下一次for循环。

CreateNewMapPoints()函数用来生成新的地图点，将当前帧与共视程度最高的几个相邻关键帧使用词袋匹配得到匹配关系然后三角化得到3D点，最后还会计算3D点在两个帧上的重投影误差，这里用到了卡方检验，如果大于阈值，则不将该点生成为地图点。

然后判断是否处理到了队列的最后一个关键帧，如果是的话会将当前关键帧与一级二级共视关键帧进行地图点的融合，分为两步，首先将当前关键帧的地图点投影到相邻关键帧中，如果有对应的特征点，而且对应的特征点有地图点，选择共视帧数多的替换这两个地图点，意味着会删掉一个，如果对应的特征点没有地图点，则为该点添加一个投影点，这个过程成为正向投影。下一步是反向投影，实际上就是上一个过程的逆过程，将相邻关键帧的地图点投影到当前关键帧中进行地图点的融合。

然后再判断如果是队列最后一个要插入的关键帧而且没有要求停止localmapping线程，那么会进行一次localBA，优化局部地图的关键帧和地图点，这要求地图里的关键帧的数量大于2，优化后，会剔除冗余关键帧，冗余意味着参考价值不大，它所带来的信息绝大部分可以由其他关键帧获得，具体一点就是这个关键帧90%的地图点可以被其他关键帧观测到，那么它就是冗余的。

最后还需要将关键帧插入到闭环检测的队列里面，这里无关他是否是最后一个。

loopclosing线程：闭环线程的关键帧来自局部建图线程，局部建图的关键帧来自tracking。

如果队列中有关键帧，而且检测到了回环，且计算出了SIM3矩阵(相似变换矩阵，s、R、t双目和RGBD中s为1)那么就进行闭环融合和优化