ORB-SLAM2程序导图

以思维导图形式，梳理ORB-SLAM2程序的框架和步骤

mono_tum.cc

• System::LoadImages() （从数据集中加载所有图像的名称和 timestamp）
• 创建 ORB_SLAM2::System 对象
  • 加载 ORB Vocabulary
  • 创建 KeyFrame Database
  • 创建 Map
  • 创建 FrameDrawer 和 MapDrawer
  • 创建 Tracking 对象
  • 创建 LocalMapping 对象及线程
    • LocalMapping::Run()
  • 创建 LoopClosing 对象及线程
    • LoopClosing::Run()
• main loop
  • 读入一帧图像
  • System::TrackMonocular()
    • Tracking::GrabImageMonocular(im)
      • 转化为灰度图
      • Frame (创建 Frame 对象)
        • 提取 ORB 特征 （如果是初始化时的帧，则提取两倍特征点数）
      • Tracking::Track()
• System::Shutdown()
• System::SaveKeyFrameTrajectoryTUM() （将计算得到的数据集中 KF 的轨迹存储下来）
• 标记说明
  • 加粗字体：表示程序主框架
  • 加粗字体下的（非加粗字体）：表示对加粗字体及其子内容的解释
  • （非加粗字体）：表示对其子内容的概括性解释
  • 带颜色的内容：表示与 ORB-SLAM 论文中系统概览图相对应的每个部分
• 简称说明
  • KF：表示 KeyFrame
  • Current KF：表示当前 KeyFrame
  • Covisible KF：表示在 Covisibility Graph 中与 Current KF 相连的 KFs
  • Loop KF：表示回环 KeyFrame，即和 Current KF 相匹配的 KeyFrame Database 中的 KF
  • Candidate KF：表示可能是 Current KF 的 Loop KF 的候选 KF
  • Reference KF：表示距离当前帧最近的上一 KF
  • KF Database：表示用来存储 KFs 的 Database，这些 KFs 会用于回环检测和重定位
• Global BA 线程
  • LoopClosing::RunGlobalBundleAdjustment()
• LoopClosing 线程
  • LoopClosing::Run()
    • while(1)
      • if(LoopClosing::CheckNewKeyFrames()) （New KFs 队列中 有新送入且未处理的 KF）
        • 取出 New KFs 队首元素作为 Current KF
          • （回环检测 —— 检测 Candidate KFs） （筛选出一些 Candidate KFs）
            • LoopClosing::DetectLoop()
              • 之前刚进行完回环检测的10帧 KFs 内，不进行回环检测，且这10帧 KFs 不会添加进 KF Database
              • 计算 Current KF 与其 Covisible KFs 的最低BoW相似分数 s_min
                • TemplatedVocabulary::score()
              • vpCandidateKFs = KeyFrameDatabase::DetectLoopCandidates() （初步提取 Candidate KFs）
                • 在 KF Database 中找到所有和 current KF 含有任意相同 word 的 KFs （但不包括 Current KF 的 Covisible KFs）
                  • for (Current KF 含有的每一个 word)
                    • for ( KF Database 中含有该 word 的每一个 KF)
                • maxCommonWords = max(上述候选 KFs 中与当前 KF 具有的共同 words)
                • 找出 Candidate KFs 中与 Current KF (共同 words 数 > 0.8 * maxCommonWords) 且 (相似分数 > s_min) 的所有 KFs
                • for (上述所有符合要求的 KFs)
                  • accScore = sum(该 KF 的所有 Covisible KFs 的相似分数)
                  • pBestKF = 该 KF 的所有 Covisible KFs 中相似分数最高的 KF
                • 计算 bestAccScore = max(accScore)
                • vpLoopCandidates = 上述所有 (accScore > 0.75 * bestAccScore 的 KF) 的 pBestKF
                • return vpLoopCandidates
              • if (vpCandidateKFs 为空) （未检测到回环，等待下一 KF）
                • KeyFrameDatabase::add(Current KF)
                • return false
              • for (pCandidateKF in vpCandidateKFs) （进一步筛选 Candidate KFs：进行连续性检测，该 Candidate KF 需要在 Covisibility Graph 中与三个以上 Candidate KFs 以 Group 形式相连）
                • spCandidateGroup = pCandiidateKF->GetConnectedKeyFrames() + 该 KF （将该 Candidate KF 和其 Covisible KF 组成一个 Group）
                • for (mvConsistentGroups 中的 Groups)
                  • for (spCandidateGroup 中的 KFs)
                    • if (spCandidateGroup 中的该 KF 存在于 mvConsistentGroups 中的该 Group)
                      • bConsistent = true
                      • bConsistentForSomeGroup = true
                      • break
                  • if (bConsistent)
                    • 该 Group 的 Consistency += 1
                    • if (该 Group 没有与之前的 Candidate KFs 的 spCandidateGroup 匹配过)
                      • 将 spCandidateGroup 和其 Consistency 添加进 vCurrentConsistentGroups
                    • if (Consistency >= 3)
                      • 将 pCandidateKF 添加至 mvpEnoughConsistentCandidates （这些 Candidate KFs 通过了 LoopClosing::DetectLoop()，储存下来）
                • if (bConsistentForSomeGroup == false)
                  • 将 spCandidateGroup 添加至 vCurrentConsistentGroups，Consistency = 0
                • mvConsistentGroups = vCurrentConsistentGroups;
              • KeyFrameDatabase::add(Current KF)
          • if(DetectLoop())
            • （回环检测 —— 计算 Sim3） （从上一步筛选出的 Candidate KFs 中确定最终的 Loop KF）
              • LoopClosing::ComputeSim3()
                • for (Candidate KF in mvpEnoughConsistentCandidates) （DetectLoop() 中初步筛选出的 Candidate KFs）
                  • ORBmatcher::SearchByBoW()
                  • 筛选
                  • 设置 Sim3 求解器，并设置参数
                • while (RANSAC 迭代)
                  • for (所有通过上一步筛选的 Candidate KFs)
                    • Sim3Solver::iterate()
                    • if (求解出解)
                      • ORBmatcher::SearchBySim3()
                      • Optimizer::OptimizeSim3()
                      • if (优化得到的 nInliers >= 20)
                        • 得到 mpMatchedKF 及 mScw （满足要求的 Candidate KF 及当前 KF 闭环后的 Sim3 位姿）
                        • break RANSAC 迭代
                • if (上一步没有找到匹配当前 KF 的 合适 Candidate KF)
                  • 回环检测失败，放弃当前 KF，清除相关变量
                  • return false
                • 提取 mpMatched 及其 Covisible KFs 中含有的 MapPoints
                • mvpLoopMapPoints = ORBmatcher::SearchByProjection()
                • if(经过上述步骤后，匹配点总数)
                  • >= 40
                    • 接受该回环，回环检测成功，Loop KF 为 mpMatchedKF
                    • return true
                  • < 40
                    • 回环检测失败，放弃 Current KF，清除相关变量
                    • return false
            • if(CommputeSim3())
              • LoopClosing::CorrectLoop() （回环检测成功后，进行回环校正）
                • 通知 LocalMapping 暂停，防止插入新的 KF
                • 放弃正在运行 Global BA （若有）
                • 等待 LocalMapping 线程停止
                • KeyFrame::UpdateConnections()
                • for (Current KF 及其 Covisible KFs) （计算闭环后 Current KF 附近的 Sim3）
                  • 计算该 KF 闭环后的 Sim3，并记录至 CorrectedSim3
                  • 记录该 KF 闭环前 的 Sim3 至 UnCorrectedSim3
                • for (上述所有 pair(KF 和其 Sim3)) （对Current KF 附近 KFs 的 MapPoints 的坐标进行调整）
                  • for (该 KF 的所有有效 MapPoints)
                    • MapPoint::SetWorldPos()
                  • Sim3 转化为 SE3
                  • KeyFrame::SetPose()
                  • KeyFrame::UpdateConnections()
                • （回环校正 —— 回环融合）
                  • for( Current KF 的 KeyPoints 与 回环 KF 的 MapPoints 的 matches) （先融合 Current KF 和 Loop KF 的MapPoints）
                    • if (该 KeyPoint 本身对应了 MapPoint)
                      • 用 Loop KF 的 MapPoints 替换 该 KeyPoint 本身的MapPoint
                    • if (该 KeyPoint 原来没有对应的 MapPoint)
                      • KeyFrame::AddMapPoint(回环 KF 的 MapPoint)
                      • 回环 KF 的 MapPoint
                        • MapPoint::AddObservation()
                        • MapPoint::ComputeDistinctiveDescriptors()
                  • LoopClosing::SearchAndFuse(CorrectedSim3) （再匹配融合 Current KF 和 Loop KF 各自 Convisible KFs 的 MapPoints
                    • for (Current KF 及其 Covisible KFs)
                      • ORBmatcher::Fuse(该 KF, mvpLoopMapPoints) （mvpLoopMapPoints 是回环 KF 及其 Covisible KFs 的 MapPoints）
                      • 计算该 KF 的哪些 MapPoints 需要被替换
                      • 用 mvpLoopMapPoints 替换上述 MapPoints
                  • （更新 KFs 连接关系，得到由闭环形成的连接关系）
                    • for (Current KF 及其 Covisible KFs)
                      • KeyFrame::UpdateConnections()
                • （优化 Essential Graph）
                  • Optimizer::OptimizeEssentialGraph()
                • （添加回环边）
                  • Loop KF -> AddLoopEdge(Current KF)
                  • Current KF -> AddLoopEdge(Loop KF)
                • 创建 Global BA 线程 （做一次最终的优化，锦上添花）
                  • LoopClosing::RunGlobalBundleAdjustment()
                • LocalMapping::Release()
• LocalMapping 线程
  • LocalMapping::Run()
    • while(1)
      • （插入 KF）
        • LocalMapping::SetAcceptKeyFrames(false) （通知 Tracking 线程，LocalMapping 线程正忙）
        • LocalMapping::CheckNewKeyFrames()
      • if(LocalMapping::CheckNewKeyFrames()) （New KFs 队列中 有新送入且未处理的 KF）
        • LocalMapping::ProcessNewKeyFrame()
          • 取出 New KFs 队首元素
            • KeyFrame::ComputeBoW()
            • 更新 MapPoints 与 KeyFrame 的关联
            • KeyFrame::UpdateConnections()
            • Map:::AddKeyFrame()
        • （剔除质量较差的 MapPoints）
          • LocalMapping::MapPointCulling() （剔除标准可见论文）
        • （在 Map 中生成新的 MapPoints）
          • LocalMapping::CreateNewMapPoints() （通过三角化，在Map 中生成新的 MapPoints）
        • if (LocalMapping::CheckNewKeyFrames() == false) （如果所有新送入的 KFs 都经过上述处理了）
          • LocalMapping::SearchInNeighbors() （检查 Current KF 与其 Covisible KFs 是否有重复构建的 MapPoints，并进行融合）
            • 找到 Current KF 的 Covisible KFs
            • for (每一个 Covisible KFs)
              • ORBmatcher::Fuse(Covisible KF, Current KF 的 MapPointMatches )
            • ORBmatcher::Fuse(当前 KF, 所有相邻 KFs 的有效 MapPointMatches)
            • 更新 MapPoints 的信息
              • MapPoint::ComputeDistinctiveDescriptors()
              • MapPoint::UpdateNormalAndDepth()
        • if (LocalMapping::CheckNewKeyFrames() == false && 其他线程没有让 LocalMapping 线程暂停)
          • （Local BA）
            • Optimizer::LocalBundleAdjustment()
          • （剔除冗余的 KFs）
            • LocalMapping::KeyFrameCulling() （剔除标准可见论文）
        • LoopClosing::InsertKeyFrame() （筛选后的 KFs 送入 LoopClosing 线程）
      • LocalMapping::SetAcceptKeyFrames(true)
• Tracking 线程
  • Tracking::Track()
    • if(mState) == NO_IMAGES_YET (读入第一帧图像时)
      • mSate = NOT_INITIALIZED （去做初始化）
    • if(mState) （mState 是当前（上一帧）的追踪状态）
      • == NOT_INITIALIZED
        • （初始化）
          • MonocularInitialization() （单目的初始化程序）
            • 第一有效帧设为 Initial Frame 创建 Initializer 对象
            • 第二有效帧设为 Current Frame
              • 设为 Current Frame
            • ORBmatcher::SearchForInitialization() （Initial Frame 与 Current Frame 之间做特征点匹配）
            • if (如果匹配特征点对数 < 100)
              • 放弃当前的Initial Frame 和 Current Frame，等待新帧进行初始化
            • Initializer::Initialize()
              • 两个线程同时计算 （同时计算 IF 和 CF 之间的 Fundmental Matrix 和 Homography）
                • Initializer::FindHomography()
                • Initializer::FindFundamental()
              • if(RH = SH/(SH+SF)) （计算使用两种不同模型的重投影得分，哪个高，就用哪个模型计算出 R 和 t）
                • > 0.4
                  • ReconstructH()
                • < 0.4
                  • ReconstructF()
            • Tracking::CreateInitialMapMonocular() （以 Initial Frame 作为地图坐标原点，创建初始的单目地图，地图初始化）
              • （将 Initial Frame 和 Current Frame 作为 KF 插入 Map 中）
                • 创建 KeyFrame 对象
                • KeyFrame::ComputeBoW()
                • Map::AddKeyFrame()
              • for (每一个有效匹配点) （根据两个 KFs 匹配的特征点生成 MapPoints）
                • 创建 MapPoint 对象
                • KeyFrame::AddMapPoint()
                • MapPoint::AddObservation()
                • MapPoint::ComputeDistinctiveDescriptors()
                • MapPoint::UpdateNormalAndDepth()
                • Map::AddMapPoint()
              • KeyFrame::UpdateConnections()
              • Optimizer::GlobalBundleAdjustmnt() （使用一次 Global BA 来对 KFs 位姿和 MapPoints 坐标进行优化）
              • 对 points depth和 t 的尺度进行归一化
              • LocalMapping::InsertKeyFrame()
              • Map::SetReferenceMapPoints()
              • 设置 mState = OK
      • == OK or LOST （系统已经过初始化，进入常规运行）
        • （相机位姿初值跟踪估计） （根据运动模型或上一 Reference Frame 计算出相机位姿的初值， 便于在局部地图跟踪中进行进一步优化）
          • if (mbOnlyTracking) （SLAM 模式 / Localization 模式）
            • == SLAM 模式
              • if(mSate)
                • == OK
                  • Tracking::CheckReplacedInLastFrame()
                  • if(运动模型还未建立 or 刚刚进行了重定位)
                    • == true
                      • Tracking::TrackReferenceKeyFrame() （根据 Reference KF 进行估计）
                        • Frame::ComputeBoW()
                        • ORBmatcher::SearchByBoW()
                        • Optimizer::PoseOptimization()
                    • == false
                      • Tracking::TrackWithMotionMode() （根据运动模型（速度不变模型）进行估计）
                        • 当前帧 -> SetPose(上一帧位姿变化量 * 上一帧位姿)
                        • ORBmatcher::SearchByProjection()
                        • Optimizer::PoseOptimization()
                      • if (追踪失败)
                        • Tracking::TrackReferenceKeyFrame()
                • == LOST
                  • Tracking::Relocalization() （Tracking LOST 后，需要进行重定位，将当前帧与 KeyFrameDatabase 中的 KF 进行匹配）
                    • Frame::ComputeBoW()
                    • KeyFrameDatabase::DetectRelocalizationCandidates(当前帧)
                    • for (每一个 Candidate KF) （使用 BoW 匹配初步筛选出一些有足够匹配 words 的 Candidate KFs）
                      • ORBmatcher::SearchByBoW()
                      • 筛选
                      • 创建 PnPsolver 对象，并设置参数
                    • while (RANSAC 迭代) （通过 RANSAC 迭代求解并匹配，找到最合适的 Candidate KF）
                      • for (每一个筛选后的 Candidate KF)
                        • PnPsolver::iterate() （少量迭代）
                        • nGood = Optimizer::PoseOptimization()
                        • if(nGood)
                          • < 10
                            • continue (不会确定该 Candidate KF 为匹配关键帧)
                          • 10 < nGood < 50
                            • ORBmatcher::SearchByProjection()
                            • if(上一步匹配后成功匹配对 > 50)
                              • nGood = Optimizer::PoseOptimization()
                              • if(nGood > 30 & < 50)
                                • ORBmatcher::SearchByProjection()
                                • if(上一步匹配后成功匹配对 > 50)
                                  • nGood = Optimizer::PoseOptimization()
                        • if(nGood) >= 50
                          • mnLastRelocFrameId = mCurrentFrame.mnId （将当前帧设为 Tracking 线程中上一次进行 Relocalization 的帧）
                          • break
              • 根据上述步骤的追踪结果设置 bOK 为 OK 还是 LOST （bOK 为当前帧 暂时的 追踪情况）
            • == Localization 模式 （此时 LocalMapping 线程不工作，即不插入关键帧）
              • if(mSate)
                • == OK
                  • if(mbVO) （VO 模式 / 正常模式，VO 模式表示上一帧追踪到的大部分是 VO 点）
                    • == 正常模式
                      • if(运动模型已建立)
                        • == false
                          • Tracking::TrackReferenceKeyFrame()
                        • == true
                          • Tracking::TrackWithMotionMode()
                    • == VO 模式
                      • if(运动模型已建立)
                        • == true
                          • Tracking::TrackWithMotionMode()
                      • Trackng::Relocalization()
                      • if(TrackWithMotionMode() 成功， 同时 Relocalization() 失败)
                        • 使用 TrackWithMotionMode() 的结果
                      • if(Relocalization()成功)
                        • 使用 Relocalization() 的结果
                        • mbVO = false （从 VO 模式切换为正常模式）
                • == LOST
                  • Tracking::ReLocalization()
        • （局部地图跟踪）
          • if (mbOnlyTracking)
            • == SLAM 模式
              • if(当前帧 Tracking OK)
                • Tracking::TrackLocalMap()
                  • Tracking::UpdateLocalMap()
                    • Tracking::UpdateLocalKeyFrames()
                    • Tracking::UpdateLocalPoints()
                  • Tracking::SearchLocalPoints()
                  • Optimizer::PoseOptimization()
                  • 判断 tracking 是否成功
            • == Localization 模式
              • if(当前帧 Tracking OK && mbVO == false ) （VO 模式没有局部地图）
                • Tracking::TrackLocalMap()
          • 根据上述步骤的追踪结果设置 bOK 为 OK 还是 LOST
          • 根据 bOK 设置 mState 为 OK 还是 LOST
          • FrameDrawer::Update()
        • （决定是否生成关键帧，并插入关键帧）
          • if(bOK)
            • == LOST
              • if (Map 中的 KeyFrame 少于5帧) （如果系统才刚初始化就 LOST 了，重新初始化吧）
                • System::Reset()
            • == OK
              • 更新 运动模型
              • MapDrawer::SetCurrentCameraPose()
              • 清除 VO 生成的临时性 MapPoints
              • if (Tracking::NeedNewKeyFrame()) （SLAM模式下才会有 LocalMapping 线程，另外其他判断标准可见论文）
                • Tracking::CreateNewKeyFrame()
                  • LocalMapping::InsertKeyFrame()
    • 存储计算得到的相机位姿等各种信息