ORB-SLAM代码理解及笔记

用户详细介绍了学习ORB-SLAM的过程，包括算法的核心逻辑和关键步骤。用户提到花了三天时间理清代码逻辑，重点学习了Local Mapping和Loop Closing线程，以及初始化方法。用户还参考了吴博和冯兵的视频和PPT，学习了ORB-SLAM的代码结构和优化方法。文本详细描述了算法的流程，包括检测回环、计算Sim3、闭环融合和优化Essential Graph等关键步骤。用户还提到使用泡泡机器人平台学习，对泡泡机器人平台的学习效果表示赞赏。摘要涵盖了用户对ORB-SLAM的学习过程、算法流程和关键点的总结。

最近，我正在学习ORBSLAM算法。ORBSLAM方案在SLAM领域占据着举足轻重的地位。经过三天时间，我对代码的运行逻辑和整体架构有了较为清晰的认识。然而，具体的实现细节尚未深入研究。因此，可能存在一些不足之处，恳请大家批评指正。此外，我参考的资料还包括了泡泡机器人平台提供的吴博和冯兵两位老师的视频以及配套的PPT演示文稿。在此，对泡泡机器人平台提供的学习资源表示由衷的感谢，从中受益匪浅。

在论文中，我曾对这幅图未给予过多关注，但后来才意识到它的逻辑非常清晰，堪称上乘。此外，orb-slam代码的逻辑同样清晰可见，Local Mapping和Loop Closing线程中都包含一个run函数，可以直观地找到每一步在执行什么操作。

我研读了吴博老师的PPT，内容条理清晰，于是为了加深对知识的记忆，我又重新绘制了吴博的整个流程图。虽然绘制得不够完美，但加深了对知识的记忆，同时加深了对知识的理解也非常不错。另外，在冯兵老师的PPT中，我发现了可以帮助快速理清frame、feature和map point之间关系的图。

随后，我着手编写代码的流程图，并参考了各方面的资料整理了一些说明。之后，我主要集中在单目开发上，没有涉及双目和立体效果的相关内容。

这个过程相当于预处理的一个步骤，提到了一个单目初始化技术，是实现高效运行的关键技术，该算法通过自动完成单目初始化来实现。

在单目构造帧的过程中，未完成初始化时，提取的特征数量将是初始化后的两倍。单目初始化过程中，有两种方案可供选择：首先，计算单应矩阵和基础矩阵的R,t；其次，通过计算两者的分数进行比较，选择得分较高的方案。

随后立即进入tracking线程的处理流程。请解释：英文函数名与对应的中文解释或理解。

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

基于单目初始化的方法，当两帧的特征点数量均超过100时，即可作为初始化的基础。随后进行特征点匹配。若两帧之间的匹配特征点数量不足，则重新确定两帧。通过H模型或F模型进行单目初始化，获得两帧间的相对运动参数和初始MapPoints。随后删除无法三角化的点，最终获得完整的Map Point。

在跟踪模块中，有三种跟踪类型：匀速运动跟踪、实时跟踪关键帧并进行重定位，以及实时更新局部地图信息。

基于TrackWithMotionModel的匀速运动跟踪模型：该模型假设被跟踪物体的运动为匀速，认为相邻帧之间的运动模型相同。通过确定初始位姿T和旋转r，将上一帧的三维点（Map Point）映射到当前帧，并通过最小化重投影误差进行优化以获得最优解。

在运动模型未建立或刚完成重定位时，TrackReferenceKeyFrame()首先通过B bags of visual words（B bags of visual words）进行匹配，以加快当前帧与上一关键帧之间的匹配速度。随后，将关键帧的位姿更新为当前帧的位姿，并通过最小化重投影误差进行优化。

局部地图跟踪技术：该技术通过引入更多3D点（Map Point）进行约束，经过优化计算，最终实现更精确的位姿估计。

Relocalization（重定位）过程如下：首先，基于BOW的特征匹配算法提取关键帧特征，并将所有关键帧信息存储到预训练的BOW数据库中。当匹配点数超过15个时，采用EPnP算法进行求解，通过5次迭代的RANSAC算法得到相机初始值。随后，对相机位姿进行优化。如果优化后的匹配点数少于50个，则通过投影计算对之前未匹配的点进行重新定位。

在实现过程中，首先需要建立一个局部地图，该局部地图被定义为与当前帧相邻的关键帧（定义相邻为重合特征点数大于15）。而跟踪局部地图的主要作用是通过跟踪更多的3D点来建立约束关系，从而使得整体的优化效果更为显著。

NeedNewKeyFrame()（是否需要关键帧）：在以下情况下，当前帧可以认为是关键帧：长时间未插入关键帧，局部地图空闲，跟踪接近极限，MapPoints比例较低。ORB-SLAM的准入机制较为宽松，类似于国外大学的宽进严出机制。

CreateNewKeyFrame()（生成关键帧的函数）：该函数生成并返回一个新的关键帧对象，并将当前帧设置为该关键帧的前一帧。

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

由局部地图线程负责处理新的关键帧，去除Map Point，同时新增Map Point的生成，并进行局部优化处理，最后删除关键帧。

ProcessNewKeyFrame()（处理新的关键帧）：首先从缓冲区读取一个关键帧，计算该关键帧的BOW表示，并将其与局部Map点建立关联。随后更新当前关键帧与相邻帧之间的连接关系，最后将当前帧插入到地图中。

MapPointCulling()（Map Point剔除）：已确定为无效点的MapPoints将直接从其所在检查链表中删除。该点的帧数少于理论值的1/4，自创建该MapPoint以来，已过去了至少两个关键帧，但实际观测到的该点关键帧数量不超过两个。

CreateNewMapPoints()用于生成新的Map Point：在当前帧的共视关键帧中找到共视程度最高的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后更新与其它帧的连接关系。

该算法采用局部优化策略，其核心思想是通过逐步精炼关键帧之间的几何关系来提升整体精度。具体而言，首先，将当前帧加入关键帧列表，找到与之关联的一级帧，这些帧的共同Map Point数量超过10，并加入到lLocalKeyFrames中。接着，遍历所有相邻的关键帧，将这些帧观测到的Map Point信息收集到lLocalMapPoints中。随后，识别那些虽然被LocalMapPoints观测到，但不属于LocalKeyFrame的关键帧，并将这些关键帧固定在原位，优化过程中不对其做调整。最后，将关键帧和Map Point作为图优化的节点，边上的观测值记录了对应特征的坐标信息。

KeyFrameCulling：关键帧剔除。从当前帧中提取其共视关键帧，进而提取每个共视关键帧的Map Point。接着，遍历该局部关键帧的Map Point，计算其是否能被至少90%的其他帧观测到。如果满足条件，则移除该帧。

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

该算法主要包含环检测模块、Sim3算法计算模块、闭环融合模块以及Essential Graph优化模块四个主要部分。

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

采用Sim3算法进行求解

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

该优化目标由三个关键部分构成：扩展树的连接关系、闭环产生的连接关系以及共识权重较高的边。这些要素共同构成了需要优化的图结构。

GlobalBundleAdjustment（全局优化）：最后，我们实施了一个全局优化方案，旨在优化所有关键帧和Map Point。

整个思路大致就是这样，还有很多细节可能不太完善，欢迎批评指正