ORB-SLAM2 学习阶段性总结——单目代码总结(二)

第二章：代码详解

代码框架介绍

 

时隔半个月，再次写关于ORB-SLAM代码这一章，代码很复杂。研究有一些心得，这里就介绍关于整个算法结构的一些总结

mono_tum.cc

代码就不粘贴了，先介绍的是关于单目运行tum数据集算法

在主程序代码中值得注意的就是以下几点，这个部分也就会按照这三点叙述：

1. 加载文件LoadImages过程
2. ORB_SLAM2::System SLAM构造（*开了线程）
3. SLAM.TrackMonocular(im,tframe);跟踪
4. SLAM.Shutdown();结束

（2、3在一个循环里，结束循环就要求结束）

对于1、4较简单，基本不用详解，难点在后面2、3

 

2.第二部分：ORB_SLAM2::System SLAM构造函数

首先是ORB_SLAM2::System SLAM构造函数，如果细说确实没有好说的，基本上所有的类初始化还有构造器初始化都在这里面构造。

（值得备注一下：9.21日学习C++之后发现类中函数不调用就不会执行，之前看来理解还有些错乱，重新更新一下）

首先是class System中

public:函数暂时不需要细看

private:变量需要注意

ORBVocabulary* mpVocabulary;

KeyFrameDatabase* mpKeyFrameDatabase;

Map* mpMap;

Tracking* mpTracker;

LocalMapping* mpLocalMapper;

LoopClosing* mpLoopCloser;

Viewer* mpViewer;

FrameDrawer* mpFrameDrawer;

MapDrawer* mpMapDrawer;

std::thread* mptLocalMapping;

std::thread* mptLoopClosing;

std::thread* mptViewer;

std::mutex mMutexReset;

bool mbReset;

                        std::mutex mMutexMode;

 bool mbActivateLocalizationMode;

 bool mbDeactivateLocalizationMode;

                        int mTrackingState;

 std::vector<MapPoint*> mTrackedMapPoints;

 std::vector<cv::KeyPoint> mTrackedKeyPointsUn;

 std::mutex mMutexState;

也就是说，基本上所有的指针在System中就定义好了，不需要后面在定义了

该构造函数中：

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

则会加载函数

1. 配置相机模型（只要知道在哪能找到，能调参就可以）

cv::FileStorage fsSettings(strSettingsFile.c_str(),       // 就是指TUM1.yaml

cv::FileStorage::READ);        //只读

1. 建立一个新的ORB字典、加载字典（第三方库不需要理解过深）

mpVocabulary = new ORBVocabulary();

bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);

1. 建立一个关键帧的数据库

mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

//进入发现里面只有mvInvertedFile.resize(voc.size());

1. 新地图

mpMap = new Map();

1. 以及可视化的界面绘制

mpFrameDrawer = new FrameDrawer(mpMap);

mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);

1. 值得注意的是在主线程这里面开了三个小线程，也就意味着这三个线程一开始就构造了。主线程：跟踪Tracking。副线程：局部建图、回环检测、可视化绘制。

mpTracker = new Tracking(this,      //现在还不是很明白为什么这里还需要一个this指针

           mpVocabulary,    //字典

           mpFrameDrawer,    //帧绘制器

           mpMapDrawer,    //地图绘制器

                             mpMap,      //地图

                             mpKeyFrameDatabase,   //关键帧地图

                             strSettingsFile,    //设置文件路径

                             mSensor);     //传感器类型iomanip

//初始化局部建图线程并运行

    //Initialize the Local Mapping thread and launch

    mpLocalMapper = new LocalMapping(mpMap,     //指定使iomanip

    mSensor==MONOCULAR);

    //运行这个局部建图线程

    mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,

            mpLocalMapper);    //这个调用函数的参数

    //Initialize the Loop Closing thread and launchiomanip

    mpLoopCloser = new LoopClosing(mpMap,       //地图

              mpKeyFrameDatabase,    //关键帧数据库

              mpVocabulary,     //ORB字典

              mSensor!=MONOCULAR);   //当前的传感器是否是单目

    //创建回环检测线程

    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run,

           mpLoopCloser);     //该函数的参数

1. 如果选择了可视化界面（if），还需要开可视化线程

    if(bUseViewer){...}

1. 关于进程的指针（略）

查看这些线程，发现里面都是while（1）{}。也就是说这三个线程在程序开始就开始了。而SLAM.TrackMonocular(im,tframe)跟踪获得帧是后面才有的环节，我们知道这三个进程都需要帧的数据，这就意味着这三个线程虽然开始了但是一直在等待数据的传入。

接下来就到了整个代码最重要的跟踪函数，同时该跟踪流程给副线程传入数据，副线程开始工作，跟踪、建图、一直在检测回环检测……

以上就是运行tum数据集的算法过程，接下来来就是细节详解（不涉及数据结构），先介绍主线程，在介绍副线程

2.1Tracking.cc

***代码略（十分简单的代码）***

我们加载了一些参数信息，这些需要加载的参数都是在：

Examples/Monocular/TUM1.yaml定义好了的

在加载特征点参数的时候，需要额外注意特征点提取器：

mpORBextractorLeft = new ORBextractor(…

2.1.1Tracking.cc中的ORBextractor

提取器只是定义了提取的步骤，并没有提取

在代码中简单的规定了金字塔降采样每层缩放系数，以及每层应该提取的特征点数目还有非常难懂的灰度值心法来确定特征点的旋转不变性,这里都已经在pdf解释过了就不复制过来了。(14页)

这里在重复一下：特征点———角点、描述子、尺度、旋转、

FAST是根据某一像素点周围半径为3的周围像素点满足某个规则来提取（FAST 特征点概述_木独的博客-优快云博客_fast特征）

为了匹配这个角点，使用描述子概念，只有两个角点的描述子一样才能够匹配（略）

对于尺度问题，我们建立了图像金字塔来处理

对于旋转问题，我们来计算特征点的方向（这里非常的麻烦，同样是取圆，但是不是按照四舍五入的规则的按四舍五入来计算边界像素点，不然会不对齐）

单目初始化器特征点构造

mpIniORBextractor = new ORBextractor(2*nFeatures,

这是单目独有的，需要双倍的特征点提取。

紧接着在还需要判断3D点的远近（大于基线40倍叫做远点）

代码绕了一圈，最后结果也就是算basline*40，这就是深度阈值

2.1..2.LocalMapping.cc和LoopClosing.cc

这两个并没有在构造函数有过多的定义，只是初始化了参数的值

副线程

mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,

mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run,

后面会说其副线程

这样的话第二部分关于ORB_SLAM2::System SLAM构造函数就讲完了

3.第三部分：SLAM.TrackMonocular(im,tframe);

值得注意的是，第三部分在一个while循环中，这个循环不断地读取图片，开始将图片信息输入帧im里面了

SLAM.TrackMonocular(im,tframe);

值得注意的在获取相机位姿的估计（也就是前端）

cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);

3..1GrabImageMonocular

转灰度图就不说了

我们在上面读取了图片信息im，现在就开始了构造Frame类

mCurrentFrame = Frame(

要注意：单目需要判断初始化所以这里使用了前面的特征点提取器来初始化

初始化的目的是为了获得第一帧到第二帧之间的位姿变化，因为单目相机在这一过程（尤其是在特征点匹配过程使用运动恢复初始化SFM）比较困难，所以在初始化过程一般都需要经过很长时间来实现，而且过程比较坎坷。

3.2GrabImageMonocular中Frame.cc

帧id还有一些参数就不细说了

    最重要的是提取图像的特征点函数

ExtractORB(0,imGray);

进入该函数：发现使用了伪函数来实现

(*mpORBextractorLeft)(im,       //待提取特征点的图像

            cv::Mat(),                        //掩摸图像, 实际没有用到

            mvKeys,                         //输出变量，用于保存提取后的特征点

            mDescriptors);               //输出变量，用于保存特征点的描述子

再进入该伪函数（在ORBextractor中）

3.2.1GrabImageMonocular中Frame.cc中ORBextractor.cc

void ORBextractor::operator()( InputArray _image,

                                          InputArray _mask,

                                          vector<KeyPoint>& _keypoints,

                          OutputArray _descriptors){…}

3.2.2同在这个函数中

void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints, OutputArray _descriptors)

在特征点提取函数中需要注意的是构造特征金字塔

ComputePyramid(image);

里面比较复杂的是处理图像边缘——这部分希望直接看代码，我的代码注释的很详细。

3.2.3同这个函数：四叉树

然后是使用四叉树来合理分配特征点

    ComputeKeyPointsOctTree(allKeypoints);

里面的ceil是第三方库设计的就不需要多了解，大概理解pdf上自己画的图像网格即可。

3.2.4同这个函数：提取角点

角点提取也不需要自己理解

我们需要知道的就是他将角点数据丢进了vKeysCell，以及每一层都有一个keypoints容器，然后计算了这些特征点的方向（同样是分层计算）

computeOrientation(

3.2.5进入DistributeOctTree(

这块四叉树概念比较复杂如果想改进的话看一看

3.2.6GrabImageMonocular中Frame.cc中ORBextractor.cc

_descriptors.create(nkeypoints,      

拷贝图像描述子到新的矩阵descriptors

descriptors = _descriptors.getMat();

由角点来分配描述子个数

值得注意的是：

allKeypoints是一个容器，里面是每层特征点的指针keypoints。但是描述子却是装在一个容器里面descriptors。因为遍历特征点是是一层一层进行的，但是描述子那个矩阵是存储整个图像金字塔中特征点的描述子，所以后面又设置了Offset变量来保存“寻址”时的偏移量，辅助进行在总描述子mat中的定位。

3.2.7高斯模糊

GaussianBlur

以及高斯模糊后的计算描述子

computeDescriptors（

3.2.8坐标恢复

金字塔高层最后还是要恢复到第零层，然后得到在第零层相应的特征点坐标

If（…）{…}

3.2.9插入到关键点

最终将每层（所有）的特征点放入_keypoints（也就是）

    _keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());

3.3跟踪Track();

就在该函数下面由  Track();过程

3.1单目初始化

MonocularInitialization();

3.1.1初始化MonocularInitialization();

该函数主要是借助第一帧为初始化帧，利用第二帧与第一帧之间进行特征点匹配，匹配特征点（前端初略的进行获取位姿第一步：特征点匹配）

如果太少就认为不具有初始化的条件就会重新确立第一帧，然后重新开始初始化。

3.1.2 matcher.SearchForInitialization(特征点匹配

在这个函数中使用了直方图分布来进行检测特征点匹配的方向确定，这部分pdf解释的很好了，就不解释了，看cvlife视频就已经说这块似乎有bug

其中不仅仅要注意使用了半径快速滑动窗口搜索当前帧中的所有候选特征点，匹配最优的特征点，紧接者还用了旋转直方图来检查方向。

最后得到的匹配好的点放入vbPrevMatched

这部分代码需要注意一下：因为代码在函数中给封装的vector很好，以至于后面突然蹦出几个vector理解不了（比方说vnMatches12）

3.1.2.1 mpInitializer->Initialize

上一节，我们得到了关于特征点的匹配结果，接下来就是恢复位姿（也就是计算相机的运动），位姿恢复在14讲就说可以用本质矩阵还有单应矩阵来恢复

这个函数中就用了两种方法来进行估测（甚至开了多线程），选择最好的一种来进行检测

最后调用函数来恢复位姿R，T，同时得到了地图点的空间坐标

ReconstructH（

3.1.2.2 创建初始化地图点MapPointsCreateInitialMapMonocular();

初始化帧就认为是关键帧，然后处理关键帧生成地图点（主要目的），还做了一些小的操作，比如说来了一次全局BA，尺度归一以及插入关键帧进入局部地图所需要更新的信息

 

 

3.2正常SLAM跟踪模式

 

判断是正常的SLAM模式之后

替换地图点

                CheckReplacedInLastFrame();

所要注意的是后面两种给跟踪模式

一种是基于上一关键帧进行跟踪（也叫参考关键帧跟踪）

另一种是跟踪上一帧进行跟踪（也叫恒速模型跟踪）

刚开始使用第一种参考关键帧跟踪

bOK = TrackReferenceKeyFrame();

猜测初始化质量不太好，需要一直对初始关键帧来做位姿获取

接下来就是固定套路了

3.2.1关键帧跟踪TrackReferenceKeyFrame

（这块带代码量很大，虽然封装的很好但是理解很慢）

* Step 1：将当前普通帧的描述子转化为BoW向量

 * Step 2：通过词袋BoW加速当前帧与参考帧之间的特征点匹配

 * Step 3: 将上一帧的位姿态作为当前帧位姿的初始值

 * Step 4: 通过优化3D-2D的重投影误差来获得位姿

 * Step 5：剔除优化后的匹配点中的外点

简单来说：关键帧跟踪利用bow向量进行加速匹配帧与关键帧之间进行匹配，通过优化重投影误差获得位姿更新地图点。

一般来说都是用恒速模型跟踪

bOK = TrackWithMotionModel();

3.2.2恒速模型跟踪TrackWithMotionModel();

目的是快速跟踪

* Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点

 * Step 2：根据上一帧特征点对应地图点进行投影匹配

 * Step 3：优化当前帧位姿

 * Step 4：剔除地图点中外点

要注意恒速模型跟踪不是利用描述子来进行匹配，而是利用上一帧的3D点反投影到当前帧的图像上（也就是上一次的位姿、以及上一帧的RT）大致确定一个可能的特征点的区域。然后进行匹配

matcher.SearchByProjection

找到匹配点后还会用BA进行优化位姿（作为这次的位姿）

Optimizer::PoseOptimization

当然，这种跟踪肯定会有问题（比方说运动速度过快，就会找不到匹配点）

这时候就会进入关键帧跟踪，简单来说这是一种快速匹配的方法。

3.2.3重定位Relocalization

上面两种作为最基础的跟踪方式，如果跟踪失误（也就是bOK会变为o）

判断不正常就意味着跟踪失败了

bOK = Relocalization();

也就是说这个时候跟踪出错，试图换用更好的方法进行定位

上面采用的跟踪方法是2D到2D，之间恢复了3D点。如果我们再用2D到2D会有问题。（因为无论是用关键帧跟踪还是恒速模型跟踪已经失败了）

就意味着最近的帧（甚至关键帧）都不能定位这一帧了，我们就需要在全局的角度进行定位。常见的方法就是使用pnp（3D到2D的方法）

利用前面获取的3D点来定位这帧的2D。

步骤虽然麻烦，但是现在理解了

就是：

1搜索与该帧相似的关键帧（利用词袋实现的）

2这些相似的关键帧与当前帧进行匹配特征点（Bow快速匹配）。但是不用来计算位姿，而是取出相似关键帧匹配特征点对应的3D地图点，利用pnp方法来仅计算位姿，这种方法肯定匹配的点很少，但是我们利用关键帧投影（SearchByProjection）来寻找更多的匹配点，然后接着做BA

就像代码中写的那样，为什么这么麻烦，其实是担心闭环

三种跟踪结束之后，现在已经知道跟踪过程初略的位姿了，但是有一些误差，为了修正这些小误差，我们使用了局部地图跟踪

bOK = TrackLocalMap();

3.2.4关键帧跟踪TrackLocalMap();

这种跟踪是基于关键帧的，而且是在有一些关键帧的基础之上进行位姿优化的。最后优化了一级和二级的共视关键帧（注意：什么是二级共视关键帧）

如果上面跟踪都失败了，那就重新开始初始化了

跟踪成功就会开始新建关键帧，添加地图点，删除多余的点……

 

第二部分：副线程（补充）

在上面就说过：副线程在初始化的时候就已经创建好了，只是一直在发等待数据（关键帧）的传入。上面跟踪最后一步就是新建关键帧。

这里还是要注意一下关键帧新建环节，在描述一下：

            if(NeedNewKeyFrame())

                CreateNewKeyFrame();

* Step 1：纯VO模式下不插入关键帧，如果局部地图被闭环检测使用，则不插入关键帧

 * Step 2：如果距离上一次重定位比较近，或者关键帧数目超出最大限制帧率，不插入关键帧

 * Step 3：得到参考关键帧跟踪到的地图点数量

 * Step 4：查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧

 * Step 5：对于双目或RGBD摄像头，统计可以添加的有效地图点总数 和 跟踪到的地图点数量，近点少就要插入关键帧

 * Step 6：决策是否需要插入关键帧

Step 7.1：设定比例阈值，当前帧和参考关键帧跟踪到点的比例，比例越大，越倾向于增加关键帧

Step 7.2：c1a 很长时间没有插入关键帧，可以插入

Step 7.3：c1b 满足插入关键帧的最小间隔并且localMapper处于空闲状态，可以插入

Step 7.4：c1c 在双目，RGB-D的情况下当前帧跟踪到的点比参考关键帧的0.25倍还少，或者满足bNeedToInsertClose

Step 7.5：c2 和参考帧相比当前跟踪到的点太少 或者满足bNeedToInsertClose；同时跟踪到的内点还不能太少

在代码中所有的插入要求最后写成了

if((c1a||c1b||c1c)&&c2)

2.1局部建图LocalMapping.cc

       接下来就是研究局部建图这块了

 

进入LocalMapping.cc

该线程主要是主函数void LocalMapping::Run()

注意;

关键帧在step2 ProcessNewKeyFrame();会被弹出，放入mpCurrentKeyFrame中然后处理该指针的数据，也正是因为有该弹出mpCurrentKeyFrame，在InsertKeyFrame()插入给mlpLoopKeyFrameQueue。这样执循环行到最后就没有局部建图帧了。

地图点融合就不仔细说了

主要是进行局部地图的BA

Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

2.1.2局部BA：LocalBundleAdjustment

G2o的优化一般都比较难懂，这里不过多的介绍。

2.2闭环检测：LoopClosing

闭环检测和局部建图是同时进行的都需要CheckNewKeyFrames()来检测mlpLoopKeyFrameQueue是否有帧数据。

小疑惑

刚开始看到这里就开始疑惑，理论上开了线程在局部建图和闭环检测中应该同步进行（就像跟踪和局部建图，跟踪设置关键帧给局部建图，局部建图才有数据，进行工作），但是为什么局部建图在前面，闭环检测在后面？换句话说：两者有什么逻辑顺序

带着这个疑惑，我有重新看了跟踪和局部建图：

跟踪：关键帧KF插入mlNewKeyFrames

局部建图：mlNewKeyFrames弹出给mpCurrentKeyFrame经过优化、剔除、然后又插入mpCurrentKeyFrame

我有个疑惑：关键帧在哪定义的？似乎只说：跟踪后更新关键帧，但是在跟踪最后没有看到关键帧呀。然后我查看了InsertKeyFrame函数，查看调用情况。最后发现确实是我弄错了，在Track()中Step 8：就插入了关键帧。（我甚至自己还写了这部分的介绍。晕，是个乌龙）。

紧接着，我看了关于闭环检测：发现局部建图和闭环检测，所检测的队列不是同一个，前者是mlNewKeyFrames，后者是mlpLoopKeyFrameQueue（尴尬了，这里看的太快，以为是同一个了），跟踪将关键帧插入mlNewKeyFrames，局部建图读取mlNewKeyFrames插入mlpLoopKeyFrameQueue，也就是说（闭环检测有数据了，要开始闭环检测了）

2.2.1检测闭环etectLoop()

闭环检测要求很高，为了防止错误的闭环，ORB-SLAM在这块实现的很强

简单来说：如果某一关键帧发现自己bow向量和以前的关键帧列表bow向量匹配得分比较高，就会认为匹配了，这个时候我们就会找到与这一关键帧共视程度最高（>15个共视地图点）的几个关键帧，检查他们是否也发生了闭环。

但是不仅仅如此，这个匹配的标准是按照分数评判的，该关键帧与共视关键帧会有一个最低得分minScore，如果这些共视帧在匹配闭环帧的得分低与这个数值就不认为匹配成功

       代码介绍的比较详细，但是概念也多，这里就不细说了。