【ORB-SLAM2代码】（三）定位

上一篇：ORB-SLAM2代码（二）建图

---

参考：orb-slam2源码注释版

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

---

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<nRefMatches*0.25条件成立，表示跟踪到的MapPoint数目已经不及关键帧中稳定的MapPoint数目的四分之一
ratioMap<0.3f表示 $"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在这个局部关键帧中的尺度，这样做是因为，特征点的尺度越高，精度越低，误匹配的概率越大.

---

下一篇：ORB-SLAM2代码（四）特征点匹配

---

@leatherwang

二零一八年九月十三日

---