ORB-SLAM(四)追踪

最近在读ORB-SLAM的代码,虽然代码注释算比较多了,但各种类和变量互相引用,看起来有点痛苦。索性总结了一下Tracking部分的代码结构,希望能抓住主要思路,不掉坑里。

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

以下介绍SLAM模式下的追踪算法。

主要流程

初始追踪(Init pose estimation)

作者在追踪这部分主要用了几种模型:运动模型(Tracking with motion model)、关键帧(Tracking with reference key frame)和重定位(Relocalization)。

下面一一介绍。

Tracking with motion model

假设物体处于匀速运动,那么可以用上一帧的位姿和速度来估计当前帧的位姿。上一帧的速度可以通过前面几帧的位姿计算得到。这个模型适用于运动速度和方向比较一致,没有大转动的情形下,比如匀速运动的汽车、机器人、人等。而对于运动比较随意的目标,当然就会失效了。此时就要用到下面两个模型。

Tracking with reference key frame

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

Relocalization

假如当前帧与最近邻关键帧的匹配也失败了,意味着此时当前帧已经丢了,无法确定其真实位置。此时,只有去和所有关键帧匹配,看能否找到合适的位置。首先,计算当前帧的Bow向量。其次,利用BoW词典选取若干关键帧作为备选(参见ORB-SLAM(六)回环检测);再次,寻找有足够多的特征点匹配的关键帧;最后,利用特征点匹配迭代求解位姿(RANSAC框架下,因为相对位姿可能比较大,局外点会比较多)。如果有关键帧有足够多的内点,那么选取该关键帧优化出的位姿。

姿态优化(Track local map)

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

更新局部地图(Local mapping thread)

这里简单提一下local mapping thread。Tracking成功以后,需要更新motion model,并判断当前帧是否是新的关键帧。如果是,将其加入并更新局部地图(local map),建立当前关键帧与其它关键帧的连接关系,更新当前关键帧与其它关键帧之间的特征点匹配关系,并利用三角法生成新的三维点,最后做一个局部优化(local BA,包括相邻关键帧和它们对应的三维点,参见ORB-SLAM(五)优化)。

程序

作者的这部分程序逻辑判断很多,如果读者对作者的程序有兴趣,那么可以参照下图来阅读追踪这部分的程序。

Map is active应改为Localization is active。从Localization is active开始,NO代表SLAM模式,YES代表Localization模式。

 

该系列的其它文章:

ORB-SLAM(一)简介

ORB-SLAM(二)性能

ORB-SLAM(三)初始化

ORB-SLAM(五)优化

ORB-SLAM(六)回环检测

 

posted @ 2016-04-08 14:18  路游侠  阅读(25446)  评论(4编辑  收藏  举报