参考:orb-slam2源码注释版
上一讲到了SLAM中的建图(其本质是对MapPoint的管理),有了地图,我们就有了环境的struct,这样我们就可以通过特征点匹配、优化等一系列步骤进行定位了,这次将对orb-slam中的定位部分进行解析,其本质是对frame的处理,包括关键帧和普通帧.
在设置当前帧的参考关键帧时(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;
在 TrackWithMotionModel() 函数, UpdateLastFrame() 函数, 使用上一普通帧的参考关键帧,更新上一帧的位姿,这里主要是考虑到上一普通帧的参考关键帧会在local mapping中优化后改变,所以才这么做的.
// Update pose according to reference keyframe
KeyFrame* pRef = mLastFrame.mpReferenceKF;
cv::Mat Tlr = mlRelativeFramePoses.back();
mLastFrame.SetPose(Tlr*pRef->GetPose());
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
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);
每一个关键帧都会维护一个共视图,根据共视程度,共视图包括下面三种:
1) Covisibility Graph
(阈值15),主要用于:
Covisibility Graph
选择前20个(双目是10个)作为一级相邻Covisibility Graph
,对共视的关键帧进行筛选2) Essential Graph
(阈值100),用于闭环检测,建立pose graph,做位姿图优化,将累积误差分配到整个位姿图中.
3)Spanning Tree
(共视程度最高的作为父节点),即,最大生成树,数据结构里的内容,这里表示与新加入的关键帧共视程度最高的那个关键帧,将其作为这个新加入的关键帧的父节点mpParent
,这个在 SaveTrajectoryKITTI() 函数中会用,用于在存储普通帧的位姿时,寻找这个所依靠的没被剔除的关键帧
更新关键帧之间共视图实现在UpdateConnections() 函数和 AddConnection() 两个函数,调用的地方有:
一般来说,在使用到pKF->GetVectorCovisibleKeyFrames()
之前都要先进行UpdateConnections()
,这样返回的mvpOrderedConnectedKeyFrames
才是当前的情况,因为增加和减少nObs
属性(参考上一篇博客)都会影响共视图.
操作对象是mpCurrentKeyFrame
的共视关键帧,即Covisibility Graph
(阈值大于15),如果这个局部关键帧看到MapPoints中能够被其它至少三个关键帧也观测到MapPoints所占的比例多余90%时,就剔除该关键帧. 中间有个细节要注意下,在搜寻这个MapPoint对应的observations时,要对尺度进行进行检查,尺度尽量不要高于这个MapPoint在这个局部关键帧中的尺度,这样做是因为,特征点的尺度越高,精度越低,误匹配的概率越大.
<完>
@leatherwang