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

*/

posted @ 2018-03-05 16:45  vh_pg  阅读(604)  评论(0编辑  收藏  举报