视觉里程计之orb-slam代码理解

最近在看orb-slam，orb-slam方案在slam领域的地位就不用说了，我花了三天大概理清了一下代码的逻辑和思路。具体的细节还没有仔细去看，由于本人也是刚刚学完高博的视觉slam十四讲，所以有一些地方有错误的还望批评指正。另外我参考的资料除了论文以外，还有在泡泡机器人上面找到的吴博和冯兵两位老师的视频以及PPT，非常感谢泡泡机器人这个平台，学习到了很多知识！

首先还是看论文里面的这幅图，当时就没有太在意，后来才发现这幅图的逻辑非常的清晰，真的是非常的好，另外orb-slam的代码逻辑也非常的清晰，Local Mapping和Loop Closing线程里面都有一个run函数，可以很直观的找到每一步在做一些什么事情。

 

我看了吴博老师的ppt感觉也是非常的清晰，所以为了加强自己的记忆，我就把吴博的整个流程图自己又画了一遍，虽然画的不是特别好，但是加深了一下记忆，我觉得加强了自己的理解也是非常不错的。另外在冯兵老师的PPT里面，我找到了可以比较快理清关于frame,feature，map point之间关系的图。

然后接下来就是代码的流程图以及我个人参考各路博客资料整理的一些说明吧。后面的我主要是看的单目的，所以双目和立体的都没看，以下针对的都是单目情况。

 

这个就是一个相当于预处理的一个过程，在这里提到了一个单目初始化，是一个非常重要的东西，ORB-SLAM通过自动单目初始化。

在单目构造帧的时候，如果此时未进行初始化，那么提取的特征数将会是已经进行初始化之后特征数的2倍。在进行单目初始化的时候，有两种方案，分别是计算单应矩阵和基础矩阵的R,t，然后对两个矩阵进行分值评估，根据分值确定选择哪一种方案。具体的计算方案如下：

 

然后就开始进入tracking线程了。解释以下，前面英文是函数名，后面是中文的一个解释或者理解吧。

 

 

 

主要包括初始化，跟踪，重定位，确定关键帧。

MonocularInitialization()(初始化)：两帧的特征点数目都大于100，则可以用于初始化，然后进行匹配，如果两帧的匹配特征点较少，则重新确定两帧。通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints，删除无法三角化的点，得到Map Point。

在跟踪里面又有三类跟踪，匀速跟踪，跟踪上关键帧、重定位以及跟踪局部地图。

TrackWithMotionModel()(匀速跟踪)：采取的时假定匀速运动也就是说相邻两帧之间的运动认为相同，然后得到一个初始T和r，将上一帧的3D点（Map Piont）投影到当前帧，利用最小化重投影误差来进行优化。

TrackReferenceKeyFrame()（跟踪上一关键帧）：在运动模型还未建立或者刚刚进行了重定位的时候，首先通过BOW进行匹配来加快当前帧和上一关键帧的匹配，然后利用把关键帧的位姿设定为当前帧的位姿，通过最小化重投影误差进行优化。

跟踪局部地图：局部地图跟踪就是通过更多的3D点(Map Point)进行约束，然后优化得到更好的位姿。

Relocalization（）（重定位）：首先通过BOW进行特征匹配，选出候选关键帧（在ORB-SLAM中所有关键帧都会被记录到BOW数据库中）如果匹配数目超过15个则进行EPnP求解，通过5次迭代RNASAC得到相机初始值，然后进行优化相机位姿。如果优化后匹配点小于50，则通过投影对之前未匹配的点进行再次计算。

TrackLocalMap()（局部地图跟踪）：首先需要建立一个局部地图，局部地图是指与当前帧相邻的关键帧（定义相邻为重合特征点数大于15）。跟踪局部地图也就是跟踪更多的3D点来建立约束，使其优化效果更好。

NeedNewKeyFrame()（是否需要关键帧）：很长时间没有插入关键帧、局部地图空闲、跟踪快要跪、跟踪地图MapPoints的比例比较少在这几种情况下当前帧可以认为是关键帧。ORB-SLAM的关键帧准入是比较轻松的，有点类似于国外大学，宽进严出。

CreateNewKeyFrame()（生成关键帧）：把当前帧构造为关键帧，同时把当前关键帧设为当前帧的参考帧。

 

接下来将进入局部地图线程

局部地图线程主要包括处理新的关键帧、Map Point剔除，新的Map Point生成、局部优化以及关键帧剔除这五个部分。

 

ProcessNewKeyFrame()（处理新的关键帧）：首先从缓冲区读取一个关键帧，然后计算该关键帧的BOW映射，将当前关键帧与局部Map Point进行关联，更新关键帧的连接关系，然后将当前帧插入到地图当中。

MapPointCulling()（Map Point剔除）：已经是坏点的MapPoints直接从检查链表中删除，能找到该点的帧少于理论上观测到该点的帧的1/4，从创建该map point开始到现在已经过了至少2个关键帧，但是观察到该点的关键帧不超过2个。

CreateNewMapPoints()(生成新的Map Point): 相机运动过程中与相邻关键帧通过三角化恢复出一些MapPoints。首先在当前帧的共视关键帧中找到共视程度最高的20帧相邻帧，遍历这20帧，当基线足够长的时候，根据两帧的位姿计算运动矩阵，通过极限约束进行匹配，然后三角化，随后将这些点放入检测。所有生成的Map Point都需要进行检测。

SearchInNeighbors()(对Map Point进行融合)：首先找到与当前帧共识点最多的前20帧，再找出这20帧公示点最多的5帧，将这些关键帧的Map Point与当前帧进行融合，更新当前帧Map Point的描述子.Map point保存下列信息： 它的3D世界坐标系位置;视图方向Ni, 这是它的所有视图方向的平均单位向量（视图方向为那些连接观测到这个点的关键帧的光心和这个点的射线）;一个代表ORB 描述符 Di, 它是在这个Map point 被观测到的那些关键帧里面的所有相关联描述符里，加权平均距离最小的一个关联描述符;根据ORB特征的尺度不变限制条件， 这个点能够被观测到的最大距离 dmax 和最小距离 dmin.最后更新当前帧的MapPoints后更新与其它帧的连接关系。

LocalBundleAdjustment()(局部优化);首先将当前帧加入关键帧，找到它的一级关联帧(共同Map Point大于10)加入到lLocalKeyFrames，遍历所有相邻关键帧，将他们观测到的Map Point都加入到lLocalMapPoints当中，找到能被LocalMapPoints观测到的关键帧但是不属于LocalKeyFrame的关键帧，将其固定，优化过程中不进行优化。把关键帧和Map Point做为图优化的顶点，边的观测值为对应特征的坐标信息。

KeyFrameCulling()(关键帧剔除)：根据Covisibility Graph提取当前帧的共视关键帧，提取每个共视关键帧，提取每个共识关键帧的Map Point,遍历该局部关键帧的Map Point计算是否90%都能被其他帧观测到，如果是，则剔除该帧。

然后就进入了回环检测阶段

主要由检测回环、计算Sim3,、闭环融合以及优化Essential Graph四个大的部分所组成。

 

 

DetectLoop() (检测回环)：首先从队列中取出一个关键帧，如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测。否则就遍历所有的共识关键帧计算所有共识关键帧的得分。mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF,
minScore)这个函数就是用于选择出候选关键帧的，具体操作如下：将与当前帧相连的局部关键帧剔除然后遍历所有关键帧，找出与当前关键帧具有相同单词的关键帧，然后统计所有闭环候选帧中与当前关键帧具有共同单词最多的单词数，将最多单词的80%设置为阈值，然后找出所有单词书超过阈值，且相似度检测大于相邻关键帧最低分数的关键帧。然后将这些关键帧和与他自己相邻最紧密的前10个关键帧设定为一组（注意这里的组是以每一个关键帧为中心，加上与其相邻的关键帧所形成的，一个关键帧可以在多个组里面）。然后计算每组的总得分以及每组得分最高的关键帧，然后以组得分最高的0.75作为阈值，找出高于这个阈值的所有组里面得分最高的帧，作为候选帧。此时这个函数结束，返回到detectLoop中，然后进行一致性检验。所谓一致性检验就是通过两个for循环将当前帧及其相邻帧与候选关键帧及其相邻帧进行匹配，也就是是否有相同的相邻关键帧，如果大于3那么就认为当前帧通过检测。但是此时还是有一些候选的关键帧。

ComputeSim3()(Sim3求解):对于每一个候选帧都进行求解。首先通过BOW进行匹配，如果改帧的匹配特征点少于20，则直接删除，然后进行sim3求解，求解之后进行inliner检测，只要有一次的是合格的就可以返回，最多只能迭代5次。然后通过求得的Sim3完善通过BOW进行的匹配，然后算出大致的匹配区域再进行Sim3优化，在优化过程中，先迭代5次，然后剔除误差过大的边（卡方检验，这里设定的阈值是10），然后继续对剩下的边进行优化，然后得到优化结果，只要优化后的nInliers大于等于20就认为通过回环检验。同时停止对其他候选帧的优化。然后取出闭环匹配上关键帧（也就是刚刚通过回环检验的这一帧）的相邻关键帧提取出来得到一个集合（同时把匹配上的关键帧也加入到这个集合中），再把它们所对应的Map Point取出来（这里也包括匹配上的关键帧的Map Point，不过这个需要标记一下，避免重复添加），得到一个集合。然后把这些Map Point全部投影到当前关键帧上，进行匹配，根据Sim3确定一个大致区域，然后在附近区域搜索，得到匹配。然后根据匹配判断是否超过40个匹配点，如果超过则认为匹配成功，否则将有Local Mapping送进来的队列清空，等待下一次。

 

 

CorrectLoop()（闭环校正）：首先通知局部地图，让他停止关键帧的插入。然后根据更新之后的共视关系更新当前帧与其它关键帧的联系，根据位姿传播得到与当前帧相连关键帧闭环后的Sim3，然后根据得到的闭环Sim3（也就是每一个关键帧需要调整的位姿）更新Map Point的位置，将sim3转换为SE3，修正闭环帧的位姿。然后再根据共视关系，重新更新各个关键帧的连接关系。然后再检查当前帧的Map Point与闭环匹配帧的MapPoint是否存在冲突，对冲突的Map Point进行替换或填补。然后通过将闭环时相连关键帧的Map Point投影到这些关键帧中，进行Map Point检查与替换（如有冲突以闭环帧及其相邻帧的Map Point为准，这是因为前期可以认为没有误差积累，后期这些冲突是由于在运行中产生的积累误差）。再次更新图关系，得到了因闭环Map Point融合之后得到的图关系。这里面需要注意一下，更新后的连接关系分为两个部分（一部分是本来就有的连接关系，另外一部分是由于闭环之后所产生的连接关系）

 

OptimizeEssentialGraph()(EssentialGraph优化)：这里的优化对象由三个部分组成，扩展树连接关系、闭环产生的连接关系和一些共识关系非常好的边（这里设定的是共识权重超过100的），由着三部分组成的图进行优化。

GlobalBundleAdjustemnt()（全局优化）:最后建立一个全局优化，优化所有的关键帧和Map Point.