ORB-SLAM2代码(三)定位

参考:orb-slam2源码注释版


上一讲到了SLAM中的建图(其本质是对MapPoint的管理),有了地图,我们就有了环境的struct,这样我们就可以通过特征点匹配、优化等一系列步骤进行定位了,这次将对orb-slam中的定位部分进行解析,其本质是对frame的处理,包括关键帧和普通帧.

文章目录

        • 1、设置当前帧的参考关键帧
        • 2、更新上一普通帧的位姿
        • 3、生成关键帧
        • 4、更新关键帧之间共视图
        • 5、关键帧的剔除


1、设置当前帧的参考关键帧

在设置当前帧的参考关键帧时(mCurrentFrame.mpReferenceKF),一般都会同时设置tracking线程的参考关键帧(mpReferenceKF). 涉及到的地方有:
1)初始化完成时.
2)在跟踪局部地图之前,使用上一次普通帧跟踪时生成的mpReferenceKF设置当前帧的参考关键帧.

mCurrentFrame.mpReferenceKF = mpReferenceKF;

3)TrackLocalMap() 函数,UpdateLocalKeyFrames() 函数选择共视程度最高的关键帧作为当前帧的参考关键帧. 注意,这里选择的并非时间上最近的,而是共视程度最高的.

if (pKFmax) {
	mpReferenceKF = pKFmax;
	mCurrentFrame.mpReferenceKF = mpReferenceKF;
}

4)CreateNewKeyFrame() 函数,使用新生成的关键帧设置

KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
mpReferenceKF = pKF;
mCurrentFrame.mpReferenceKF = pKF;

2、更新上一普通帧的位姿

TrackWithMotionModel() 函数, UpdateLastFrame() 函数, 使用上一普通帧的参考关键帧,更新上一帧的位姿,这里主要是考虑到上一普通帧的参考关键帧会在local mapping中优化后改变,所以才这么做的.

// Update pose according to reference keyframe
KeyFrame* pRef = mLastFrame.mpReferenceKF;
cv::Mat Tlr = mlRelativeFramePoses.back();
mLastFrame.SetPose(Tlr*pRef->GetPose());

3、生成关键帧

NeedNewKeyFrame() 函数用于判断是否需要插入关键帧,下面对需要满足的条件进行分析
条件1:
local Mapping 是否被 Loop Closure 冻结,关于线程调度,可以参考之前的博客:orb-slam2代码总结(一)线程调度,代码如下:

if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
        return false;

条件2:
条件2-1:
首先地图中的关键帧的数量大于mMaxFrames(等于相机的帧率,比如,30Hz),即从初始化开始后插入到Map中关键帧数目必须大于30,该条件才能被使能,也就说,如果当前Map中的关键帧数目少于30帧,那么这个条件永远不会成立,即插入关键帧不受该条件2的影响
条件2-2:
当Map中的关键帧数目多于30个时,那么整个条件2是否成立只与第一部分有关了,如果之前发生过重定位,那么需要在一秒之内,是肯定不允许插入关键帧的!

if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)
        return false;

条件3:
条件3-1
由下面三部分构成,下面三个条件是或的关系,所以只要一个成立,3-1就成立了
条件3-1-1:一秒钟过去了,还没有新的关键帧被插入,就需要考虑插入关键帧了

// 【Condition 1a】: More than "MaxFrames" have passed from last keyframe insertion
const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames; 

条件3-1-2:local mapping处于空闲状态,即现在该线程正在进行休眠,这个时候插入关键帧不会对local mapping中的 CheckNewKeyFrames() 函数造成影响

// 【Condition 1b】: More than "MinFrames" have passed and Local Mapping is idle                                                                      
const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);  

条件3-1-3:这个条件是为双目打造的,mnMatchesInliers是track with local map中匹配到的MapPoint数目,nRefMatches是当前帧的参考关键帧(即,与当前帧共视最多的关键帧)中所有MapPoint中nObs超过3的MapPoints(即表示那些稳定的MapPoint),
mnMatchesInliers条件成立,表示跟踪到的MapPoint数目已经不及关键帧中稳定的MapPoint数目的四分之一
ratioMap<0.3f表示 " m a t c h e s t o m a p " / " t o t a l m a t c h e s " "matches to map"/"total matches" "matchestomap"/"totalmatches",其中total matches= matches to map + visual odometry matches,即总数等于当前帧与地图中的MapPoint匹配数加上双目自己新生成的MapPoint.

// 【Condition 1c】: tracking is weak                                                                                                                                                          
const bool c1c =  mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || ratioMap<0.3f); 

条件3-2:
这个条件分成两部分,
当前帧匹配到的Map Point的个数不能超过参考关键帧(与当前帧拥有最多map point的关键帧)对应Map Point的90%
当前的帧匹配到的Map Point数目大于15

// 【Condition 2】: Few tracked points compared to reference keyframe. Lots of visual odometry compared to m                                                                           
const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio || ratioMap<thMapRatio) && mnMatchesInliers>15);

4、更新关键帧之间共视图

每一个关键帧都会维护一个共视图,根据共视程度,共视图包括下面三种:

1) Covisibility Graph (阈值15),主要用于:

  • local mapping线程中的 LocalBundleAdjustment() 优化,根据当前关键帧选择local map,即局部地图
  • SearchInNeighbors() 中用于fuse时,从当前关键帧的Covisibility Graph 选择前20个(双目是10个)作为一级相邻
  • 剔除关键帧,选取当前关键帧所有Covisibility Graph,对共视的关键帧进行筛选

2) Essential Graph(阈值100),用于闭环检测,建立pose graph,做位姿图优化,将累积误差分配到整个位姿图中.
3)Spanning Tree(共视程度最高的作为父节点),即,最大生成树,数据结构里的内容,这里表示与新加入的关键帧共视程度最高的那个关键帧,将其作为这个新加入的关键帧的父节点mpParent,这个在 SaveTrajectoryKITTI() 函数中会用,用于在存储普通帧的位姿时,寻找这个所依靠的没被剔除的关键帧

更新关键帧之间共视图实现在UpdateConnections() 函数和 AddConnection() 两个函数,调用的地方有:

  • tracking线程,初始化完成时
  • local mapping线程,ProcessNewKeyFrame() 函数和 SearchInNeighbors() 函数
  • loop close线程,CorrectLoop() 函数

一般来说,在使用到pKF->GetVectorCovisibleKeyFrames()之前都要先进行UpdateConnections(),这样返回的mvpOrderedConnectedKeyFrames才是当前的情况,因为增加和减少nObs属性(参考上一篇博客)都会影响共视图.


5、关键帧的剔除

操作对象是mpCurrentKeyFrame的共视关键帧,即Covisibility Graph(阈值大于15),如果这个局部关键帧看到MapPoints中能够被其它至少三个关键帧也观测到MapPoints所占的比例多余90%时,就剔除该关键帧. 中间有个细节要注意下,在搜寻这个MapPoint对应的observations时,要对尺度进行进行检查,尺度尽量不要高于这个MapPoint在这个局部关键帧中的尺度,这样做是因为,特征点的尺度越高,精度越低,误匹配的概率越大.


<完>
@leatherwang


你可能感兴趣的:(slam)