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