ORB-SLAM2跟踪之局部地图跟踪

回顾:帧间匹配得到初始位姿

上一次主要说明了ORB-SLAM2跟踪过程中每帧初始位姿的估计。回顾一下,初始位姿的估计主要是在帧与帧之间进行,有三种方法来估计,TrackWithMotionModel,TrackReferenceKeyFrame,Relocalization。

TrackWithMotionModel假设相机做恒速运动,根据前一帧的位姿加上恒速位移推算出当前帧的位姿,之后遍历前一帧的特征点,根据推算出的当前帧位姿,将特征点对应的3D地图点投影到当前帧,在投影点附近搜索特征点进行匹配,从而缩小特征点匹配搜索范围,加快匹配速度,这一过程通过SearchByProjection()函数实现。最终得到一组匹配好的3D地图点-2D特征点。然后进行Bundle Adjustment,最小化重投影误差来进行当前帧位姿优化,得到初始位姿,优化函数为:Optimizer::PoseOptimization(&mCurrentFrame);

TrackReferenceKeyFrame是在运动模型还未建立或者刚完成重定位时,通过BoW进行当前帧与参考关键帧特征点匹配,这一过程通过SearchByBoW()函数实现。得到一组3D地图点-2D特征点匹配后,与TrackWithMotionModel一样,优化3D-2D的重投影误差来得到初始位姿,这里在优化前将上一帧的位姿作为当前帧的初始位姿,可以使得优化收敛加快。

Relocalization是在跟踪丢失时,将当前帧与候选关键帧通过BoW进行特征点匹配,之后通过EPnP算法估计位姿,和前两种方法一致,再通过最小化重投影误差对位姿进行优化。

以上三种方法都是基于帧间匹配,优化3D-2D重投影误差得到当前帧的初始位姿。这样得到的初始位姿不是可靠的,其只利用了两帧数据的信息,如果前一帧质量太差,得到的位姿可信度低。因此为了利用更多的信息,需要进一步将当前帧与局部地图进行匹配和优化,也就是TrackLocalMap()

局部地图跟踪

随着相机运动,我们向局部地图添加新的关键帧和3D地图点来维护局部地图,这样,即使跟踪过程中某帧出现问题,利用局部地图,我们仍然可以求出之后那些帧的正确位姿。以下是进行局部地图跟踪的流程:

更新局部地图

局部地图更新UpdateLocalMap()主要包括:对关键帧的更新UpdateLocalKeyFrames()和局部地图点的更新UpdateLocalPoints()

更新局部关键帧mvpLocakKeyFrames步骤如下:

  1. 搜索具有共视关系的关键帧。由之前的帧间匹配,我们得到当前帧的地图点(MapPoint),通过遍历这些地图点,得到也能观测到这些地图点的关键帧(地图点上记录了共视信息)。同时记录关键帧的共视次数。关键帧和共视次数存储在map keyframeCounter;
  2. 将步骤1得到的关键帧全部插入到mvpLocakKeyFrames
  3. 遍历步骤1keyframeCounter中每个关键帧,得到与之共视程度最高的10个关键帧,也全部插入到mvpLocakKeyFrames
  4. 遍历步骤1keyframeCounter中每个关键帧,将每个关键帧的子关键帧(>=1)和父关键帧(=1,共视程度最高的之前的关键帧)都插入到mvpLocakKeyFrames
  5. 更新当前帧的参考关键帧,与自己共视程度最高的关键帧作为参考关键帧。

更新局部地图点mvpLocalMapPoints步骤如下:

  1. 清空mvpLocalMapPoints
  2. 遍历之前得到的局部关键帧mvpLocakKeyFrames,将局部关键帧的地图点插入到mvpLocalMapPoints

局部地图点匹配

局部地图点匹配SearchLocalPoints()将局部地图点与当前帧进行匹配。

步骤如下:

  1. 遍历当前帧的mvpMapPoints,标记这些MapPoints不参与之后的搜索。这些MapPoints是在帧间匹配过程中用到的。

  2. 将局部地图点mvpLocalMapPoints投影到当前帧,投影矩阵是通过帧间匹配得到。判断局部地图点是否在当前帧的视野内(视野内,mbTrackInView 为true),只有视野内的地图点才进行投影匹配。判断要求包括:投影点是否在图像内;地图点到相机中心距离是否在尺寸变换范围内;相机中心到地图点向量与地图点平均视角夹角是否小于60°。

  3. 对视野内的地图点通过投影进行特征匹配:

    matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);,得到一组3D局部地图点-2D特征点匹配对。

BundleAdjustment

Bundle Adjustment最小化重投影误差优化相机位姿:Optimizer::PoseOptimization(&mCurrentFrame);

可以看出,这几步和TrackWithMotionModel类似,只不过TrackWithMotionModel匹配点对是通过帧间匹配得到的,而TrackLocalMap是剔除TrackWithMotionModel中的匹配点,从局部地图中找新的更多的匹配点,从而实现对位姿的进一步优化。

你可能感兴趣的:(ORB-SLAM算法)