ORB_SLAM2学习之源码分析一-追踪


这篇文章是有关ORB_SLAM2系统源码分析的内容,主要记录Tracking模块部分。

ORB-SLAM2系统追踪、局部建图、回环检测、可视化四个线程,其中追踪模块是在主线程中完成的,SLAM视觉里程计主体就是在该线程中完成的。先介绍追踪模块的算法内容,这里从已经完成模块初始化开始。ORB_SLAM2中,重定位和闭环检测过程主要使用DBoW2来完成。

Tracking代码分析

程序分为两种模式:SLAM模式Localization模式,由变量mbOnlyTracking标记。SLAM模式中,三个线程全部都在工作,即在定位也在建图。而Localization模式中,只有Tracking线程在工作,即只定位,输出追踪结果(姿态),不会更新地图和关键帧。Localization模式主要用于已经有场景地图的情况下(在SLAM模式下完成建图后可以无缝切换到Localization模式)。Localization模式下追踪方法涉及到的关键函数是一样的,只是策略有所不同。

上图是参考资料1中的流程图,介绍的比较详细,可供参考。

初始追踪

初始化完成后,对于相机获取当前图像mCurrentFrame,通过跟踪匹配上一帧mLastFrame特征点的方式,可以获取一个相机位姿的初始值;为了兼顾计算量和跟踪鲁棒性,追踪部分主要用了三种模型:运动模型(TrackWithMotionModel)、关键帧(TrackReferenceKeyFrame)和重定位(Relocalization)。三种跟踪模型都是为了获取相机位姿一个粗略的初值,后面会通过跟踪局部地图TrackLocalMap对位姿进行BundleAdjustment(捆集调整),进一步优化位姿。

  1. TrackWithMotionModel

    该模型根据两帧之间的约束关系来求解估算位姿。假设物体处于匀速运动,那么可以用上一帧的位姿和速度来估计当前帧的位姿。上一帧的速度可以通过前面几帧的位姿计算得到。这个模型适用于运动速度和方向比较一致、没有大转动的情形,比如匀速运动的汽车、机器人、人等。如果是静止状态或者运动模型匹配失效(运用恒速模型后反投影发现LastFrame的地图点和CurrentFrame的特征点匹配很少),通过增大参考帧的地图点反投影匹配范围,获取较多匹配后,计算当前位姿;而对于运动比较随意的目标,上述操作失效的情况下,就会用到下面两个模型。

  2. TrackReferenceKeyFrame

    假如motion model已经失效,那么首先可以尝试和最近一个关键帧(即参考关键帧)去做匹配。毕竟当前帧和上一个关键帧的距离还不是很远。作者利用了bag of words(BoW)来加速匹配。首先,计算当前帧的BoW,并设定初始位姿为上一帧的位姿;其次,根据位姿和BoW词典来寻找特征匹配(参见ORB-SLAM(六)回环检测);最后,利用匹配的特征优化位姿(参见ORB-SLAM(五)优化)。

  3. Relocalization

    使用DBoW2实现。假如当前帧与最近邻关键帧的匹配也失败了,意味着此时当前帧已经丢了,无法确定其真实位置。此时,只有去和所有关键帧匹配,看能否找到合适的位置。

    重定位的过程大概是这样的:

    1. 计算当前帧的特征BoW向量;
    2. 利用BoW词典,根据词袋模型的特征匹配度,在关键帧数据库中找到与当前图像帧相似的候选关键帧,使用KeyFrameDatabase::DetectRelocalizationCandidates注意这里是参考普通图像帧(当前图像帧)寻找候选关键帧,与回环检测过程不同,回环检测使用参考关键帧去寻找闭环候选帧,使用KeyFrameDatabase::DetectLoopCandidates,所以两种情况选取候选关键帧的策略不同);
    3. 通过BoW匹配当前帧和每一个候选关键帧,如果匹配数足够,进行EPnP求解;
    4. 对求解结果使用BA优化,如果内点较少,则反投影当前帧的地图点到候选关键帧获取额外的匹配点;若这样依然不够,放弃该候选关键帧,若足够,则将通过反投影获取的额外地图点加入,再进行优化。
    5. 如果内点满足要求(>50)则成功重定位,将最新重定位的id更新:mnLastRelocFrameId = mCurrentFrame.mnId;  否则返回false。

TrackLocalMap

一旦我们通过上面三种模型获取了初始的相机位姿和初始的特征匹配,就可以将完整的地图(地图点)投影到当前帧中,去搜索更多的匹配。但是投影完整的地图,在large scale的场景中是很耗计算而且也没有必要的,因此,这里使用了局部地图LocalMap来进行投影匹配。对局部地图的更新包括对局部关键帧和局部地图点的更新。局部地图包含:与当前帧相连的关键帧K1(所有能观察到当前帧对应地图点的关键帧,图中Pos2),以及与K1相连的关键帧K2(一级二级相连关键帧,图中Pos1),并且限制了关键数量不超过80;K1、K2对应的地图点(图中X1,貌似X0不包括在内,为啥??);参考关键帧Kf。下图局部地图就是红色椭圆圈出的部分,参与局部优化,其中红色代表取值会被优化,灰色代表取值保持不变。

匹配过程如下:  

  1. 抛弃投影范围超出相机画面的;  
  2. 抛弃观测视角和地图点平均观测方向相差60o以上的;  
  3. 抛弃特征点的尺度和地图点的尺度(通过高斯金字塔层数表示)不匹配的;  
  4. 计算当前帧中特征点的尺度;  
  5. 将地图点的描述子和当前帧ORB特征的描述子匹配,需要根据地图点尺度在初始位姿获取的粗略x投影位置附近搜索;  
  6. 根据所有匹配点进行PoseOptimization优化。

位姿优化

姿态优化部分的主要思路是在当前帧和(局部)地图之间寻找尽可能多的对应关系,来优化当前帧的位姿。实际程序中,作者选取了非常多的关键帧和地图点。在跑Euroc数据集MH_01_easy时,几乎有一半以上的关键帧和地图点(后期>3000个)会在这一步被选中。然而,每一帧中只有200~300个地图点可以在当前帧上找到特征匹配点。这一步保证了非关键帧姿态估计的精度和鲁棒性。

其他操作

在处理完mCurrentFrame的跟踪定位后,需要更新motion model,并判断当前帧是否是新的关键帧

新的关键帧的创建

以下条件必须同时满足,才可以加入关键帧,但是其实ORB-SLAM中关键帧的加入是比较密集的,这样确保了定位的精度,同时在LocalMapping线程最后会进行关键帧的剔除,又确保了关键帧的数量不会无限增加,不会对large scale的场景造成计算负担。但是,ORB的作者又提到了,Tracking中除了提取特征点,TrackLocalMap也挺耗时,可以通过减少关键帧的数量来降低Local Map的规模,提高Tracking速度(但是精确度可能降低)。

  1. 距离上一次重定位距离至少20帧;
  2. 局部地图线程空闲,或者距离上一次加入关键帧过去了20帧;如果需要关键帧插入(过了20帧)而LocalMapping线程忙,则发送信号给LocalMapping线程,停止局部地图优化,使得新的关键帧可以被及时处理(20帧,大概过去了不到1s);
  3. 当前帧跟踪至少50个点;确保了跟踪定位的精确度;
  4. 当前帧跟踪到LocalMap中参考帧的地图点数量少于90%;确保关键帧之间有明显的视觉变化。

如果满足了创建关键帧的条件,在Tracking::CreateNewKeyFrame()函数中完成关键帧创建,将关键帧传递到LocalMapping线程(mpLocalMapper->InsertKeyFrame(pKF)),再由LocalMapping完成其他工作。此外,在创建关键帧之后,对于双目、RGB-D的情况,会使用深度值大于0的关键点重投影得到地图点,得到的地图点会和当前关键帧关联,并加入到Map

注意:

这里只是判断是否需要将当前帧创建为关键帧,并没有真的加入全局地图,因为Tracking线程的主要功能是局部定位,而处理地图中的关键帧、地图点,包括如何加入、如何删除的工作是在LocalMapping线程完成的,这里也可以看出作者的思路是比较清楚的,Tracking负责localization,LocalMapping负责Mapping,就构建了粗略的完整SLAM框架,然后加入初始化和闭环检测以及一些可视化模块,形成完整的SLAM。

总结

追踪模块是实现SLAM框架中视觉里程计模块的主体部分,其主要过程包括:初始化、初始追踪(初始位姿估计)、局部地图追踪(进一步的位姿估计)、局部优化、决定是否创建新的关键帧等。初始位姿估计仅仅完成了视觉里程计中的帧间追踪,该过程要么选择上一帧,要么选择参考关键帧,要么从全局关键帧数据库中选取候选关键帧与当前帧进行特征匹配,分别进行恒速运动模型(通过投影上一帧的地图点到当前帧,实现投影匹配)、参考关键帧追踪模型(通过投影参考关键帧的地图点到当前帧,实现投影匹配)、重定位模型(用DBow2实现匹配)。此外,还需要进行局部地图的追踪(通过投影局部地图点到当前帧,实现投影匹配),提高精度。

小记

在Relocalization和LoopClosing中进行匹配的是在很多帧关键帧集合中匹配,属于Place
Recognition,因此需要用DBow;

投影匹配适用于两帧之间,或者投影范围内(局部地图,前一个关键帧对应地图点)的MapPoints与当前帧之间,恒速运动模型、参考关键帧追踪模型、局部地图追踪模型都是使用的投影匹配。

后续再深入学习其他内容

  1. ORB特征提取(涉及到Fast关键点检测、rBRIEF描述子、SIFT特征)
  2. 尺度空间、金字塔、变化尺度(尺度因子)
  3. DBoW2、BoW向量
  4. 以上在ORB_SLAM中的应用

  5. 相机运动估计

地图点投影(匹配)、三角测量

2D-2D:对极几何(本质矩阵 单应矩阵)

http://zhehangt.win/2017/03/05/SLAM/EpipolarGeometry/

参考资料

  1. 视觉slam十四讲P153
  2. 计算机视觉算法与应用P264
  3. 视觉slam十四讲P141
  1. 双目立体成像与畸变校正(双目矫正)
  1. 三角剖分

参考资料

  1. http://www.cnblogs.com/luyb/p/5357790.html
  2. https://www.cnblogs.com/shang-slam/p/6395514.html

本文标题:ORB_SLAM2学习之源码分析一-追踪

文章作者:阿翔

发布时间:2018年08月12日 - 20:08

最后更新:2019年05月28日 - 21:05

原始链接:http://ttshun.com/2018/08/12/ORB_SLAM2学习之源码分析一-追踪/

许可协议: 署名-非商业性使用-禁止演绎 4.0 国际 转载请保留原文链接及作者。

点击给我一些鼓励叭!
-------------本文结束感谢您的阅读-------------
0%