这是我根据看泡泡机器人吴博的ORB-SLAM2源码详解视频做的一些笔记，因为主要是作为自己的笔记使用，所以会有点乱。大家可以看吴博在B站上面的视频：https://www.bilibili.com/video/BV1h441177pY?from=search&seid=17501489641807485361 讲的还是不错的。 大家也可以参考刘国庆师兄的注释，写的也是非常仔细，链接是：https://github.com/DreamWaterFound/self_commit_ORB-SLAM2
如果我写的有什么不对，还希望大家批评指正。
四个主要线程的代码主要放置的位置：Tracking.cpp、LocalMapping.cpp、LoopClosing.cpp、Viewer.cpp
变量命名规则： P：表示指针数据类型、n：表示int类型、b：表示bool类型、s表示set类型、v：表示vector数据类型、l：表示list数据类型、m：表示类成员变量member、KF表示KeyPoint数据类型。
ORB-SLAM的入口是System.cpp程序，这也是主进程的实现文件，这个文件主要的程序包括：1.系统的构造函数System()，将会启动其他的线程；2.双目输入时的跟踪接口TrackStereo(左侧图像，右侧图像，时间戳)；3.当输入图像为RGBD时进行追踪TrackRGBD()；4.输入位单目图像时的追踪器接口TrackMonocular()；5.ActivateLocalizationMode()为激活定位模式，DeactivateLocalizationMode()为取消定位模式，MapChanged()判断是否地图有较大的变化，Reset()准备执行复位，Shutdown()退出；6.SaveTrajectoryTUM()按照TUM格式保存相机运行轨迹并保存到指定的文件之中（只有当传感器为双目或者RGBD时才可以工作）;7.SaveKeyFrameTrajectoryTUM()保存关键帧轨迹，SaveTrajectoryKITTI()按照KITTI数据集的格式将相机的运动轨迹保存到文件中；8.GetTrackingState()获取跟踪器状态，GetTrackedMapPoints()获取追踪到的地图点（其实实际上得到的是一个指针），GetTrackedKeyPointsUn()获取追踪到的关键帧的点。
而整个程序实现主要的步骤是： 1.（从该步起代码主要位于Tracking.cpp中）输入图像(分别针对立体相机、RGBD相机和单目相机)(括号里为一些主要的参数)： GrabImageStereo(ImRectLeft,imRectRight,timestamp)：将RGB或者RGBA图像转换为灰度图像；构造Frame；跟踪。 GrabImageRGBD(imRGB,imD,timestamp)：与上类似，加一步将深度相机的disparity转为Depth , 也就是转换成为真正尺度下的深度。 GrabImageMonocular(im,timestamp) 2.转换为灰度图像得到一些主要的参数： Stero:mImGray,imGrayRight RGBD:mImGray,imDepth Mono:mImGray 3.构造Frame(mpIniORBextractor相比mpORBextractorLeft提取的特征点多一倍)： Stero:Frame(mImGray.imGrayRight,mpORBextractorLeft,mpORBextractorRight) RGBD:Frame(mImGray,imDepth,mpORBextractorLeft) Mono(未初始化):Frame(mImGray,mpIniORBextractor)、Mono(已初始化):Frame(mImGray,mpORBextractorLeft) 4.Track:数据流进入Tracking线程(VO线程开始)（将Frame传到下一个步骤） 5.初始化(Tracking线程开始)： SteroInitialization():双目和RGB-D初始化：设定初始位姿；将当前帧构造为初始关键帧；在地图中添加该初始关键帧；为每个特征点构造MapPoint；在局部地图中添加该初始关键帧。 MonocularInitialization()：单目初始化：得到用于初始化的第一帧，初始化需要两帧；如果当前帧特征点数大于100，则得到用于单目初始化的第二帧，如果当前帧特征点太少，重新构造初始器；在mInitialFrame与mCurrentFrame中找匹配的特征点对；如果初始化的两帧之间的匹配点太少，重新初始化；通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints；删除那些无法进行三角化的匹配点；将三角化得到的3D点包装成MapPoints。 6.相机位姿跟踪（出现两种情况，跟踪失败和成功）（mbOnlyTracking默认为false，用户可以通过运行界面选择跟踪定位模式）： （1）mbOnlyTracking(False):会出现两种情况，一种情况是位姿跟踪，主要有TrackWithMotionModel()和TrackRerenceKeyFame()两个方法，这两种方法的主要区别见下面的图： TrackReferenceKeyFrame()：将当前帧的描述子转化为BoW向量；通过特征点的BoW加快当前帧与参考帧之间的特征点匹配；将上一帧的位姿态作为当前帧位姿的初始值；通过优化3D-2D的重投影误差来获得位姿；剔除优化后的outlier匹配点。 TrackWithMotionModel()：更新上一帧的位姿，对于双目或rgbd摄像头，还会根据深度值为上一关键帧生成新的MapPoints；根据匀速度模型进行对上一帧的MapPoints进行跟踪, 根据上一帧特征点对应的3D点投影的位置缩小特征点匹配范围；优化位姿；优化位姿后剔除outlier的mvpMapPoints。 另一种情况就是Relocalization()重定位，主要的方法实现见下面的图： Relocalization()：计算当前帧特征点的Bow映射；找到与当前帧相似的候选关键帧；通过BoW进行匹配；通过EPnP算法估计姿态；通过PoseOptimization对姿态进行优化求解；如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解。 （2）mbOnlyTracking(true):同时跟踪与定位，不插入关键帧，局部地图不工作。 7.局部地图跟踪： （1）UpdateLocalMap():主要分为UpdateLocalKeyframes()和UpdateLocalPoints()， UpdateLocalPoints()：清空局部MapPoints；遍历局部关键帧；将局部关键帧的MapPoints添加到mvpLocalMapPoints。 UpdateLocalKeyFrames()：遍历当前帧的MapPoints，记录所有能观测到当前帧MapPoints的关键帧；更新局部关键帧。 （2）SearchLocalPoints()：获得局部地图和当前帧的匹配：遍历当前帧的mvpMapPoints，标记这些MapPoints不参与之后的搜索；将所有局部MapPoints 投影到当前帧，判断是否在视野范围内，然后进行投影匹配。 （3）PoseOptimization():最小化投影误差优化位姿 8.是否生成关键帧： 在很长时间内没有插入关键帧、局部地图空闲、跟踪快要跪的时候都要插入关键帧 跟踪地图MapPoints的比例比较少 9.生成关键帧（生成m1NewKeyFrames向下一级传）： KeyFrame(mCurrenFram,mpMap,mpKeyFrameDB) 对于双目或者RGBD摄像头构造一些MapPoints,为MapPoints添加属性 10.(以下步骤的代码位于LocalMapping.cpp线程)检查队列：CheckNewKeyFrames()：查看列表中是否有等待被插入的关键帧。 11.处理新关键帧： ProcessNewKeyFrame():更新MapPoints与KeyFrame的关联；UpdateConnections() ProcessNewKeyFrame()：从缓冲队列中取出一帧关键帧；计算该关键帧特征点的Bow映射关系；跟踪局部地图过程中新匹配上的MapPoints和当前关键帧绑定；更新关键帧间的连接关系；将该关键帧插入到地图中。 12.删除MapPoints:MapPointCulling()，删除地图中新添加的但是质量不好的MapPoints:a.IncreasFound/IncreaseVisible<25%;b.观测到该点的关键帧太少。 13.生成MapPoint:运动过程中和共视图程度比较高的关键帧通过三角化恢复出一些MapPoints，CreateNewMapPoints()：在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻帧vpNeighKFs；遍历相邻关键帧vpNeighKFs；判断相机运动的基线是不是足够长；根据两个关键帧的位姿计算它们之间的基本矩阵；通过极线约束限制匹配时的搜索范围，进行特征点匹配；对每对匹配通过三角化生成3D点。 14.MapPoints融合：检查当前关键帧和相邻（两极相邻）重复的MapPoints 15.Local BA:和当前关键帧相邻的关键帧以及MapPoints做局部BA优化，SearchInNeighbors()：获得当前关键帧在covisibility图中权重排名前nn的邻接关键帧；将当前帧的MapPoints分别与一级二级相邻帧(的MapPoints)进行融合；将一级二级相邻帧的MapPoints分别与当前帧（的MapPoints）进行融合；更新当前帧MapPoints的描述子，深度，观测主方向等属性。 16.关键帧删除：其中90%以上的MapPoints能被其他共视关键帧（至少三个）观测到的关键帧（生成mlploopKeyFrameQueue送到下一级中） 17.（以下步骤的程序位于LocalClosing.cpp线程，闭环检测）从队列中取一帧作为mpcurrentKF，并判断距离上一次闭环检测是否超过10帧，计算当前帧与u相连关键帧的Bow最低得分，由此得到mpcurrentKF和minscore. 18.检测得到闭环候选帧： (pKF,minscore)找出与当前帧有公共单词的关键帧，但不包括与当前帧相连的关键帧（1KFsharingWords）；统计候选帧中与pKF具有共同单词最多的单词数（maxcommonwords）;得到阀值（mincommons=0.8maxcommonwords）(maxcommonwords,mincommnts,minscore),筛选共有单词大于mincommons且Bow得分大于mincore的关键帧（lscoreAndMatch）；将存在相连的分为一组，计算组最高得分bestAccScore,同时得到每组中得分最高的关键帧（IsAccScoreAndMatch,bestAcoScore）;得到阀值minSoreToRetain=0.75bestAccScore(IsAccScoreAndMatch,minSoreToRetain);vpLoopGandidates. 20.（vpLoopCandidates）检测候选帧的连续性 21.Sim3计算 SearchByBow(vvpMapointMatches);构造Sim3求解器，对匹配成功的关键帧进行Sim3求解（mpCurrentKF,pKFScm–>(R,t,s),vpMapointMatches）;SearchBySim3得到更多的匹配；优化Sim3;将MatchedKF共视帧取出;将MatchedKF共视帧取出；SearchByProjection得到更多的匹配点。 22.correctLoop: 暂停LocalMapping防止插入新的关键帧（mpCurrentKF）;更新当前帧的共视相连关系（mpCurrentKF）;得到与当前帧相连的关键帧（mvpcurrentConnectedKFs（包括当前关键帧），mpCurrentKF的mg2oScw）;根据传播得到与关键帧相连关键帧闭环后的Sim3；调整当前相连关键帧的MapPoints;将Sim3转为SE3，并调整关键帧位姿；更新关键帧链接关系(mvpCurrentMatchedPoints–>ploopMapPoint,mpCurrentKF的MapPoint–>pCurrentMapPoint);用pLooMapPoints替换pCurrentMapPoints(correctedSim3,mvpLooopMapPoints);将这些关键帧的MapPoints与闭环MapPoints融合（mvpCurrentConnectedKFs）;更新关键帧连接关系（LoopConnections）;得到由闭环形成的连接关系（LoopConnections,mpMap,mpMatchedKF,mpCurrentKF,correctedSim3,NocorrectedSim3）; OptimizeEssentialGraph;新建一个线程进行全局BA优化
Frame.cpp 一、双目立体匹配： 1.为左目每个特征点建立带状区域搜索表，限定搜索区域。（已提前极线校正） 2.通过描述子进行特征点匹配，得到每个特征点最佳匹配点scaleduR0 3.通过SAD滑窗得到匹配修正量bestincR 4.(bestincR.dist)(bestincR-1,dist)(bestincR+1,dist)三个点拟合出抛物线，得到亚像素修正量deltaR 5.最终匹配点位置为：scaleduR0+bestincR+deltaR 二、Disparity与Depth（对于RGB-D来说）:
单目Initializer.cpp
ORBmatcher.cpp 1.ORB-SLAM2中特征点匹配均采用各种技巧减小特征点匹配范围 2.ORB-SLAM2中特征点通过描述子匹配后会进行旋转一致性检测，并且最佳匹配点要明显优于次匹配点； 3.ORB-SLAM2中特征点提取仍然是非常耗时的地方。 SearchByProjection函数和SearchBySim3函数利用将相机坐标系下的MapPoints投影到图像坐标系，在投影点附近搜索匹配。 SearchByBoW函数通过判断特征点的word的node是否相同可以加速匹配过程。 SearchForTriangulation函数利用对极几何约束：左目一个点对应右目一条线。将左图像的每个特征点与右图像同一node节点的所有特征点一次检测，判断是否满足对极几何约束，满足约束就是匹配的特征点。 Fuse和SearchByProjection函数差不多，只不过是判断特征点p的MapPoint是否与MapPoint点P冲突。
