ORB_SLAM2再学习——单目

ORB_SLAM2学习——单目

ORB_SLAM2包含有三个线程，是同步进行的，因此之间的数据段共享，mappoint和keyFrame的创建和删除在三个线程中都有体现。

 

一：Tracking

1. GrabImageMonocular()

（1）将图像转为mImGray并初始化mCurrentFrame

（2）进行tracking过程

（3）输出世界坐标系到该帧相机坐标系的变换矩阵

 

2. Track()

track包含两部分：估计运动、跟踪局部地图，包括有相机跟踪和局部地图跟踪

（1）      MonocularInitialization()

单目初始化过程，并行地计算基础矩阵和单应性矩阵，选取其中最优结果，恢复出最开始两帧（第一和第二帧）之间的相对姿态以及点云，得到初始两帧的匹配、相对运动、初始MapPoints（这就是局部地图的路标点，只包含了匹配成功的点，需要注意的是所有的路标点都是经过匹配成功的特征点，在Track线程中局部地图一开始相机跟踪所产生的路标点都是通过两个相机之间的匹配得到的，接着局部地图跟踪所产生的点都是局部地图与相机之间匹配的点）。

1）  首先如果单目初始器还没有被创建，则创建单目初始器，所以进入第一帧的时候创建单目初始器，里面都是对第一帧进行操作

2）  第二帧进入，开始初始化，也就是第一帧和第二帧之间进行对极几何和三角化得到1,2之间的相对变换和以及匹配特征点的3d坐标。

首先在mInitialFrame（第一帧）与mCurrentFrame（第二帧）中找匹配的特征点对，mvbPrevMatched为前一帧的特征点（数据），存储了mInitialFrame中哪些点将进行接下来的匹配，mvIniMatches存储mInitialFrame,mCurrentFrame之间匹配的特征点（编号），因此可由编号查找到数据信息；

然后通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints，就是代码中进入到Initialize中,在函数里面开起了两个线程，用于并行计算H和F矩阵，以F矩阵为例，进入到FindFundamental()函数中，在函数里面由调用了ComputeF21()函数，真正开始计算F矩阵，同时为计算的矩阵评分，里面的计算过程与书上的8点法计算F矩阵相同，（这里的细节比较重要，查看具体代码的解释），计算万F矩阵后，就是如何冲F矩阵恢复出R和t，进入到ReconstructF()恢复，在函数里调用了CheckRT函数同时检测了R,t和计算了匹配点的三地坐标。

最后将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵，同时将三角化得到的3D点包装成MapPoints，这时候会调用CreateInitialMapMonocular()，（里面很复杂，还进行了全局的BA优化，设置第一帧和第二帧为关键帧，函数的最后就是初始化成功的标志）

 

（2）      相机位姿跟踪

在初始化成功的情况下，进行相机位姿跟踪，主要有两种跟踪模型，分别是TrackReferenceKeyFrame()（参考关键帧模型）和TrackWithMotionModel()（匀速模型），之类主要介绍下参考关键帧模型。

进入TrackReferenceKeyFrame()中，其实就是跟前面的关键帧进行跟踪（计算位姿和匹配的3D点），按照初始化的理解，第一帧和第二帧都被设置为关键帧，这里假设是第三个关键帧，

1）  首先跟上一个关键帧（第二帧也就是第二关键帧）进行特征点匹配SearchByBoW，得到匹配的特征点个数和匹配的特征点，这里需要注意的是这里在当前关键帧与参考关键帧之间匹配的时候选取的是参考关键帧中已经计算得到三角化3d坐标的点（同时要判断匹配的个数要大于15的阈值），因此下面可以进行PNP求解，这也就是单目的跟踪过程，单目初始化——>PNP求解

2）  有了匹配的特征点（在这里需要注意的是这些点是已知3d坐标的），就可以作PNP求解位姿同时优化位姿，即进入Optimizer::PoseOptimization(&mCurrentFrame) 优化3D-2D的重投影误差来获得位姿（代码理解需要查看函数头注释），该优化函数主要用于Tracking线程中：运动跟踪、参考帧跟踪、地图跟踪、重定位。具体的优化过程见代码，与书上基本一致，需要注意这里一共进行了4次优化，每次优化的迭代次数为10，在每次优化的时候都会对异常点（外点）剔除，不加入下面的优化，这种方式和书上的一次性迭代100次有区别，感觉效率更高，还需要注意的是这里的预测值是在优化的时候直接设置到顶点中的，而且每次迭代都会重新设置成初始值（这样没有问题吗？难道迭代的意义就是去除outlier，然后算10次就很准确了？）。

 

3）  最后，剔除优化后的outlier匹配点（MapPoints）

 

（3）      重定位

Relocalization重定位发生在前面的相机跟踪失败的情况下

1）  在关键帧数据库中找到与当前帧相似的候选关键帧

2）  在候选关键帧中通过BoW进行匹配matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i])，在匹配到大于15个的时候就进行创建PNP优化器，一直将所有的候选关键帧遍历完，并记录标志位，然后对于满足条件的候选关键帧通过EPnP算法估计姿态cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);位姿获得后通过PoseOptimization对姿态进行优化求解Optimizer::PoseOptimization(&mCurrentFrame)；如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解

（4）      局部地图跟踪

在相机跟踪成功的情况下，进行局部地图跟踪TrackLocalMap()，在帧间匹配得到初始的姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿，local map是当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系的集合。

1）  更新局部地图，包括局部关键帧和关键点

更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints

2）  对局部地图MapPoints进行投影匹配

在局部地图中查找与当前帧匹配的MapPoints，，这样就会增加当前关键帧的匹配点的个数，增加约束条件，是的求解出来的位姿更加准确（相比于前面相机跟踪所产生的3d点来使用PNP求解的位姿更准确，也就是跟前一个关键帧的匹配，知识这里能够匹配的点都是已知3d坐标的点），这里主要是查找当前帧和局部地图的匹配点（也是已知3d坐标），首先遍历当前帧的mvpMapPoints（已经和前一关键帧匹配过的点；然后），标记这些MapPoints不参与之后的搜索，然后将所有局部MapPoints投影到当前帧，判断是否在视野范围内，然后进行投影匹配，这里也就是获得在局部地图中为匹配的特征3d点与当前帧的匹配点；最后对视野范围内的MapPoints通过投影进行特征点匹配，这里匹配的时候有一个阈值的设置，当不久前进行过重定位的时候阈值会变大，应为刚刚重定位的位姿结果准确性比较差，投影误差相应较大，所以阈值更大。

 

3）  根据匹配对估计当前帧的姿态

更新局部所有MapPoints后对位姿再次优化Optimizer::PoseOptimization(&mCurrentFrame); 在这个函数之前，在Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel中都有位姿优化，但是由于约束条件较少，因此准确性相比较差。

4）  更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果

 

（5）      是否生成关键帧

在NeedNewKeyFrame()中有对关键帧是否要生成的判断，如果用户在界面上选择重定位，那么将不插入关键帧；如果局部地图被闭环检测使用，则不插入关键帧；判断是否距离上一次插入关键帧的时间太短，如果关键帧比较少，则考虑插入关键帧，或距离上一次重定位超过1s，则考虑插入关键帧；很长时间没有插入关键帧，localMapper处于空闲状态，跟踪要跪的节奏，插入关键帧。

（6）      生成关键帧

在上面判断插入关键帧之后执行CreateNewKeyFrame()，这里对于非单目的情况下同时创建新的Mappoint。对于单目来说，直接将当前帧构造长关键帧。

 

 

二：LocalMapping

上面的Tracking线程和LocalMapping线程之间的联系主要是生成KeyFrame，是由mlNewKeyFrame这个关键帧列表传递。局部地图中主要是处理传递过来的关键帧，剔除、生成（主要是单目）和融合MapPoint，在MapPoint发生改变后，就会进行局部的BA优化，最后在进行关键帧的剔除。完成这些操作之后，将处理完的关键帧加入到闭环检测的队列中，进行下一步的闭环检测。

1.      首先设置LocalMapping正处于繁忙状态，处理关键帧列表，判断列表不为空。

2.      处理关键帧，计算关键帧特征点的BoW映射，将关键帧插入地图

（1）      首先在关键帧列表中读取一帧关键帧，计算该关键帧特征点的Bow映射关系，ComputeBoW()，其实就是关键帧的属性，将属性相同的关键帧就可以归为一类了。

 

（2）      跟踪局部地图过程中新匹配上的MapPoints和当前关键帧绑定，在Track的LocalMap函数中将局部地图中的MapPoints与当前帧进行了匹配，但没有对这些匹配上的MapPoints与当前帧进行关联（其实这一部分主要是针对非单目在track最后生成的MapPoitns，单目的MapPoint生成主要是在下面第4步）

 

（3）      更新关键帧间的连接关系，Covisibility图和Essential图(tree)，这部分很重要，在没有执行这个函数前，关键帧只和MapPoints之间有连接关系，这个函数可以更新关键帧之间的连接关系。首先获得当前关键帧的所有3D点，通过3D点间接统计可以观测到这些3D点的所有关键帧之间的共视程度，即统计当前关键帧都有多少关键帧与它存在共视关系，统计结果放在KFcounter中，对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数；然后遍历统计到的结果，判断该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高），vPairs记录与其它关键帧共视帧数大于th（阈值）的关键帧；最后对这些连接按照权重从大到小进行排序，以方便将来的处理更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树。

 

 

3.      剔除MapPoints

剔除ProcessNewKeyFrame函数中引入的不合格MapPoints，其中包括各种剔除条件。遍历等待检查的MapPoints，将不满足条件的MapPoint剔除，一共有三个剔除条件，条件1：已经是坏点的MapPoints直接从检查链表中删除；条件2：跟踪到该MapPoint的Frame数相比预计可观测到该MapPoint的Frame数的比例小于25%；条件3：观测到该点的关键帧太少。

 

4.      生成MapPoints

相机运动过程中和共视程度比较高的关键帧通过三角化恢复出一些MapPoints，这部分主要是单目生成新的MapPoins比较重要，因为在单目情况下，前面所有生成的MapPoint都是由连续的两帧匹配得到的，这样使得MapPoint的数据较少，因此这一步骤可以增加局部地图中的MapPoint的数目。

步骤1.得到共视程度好的相邻关键帧vpNeighKFs（单目20个，非单目10个）

步骤2：遍历相邻关键帧vpNeighKFs，首先判断遍历的关键帧是否满足与当前关键帧

生成MapPoint的条件（基线是不是足够长，基线与景深的比例），下面的过程就是有两个图像三角化出MapPoint。

5.      MapPoints融合

在循环处理已经处理完队列中的最后的一个关键帧后，检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints

 

6.      Local BA

和当前关键帧相连的关键帧以及MapPoints作局部BA优化。

 

7.      关键帧剔除

              删除冗余的关键帧，根据CovisibilityGraph提取当前帧的共视关键帧，对所有的局部关键帧进行遍历，提取每个共视关键帧的MapPoints，遍历该局部关键帧的MapPoints，判断是否90%以上的MapPoints能被其它关键帧（至少3个）观测到，该局部关键帧90%以上的MapPoints能被其它关键帧（至少3个）观测到，则认为是冗余关键帧，即设置坏的标志位，剔除关键帧。