ORBSLAM2学习笔记（2）——入口与Tracking

ORBSLAM2学习笔记（2）——入口与Tracking

写在前面

本笔记是按照逻辑顺序来写，也就是想到哪写到哪，就是按照学习时的思考过程来写，和其他博客中按函数和功能来分类介绍不同。这是博主个人觉得最流畅的方法。

程序入口

按照github上的readme，我们可以从mono_kitti这一可执行文件开始执行slam，因此，mono_euroc就是ORBSLAM的入口啦。打开mono_euroc.cc，可以看到main函数做了以下事情，首先通过 LoadImages函数加载了所有的图片，之后，通过如下命令实例化了slam对象：

 ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

需要注意的是，该对象在实例化时，也实例化了一个新的词典对象ORBVocabulary，之后进行Btow时会用到。
之后，通过一个for循环，对每一张图片执行

SLAM.TrackMonocular(im,tframe);

即开始了SLAM过程 。该函数会先判断是否为纯定位模式（该模式下不会更新局部地图，只是利用已有图进行定位）。之后，通过

return mpTracker->GrabImageMonocular(im,timestamp);

该句进行实际的处理。
进入该函数，该函数将图片转为灰度图，再用该图构造了frame（帧），之后便进入Track函数。该函数返回的是mCurrentFrame.mTcw.clone()，即这一帧相机的位姿，然而，这一返回值并没有用到。

Tracking线程

初始化

该线程会首先进行初始化，双目和RGBD都使用StereoInitialization函数来初始化，该线程会
1.设定初始位姿（单位矩阵）；
2.将该帧初始化为关键帧；
3.将特征点构造为Mappoint。那么，特征点是哪里来的呢？是在实例化frame时，都会通过ExtractORB函数来提取该图片的特征点。
4.为新添加的Mappoint添加属性，包括AddObservation（表征该点被哪些帧观测到，ComputeDistinctiveDescriptors（计算最优描述子），UpdateNormalAndDepth（计算平均观测方向以及观测距离的范围），之后，在地图中添加该点，在该帧也添加该点，建立帧与点之间的索引。
最后，在localmap中也添加该帧，初始化就完成了。
对于单目的初始化，则有所不同，首先，为了选取稳健的初值，单目的初始化是可能失败的。其次，为了获取深度信息，单目初始化需要两帧，具体流程如下：
1.确认连续两帧的特征点数＞100，得到用于初始化的两帧，构建初始化器。
2.寻找两帧之间的匹配点，若匹配点<100，重新初始化，否侧，进入到Initialize函数中，同时计算出基础矩阵和单应性矩阵，选择其中效果较好的那个恢复出这两帧的相对姿态和点云。
3. 通过CreateInitialMapMonocular()函数，来讲这两帧作为关键帧插入地图中，将上一步恢复出的点云包装为关键点插入地图。
如上，便完成了初始化。

帧——帧匹配

初始化之后，就开始正常的跟踪（帧间匹配）。ORBSLAM2有两种跟踪模式，TrackWithMotionModel和TrackReferenceKeyFrame,
其区别为，前者采用匀速运动模型，在前一帧基础上运动一段距离为该帧位置设置初值，与前一帧进行匹配。后者直接采用前一帧为该帧位置设置初值，与关键帧进行匹配。显然，前者精度更高。因此，除非速度模型为空，或者刚刚重定位完，其他情况均采用第一种。
TrackWithMotionModel：首先，为了方便匹配，为上一帧增加地图点，方法为把从深度较小的特征点变为临时地图点（这些点之后会删除）。之后，调用

mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);

来为当前帧设置初值。之后，通过

int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);

来搜索特征点进行匹配，从而为当前帧添加地图点，以便于之后调用

Optimizer::PoseOptimization(&mCurrentFrame);

进行纯位姿优化。优化后，删除outlier的地图点。这种方法如果失败了（如地图点不够），则也进入TrackReferenceKeyFrame方法
TrackReferenceKeyFrame：
首先，用bow的方式对该帧和参考帧进行粗匹配，获得两者之间共同的地图点以及对应的特征点，之后，以上一帧的位姿为初始位姿，同样调用

Optimizer::PoseOptimization(&mCurrentFrame);

来进行纯位姿优化，获取当前帧的位姿，之后，删除outlier的地图点。
当然，还有一种情况，那就是定位跟丢了，这时会进入到Relocalization里，开始重定位。

重定位

大致上说，重定位和闭环检测有些相似。首先要做的，是检测和当前帧相似的关键帧，怎么检测呢？这里也用到了词袋模型。在DetectRelocalizationCandidates函数中，首先找出和当前帧拥有共同单词最多的帧，这个数量×0.8作为阈值，对高于阈值的候选帧通过

float si = mpVoc->score(F->mBowVec,pKFi->mBowVec);

计算匹配得分，这时自然会想到，可以把得分最高的帧当做最有可能成为重定位的匹配帧吗？ORBSLAM2并没有这么做，因为单个帧的得分具有不稳定性，它获取了与每一帧在共视图中相连的权值最大的10帧，把它作为一个整体，获取整体的匹配分数。最后，将组得分大于一定值，且为组内得分最高值的帧返回，作为DetectRelocalizationCandidates函数的结果。
找到候选帧后，我们对每一个候选帧都和当前帧进行一次快速bow匹配，删除不合格的帧。之后，对每一帧用pnp获取姿态初值，再用PoseOptimization优化。如果内点数量过少，则通过SearchByProjection寻找地图点与当前帧的匹配来增加内点，之后再进行匹配。一旦匹配质量达标，即内点数大于50，就退出匹配，认为完成了重定位。

帧——地图匹配

帧与帧的匹配完成后，将参考帧更新为最新的关键帧之后，便开始了帧与地图的匹配TrackLocalMap().
1.首先更新局部地图关键帧（与当前帧有共同地图点的帧），之后更新局部地图关键点（所有局部关键帧的地图点）。
2.遍历上一步得到的局部地图关键点，找出此时所有在相机视野内，但未成为该
帧地图点的点，便于下一步使用SearchByProjection进行当前帧和地图点的匹配。
3.和之前一样，匹配之后对位姿进行优化。
4.最后，更新地图点的信息，主要是增加它们的被观测次数，就算结束拉。

更新速度模型

截至上一部，我们得到了当前帧的位姿优化结果，与上一帧的位姿作对比，就得出此时的速度了。

判断并插入关键帧

更新速度模型后，清楚了之前为了匹配引入的临时地图点，然后进入到

            if(NeedNewKeyFrame())
                CreateNewKeyFrame();

中，这两行分别判断是否需要插入关键帧以及真正插入关键帧。
判断如果纯定位模式，或者地图正被闭环检测使用，则退出。如果关键帧数目多余阈值，或程序刚刚重启，则退出。之后，它有四个条件来判断
1.一段时间没有插入关键帧
2.地图处于空闲
3.内点的数目小于参考关键帧数目的一定值或当前帧观测到的地图点与特征点的比值较小（说明当前帧看到很多其他帧没看到的东西）。
4内点少与定制且地图点与特征点的比少于定值（3的弱化版）
这四个条件中，123满足一个即可，4必须满足，即可继续进行判断，如果等待被插入的关键帧数目小于3，则判断完成，插入！
插入
插入比较简单，就是通过

KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

将当前帧构建为关键帧，对于非单目情况要插入新的地图点，只有深度小于阈值的特征点才会被插入为地图点。之后，调用

    mpLocalMapper->InsertKeyFrame(pKF);

为局部地图插入关键帧。
如果此时定位以及丢失了，就reset。

以上就是tracking线程的全部内容啦，可以看出条理还是非常清晰的！