ORB-SLAM2详解(四)跟踪
本人邮箱:[email protected],欢迎交流讨论,
欢迎转载,转载请注明网址http://blog.csdn.net/u010128736/
这一部分是ORB_SLAM系统中最基本的一步,会对每一帧图像进行跟踪计算。Tracking线程运行在主线程中,主要思路是在当前帧和(局部)地图之间寻找尽可能多的对应关系,来优化当前帧的位姿。每次新采集到一帧图像,就是用下列接口将图像传入SLAM系统就行处理。该代码位于主程序中:
// Pass the image to the SLAM system
SLAM.TrackMonocular(im,tframe);
在检查完系统将模式切换为跟踪模式后,是用下面接口进入功能:
mpTracker->GrabImageMonocular(im,timestamp);
一、ORB提取
本文做匹配的过程中,是用的都是ORB特征描述子。先在8层图像金字塔中,提取FAST特征点。提取特征点的个数根据图像分辨率不同而不同,高分辨率的图像提取更多的角点。然后对检测到的特征点用ORB来描述,用于之后的匹配和识别。跟踪这部分主要用了几种模型:运动模型(Tracking with motion model)、关键帧(Tracking with reference keyframe)和重定位(Relocalization)。
二、从前一帧初始化位姿估计
在成功与前面帧跟踪上之后,为了提高速率,本文使用与之前速率相同的运动模式来预测相机姿态,并搜索上一帧观测到的地图点。这个模型是假设物体处于匀速运动,例如匀速运动的汽车、机器人、行人等,就可以用上一帧的位姿和速度来估计当前帧的位姿。使用的函数为TrackWithMotionModel()。这里匹配是通过投影来与上一帧看到的地图点匹配,使用的是matcher.SearchByProjection()。
if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
bOK = TrackReferenceKeyFrame();
}
else
{
bOK = TrackWithMotionModel();
if(!bOK)
bOK = TrackReferenceKeyFrame();
}
当使用运动模式匹配到的特征点数较少时,就会选用关键帧模式。即尝试和最近一个关键帧去做匹配。为了快速匹配,本文利用了bag of words(BoW)来加速。首先,计算当前帧的BoW,并设定初始位姿为上一帧的位姿;其次,根据位姿和BoW词典来寻找特征匹配,使用函数matcher.SearchByBoW();最后,利用匹配的特征优化位姿。
三、通过全局重定位来初始化位姿估计
假如使用上面的方法,当前帧与最近邻关键帧的匹配也失败了,那么意味着需要重新定位才能继续跟踪。重定位的入口如下:
bOK = Relocalization();
此时,只有去和所有关键帧匹配,看能否找到合适的位置。首先,计算当前帧的BOW向量,在关键帧词典数据库中选取若干关键帧作为候选。使用函数如下:
// Relocalization is performed when tracking is lost
// Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
其次,寻找有足够多的特征点匹配的关键帧;最后,利用RANSAC迭代,然后使用PnP算法求解位姿。这一部分也在Tracking::Relocalization() 里。
四、局部地图跟踪
通过之前的计算,已经得到一个对位姿的初始估计,我们就能透过投影,从已经生成的地图点中找到更多的对应关系,来精确结果。函数入口为:
bOK = TrackLocalMap();
为了降低复杂度,这里只是在局部图中做投影。局部地图中与当前帧有相同点的关键帧序列成为K1,在covisibility graph中与K1相邻的称为K2。局部地图有一个参考关键帧Kref∈K1,它与当前帧具有最多共同看到的地图云点。针对K1, K2可见的每个地图云点,通过如下步骤,在当前帧中进行搜索:
(1)将地图点投影到当前帧上,如果超出图像范围,就将其舍弃;
(2)计算当前视线方向向量v与地图点云平均视线方向向量n的夹角,舍弃n·v < cos(60°)的点云;
(3)计算地图点到相机中心的距离d,认为[dmin, dmax]是尺度不变的区域,若d不在这个区域,就将其舍弃;
(4)计算图像的尺度因子,为d/dmin;
(5)将地图点的特征描述子D与还未匹配上的ORB特征进行比较,根据前面的尺度因子,找到最佳匹配。
这样,相机位姿就能通过匹配所有地图点,最终被优化。
五、关键帧的判断标准
最后一步是确定是否将当前帧定为关键帧,由于在Local Mapping中,会剔除冗余关键帧,所以我们要尽快插入新的关键帧,这样才能更鲁棒。这个部分代码为:
// Check if we need to insert a new keyframe
if(NeedNewKeyFrame())
CreateNewKeyFrame();
确定关键帧的标准如下:
(1)在上一个全局重定位后,又过了20帧;
(2)局部建图闲置,或在上一个关键帧插入后,又过了20帧;
(3)当前帧跟踪到大于50个点;
(4)当前帧跟踪到的比参考关键帧少90%。