ORB-SLAM2 源码学习 <一> 源码框架简析
最近一段时间在读ORB-SLAM2的源码，下面是对源码整体流程的一点记录，基本类似于论文中的流程图，通过阅读源码，加深了自己对该流程的理解。
/*
* cpp文件分类：
* 初始化： Initializer.cpp System.cpp
* 基本类型：Frame.cpp KeyFrame.cpp MapPoint.cpp Map.cpp (KeyPoint用的是OpenCV中的类型)
* 主要线程：Tracking.cpp -> LocalMapping -> LoopClosing
* 后端优化：PnPsolver.cpp Sim3Solver.cpp Optimizer.cpp
* ORB相关：ORBmatcher.cpp ORBextractor.cpp
* 绘图相关：FrameDrawer.cpp MapDrawer.cpp Viewer.cpp
* 其他： KeyFrameDatabase.cpp Converter.cpp
*/
/*
* 从第一帧图像开始，解析代码运行流程：
*
* Map包括：KeyFrame，MapPoint
* 其中，KeyFrame之间形成两张图：
共视图 Covisibility Graph
最大生成树 Essential Graph (Tree)
* KeyFrame和MapPoint之间的关系通过各自的变量记录：
KeyFrame中，vector<MapPoint*> mvpMapPoints 记录与它相关的MapPoint
MapPoint中，map<KeyFrame*,size_t> mObservations 记录指向它的KeyFrame以及该MapPoint在该KeyFrame中的索引
*
* LocalMap包括：KeyFrame，MapPoint
可以理解为，LocalMap是针对Frame来说的，它存储的KeyFrame和MapPoint都和当前Frame有一定关系
LocalMap随着帧的改变不断由 Tracking::UpdateLocalMap()进行更新，一直存储与当前Frame有关系的相关KeyFrame和MapPoint
KeyFrame：能观测到当前帧的MapPoint(Frame::mvpMapPoints集合)的KeyFrame（一级帧）和与一级帧共视程度较高的KeyFrame（二级帧），存储在Tracking::vector<KeyFrame*> mvpLocalKeyFrames中
MapPoint：所有上述KeyFrame所包含的MapPoint集合，存储在Tracking::vector<MapPoint*> mvpLocalMapPoints中
* 思考： 要找到当前帧的LocalMap,有一个前提条件： 能正确判断出当前帧的MapPoint可以被哪些KeyFrame观测到
Tracking::UpdateLocalMap()在更新局部地图找KeyFrame时，使用的是MapPoint::GetObservations()，直接获取哪些KeyFrame能观测到该MapPoint
也就是说，在当前Frame形成MapPoint时，已经通过某种方法知道该MapPoint能被哪些KeyFrame观测到，并存储于MapPoint::map<KeyFrame*,size_t> mObservations
这个方法是： Tracking::TrackReferenceKeyFrame()和Tracking::TrackWithMotionModel()，这两个模型把当前帧中与前一帧匹配成功的特征点变为MapPoint，存入Frame::mvpMapPoints，并为相应MapPoint添加观测
* Tracking线程：
Tracking::Track()：
1. 初始化（设置第一帧为KeyFrame，其中的特征点为MapPoint）
Tracking::StereoInitialization()
Tracking::MonocularInitialization()
Tracking::CreateInitialMapMonocular()
成功初始化：mState = OK
2. 检查上一帧中的MapPoint是否被替换
Local Mapping线程可能会将关键帧中某些MapPoints进行替换
由于Tracking中需要用到mLastFrame，这里检查并更新上一帧中被替换的MapPoints
Tracking::CheckReplacedInLastFrame()
Tracking::mLastFrame
MapPoint::GetReplaced() 返回用于替换的MapPoint引用
3. 对以后的每一帧，使用不同的模型将当前帧与前一帧进行匹配，获取MapPoint，求解初始位姿
Tracking::TrackReferenceKeyFrame()
ORBmatcher::SearchByBoW()
Frame::mvpMapPoints 该容器被更新 Frame记录MapPoint
MapPoint::Observations() MapPoint记录Frame
Tracking::TrackWithMotionModel()
Tracking::UpdateLastFrame() 对RGBD或双目相机，为上一帧恢复出一些临时的MapPoint，可提高相邻两帧的匹配成功率
Tracking::mlpTemporalPoints 这些MapPoint被标记为临时的，在Tracking::CreateNewKeyFrame()之前删除
Tracking::mLastFrame
ORBmatcher::SearchByProjection()
Frame::mvpMapPoints 该容器被更新 Frame记录MapPoint
MapPoint::Observations() MapPoint记录Frame
Optimizer::PoseOptimization() 初步位姿优化
4. 基于当前帧查找局部地图，并进一步优化当前帧位姿
Tracking::TrackLocalMap()
Tracking::UpdateLocalMap() 更新局部地图
Tracking::UpdateLocalKeyFrames() 更新局部KeyFrame
Tracking::mvpLocalKeyFrames 保存局部地图中的KeyFrame
Tracking::UpdateLocalPoints() 更新局部MapPoint
Tracking::mvpLocalMapPoints 保存局部地图中的MapPoint
Tracking::SearchLocalPoints() 局部地图中查找与当前帧匹配的MapPoint
Tracking::mvpLocalMapPoints
Frame::isInFrustum() 判断局部地图中是否有在当前帧视野内的点
ORBmatcher::SearchByProjection() 将局部地图中的MapPoint与当前帧做投影匹配
Frame::mvpMapPoints 该容器被更新， 这里没有在匹配上的MapPoint中添加该帧的观测(LocalMapping NO.2)
Optimizer::PoseOptimization() 进一步位姿优化
如果获得的匹配点过少:mState = LOST
5. 更新恒速模型的关键变量Tracking::mVelocity为当前帧位姿
删除第3步Tracking::UpdateLastFrame()中临时添加的MapPoint
6. 检查是否插入KeyFrame
Tracking::NeedNewKeyFrame() 关键帧插入的一些判别条件
LocalMapping::KeyframesInQueue() 检查关键帧候选队列中的数量
Tracking::CreateNewKeyFrame() 将当前帧构造成关键帧，并放入候选队列中
LocalMapping::InsertKeyFrame()
LocalMapping::mlNewKeyFrames
7. 如果mState = LOST, 先进行重定位：Tracking::Relocalization()
不成功则进行系统复位：Tracking::Reset()
* LocalMapping线程：
LocalMapping::Run():
1. 标记当前线程繁忙，不直接影响Tracking线程添加关键帧至候选队列
LocalMapping::SetAcceptKeyFrames()
LocalMapping::mbAcceptKeyFrames
检查候选队列中是否有还有待处理的关键帧
LocalMapping::CheckNewKeyFrames()
LocalMapping::mlNewKeyFrames
2. 取一帧候选帧开始处理
LocalMapping::ProcessNewKeyFrame()
KeyFrame::ComputeBoW() 计算该关键帧特征点的BoW映射关系
KeyFrame::GetMapPointMatches() 将在跟踪局部地图时新匹配上的MapPoint和当前关键帧进行绑定(Tracking NO.4)
KeyFrame::mvpMapPoints
LocalMapping::mlpRecentAddedMapPoints 当前帧自己生成的MapPoint放在此处，等待检查 何时自己生成？？？Tracking NO.3中RGBD和双目生成的MapPoint已被删除
KeyFrame::UpdateConnections() 更新关键帧之间的连接关系，Covisibility图和Essential图(tree)
Map::AddKeyFrame() 将当前关键帧插入地图中
Map::mspKeyFrames
3. MapPoint剔除，剔除ProcessNewKeyFrame和CreateNewMapPoints函数中引入的质量不好的MapPoint
LocalMapping::MapPointCulling()
LocalMapping::mlpRecentAddedMapPoints 待挑选的MapPoint集合
MapPoint::SetBadFlag() 将不符合条件的MapPoint标记为Bad，并彻底删除
MapPoint::mbBad
MapPoint::mObservations
KeyFrame::EraseMapPointMatch()
Map::EraseMapPoint()
4. 通过三角化，使用与当前帧共视程度比较高的一些关键帧恢复出一些MapPoint，并为MapPoint和相应两帧关键帧相互添加观测
遍历共视关键帧的过程中，如果候选队列中还有其他待处理关键帧，则退出该函数
LocalMapping::CreateNewMapPoints()
KeyFrame::GetBestCovisibilityKeyFrames() 获取共视程度比较高的关键帧
LocalMapping::CheckNewKeyFrames() 判断是否还有其他待处理关键帧，如果有，则退出该函数
LocalMapping::ComputeF12() 计算两个关键帧之间的基本矩阵F
ORBmatcher::SearchForTriangulation() 极线约束 特征点匹配
对匹配成功的特征点，三角化恢复MapPoint *** 此处大量数学运算
LocalMapping::mlpRecentAddedMapPoints 将新产生的点放入检测队列，等待检测
5. 若已经处理完队列中的最后一个关键帧，则检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints
LocalMapping::SearchInNeighbors()
KeyFrame::GetBestCovisibilityKeyFrames() covisibility图中获取一定数量的与当前关键帧有共视关系的关键帧，并将一级帧与二级帧分开
ORBmatcher::Fuse() 对每一个相邻帧，将当前关键帧中的所有MapPoint与其进行融合，消除重复MapPoint
KeyFrame::GetMapPointMatches() 获取相邻帧的MapPoint集合
ORBmatcher::Fuse() 将相邻帧MapPoint集合与当前帧进行融合
KeyFrame::UpdateConnections() 更新关键帧之间的连接关系，Covisibility图和Essential图(tree)
6. 若已经处理完队列中的最后一个关键帧，且闭环检测进程没有请求停止LocalMapping
LocalMapping::stopRequested()
Optimizer::LocalBundleAdjustment() 进行局部BA优化
LocalMapping::KeyFrameCulling() 找到与当前关键帧具有共视关系的所有关键帧，剔除冗余关键帧，标准：该关键帧的90%的MapPoints可以被其它关键帧(至少3个)观测到
KeyFrame::SetBad() 标记关键帧为Bad
7. 将当前关键帧插入闭环检测队列，是否真正对该帧执行闭环检测，由LoopClosing::DetectLoop()决定
LoopClosing::InsertKeyFrame()
8. 标记线程空闲
* LocalClosing线程：
LocalClosing::run()
1. 若闭环检测队列中有候选帧
LoopClosing::DetectLoop() 判断是否需要对当前帧进行闭环检测
KeyFrame::GetVectorCovisibleKeyFrames() 获取共视关键帧
ORBVocabulary::score() 计算当前帧与每一个相邻帧的BoW相似度得分
KeyFrameDatabase::DetectLoopCandidates() 找出闭环候选帧
在候选帧中找出连续候选帧 复杂逻辑
KeyFrameDatabase::add() 将当前帧加入数据库
2. 若需要进行闭环检测
LoopClosing::ComputeSim3() 计算当前帧与闭环帧的相似变换Sim3
3. 形成闭环
LoopClosing::CorrectLoop()
KeyFrame::UpdateConnections() 根据共视关系更新当前帧与其它关键帧之间的连接
调整与当前帧相邻的关键帧的位姿及MapPoint坐标
Optimizer::OptimizeEssentialGraph() 优化EssentialGraph
*/
