生成关键帧
根据轨迹跟踪的总体过程， 前端里程计完成了相机的位姿估计， 并根据局部地图对其进行优化。 此时如果一切正常，就可以根据一些条件生成关键帧了。关键帧的目的是对相机的数据进行采样， 尽量完整的描述地图环境的同时，减少需要保存的数据。达到降低内存和算力的开销，提高系统效率的目的。
本文，我们将研究轨迹跟踪器 Tracking 生成关键帧的规则，以及前端里程计的一些其它收尾操作。
1. 生成关键帧的总体流程
轨迹跟踪器 Tracking 在成员函数 Track() 中完成了相机的位姿估计并根据局部地图进一步优化了位姿之后， 就会运行下面的代码片段。第一行条件语句中的变量 bOK 是 Track() 中定义的一个局部变量，它反映了前端里程计的状态，为真，说明视觉里程计是跟上了相机的运动。 这段代码大体上可以分为两个部分：更新匀速运动模型、判定和生成关键帧。
if (bOK) {
if (!mLastFrame.mTcw.empty()) {
cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
mVelocity = mCurrentFrame.mTcw*LastTwc;
}
else
mVelocity = cv::Mat();
// 省略日志更新以及释放临时资源相关的语句
if (NeedNewKeyFrame())
CreateNewKeyFrame();
// 省略进一步剔除野点的语句
}
我们专门介绍过 ORB-SLAM2 的匀速运动模型， 这里主要是将上一帧姿态矩阵的逆右乘到当前帧上，得到前后两帧相机的位姿增量，将结果保留在成员变量 mVelocity 中。 然后，我们省略了一些关于日志、临时资源释放的语句。函数 Track() 会先后调用函数 NeedNewKeyFrame() 判定当前帧是否满足关键帧的筛选条件， 然后通过函数 CreateNewKeyFrame() 生成。
2. 判定是否生成关键帧
ORB-SLAM论文中说要插入新的关键帧需要满足如下的几个条件：
- 如果发生了重定位，那么需要在20帧之后才能添加新的关键帧;
- LOCAL MAPPING线程处于空闲(idle)的状态，或者距离上次插入关键帧已经过去了20帧;
- 当前帧至少保留了50个匹配的地图点;
- 当前帧跟踪的地图点数量少于参考帧的90%。
条件一应该是在防止错误的重定位，如果前端里程计在重定位之后，连续20帧都能跟上相机的运动，那么说明重定位的效果还是不错的； 因为 ORB-SLAM2 中专门有一个 LOCAL MAPPING 的线程管理关键帧，条件二选择在这个线程空闲的时候添加关键帧，可以保证 LOCAL MAPPING 尽快的处理， 同时，估计是考虑到累积误差的问题吧，希望生成的关键帧有一定密度，所以该条件还在20帧之后生成一个关键帧； 条件三是为了选择有足够多的地图点的帧，保证轨迹跟踪时有足够的地图点来估计相机位姿；条件四是一个比较严苛的约束，可以适当的降低关键帧数量。
条件四和条件二是一对互相制约的条件，它们一方面希望减少关键帧的数量，提高系统的效率，另一方面希望生成的关键帧具有一定的密度，降低系统误差。 论文中只讲述了什么时候插入新的关键帧，源码里还写下了很多拒绝新建关键帧的条件。
下面的代码片段是函数NeedNewKeyFrame的具体实现。首先，如果当前的工作模式就是纯跟踪定位，那么就没有必要更新地图，也就不需要插入新的关键帧了。 其次如果LOOP CLOSURE线程检测到了闭环，那么LOCAL MAPPING将被暂停，此时也不会插入新的关键帧的。
bool Tracking::NeedNewKeyFrame() {
if(mbOnlyTracking)
return false;
// If Local Mapping is freezed by a Loop Closure do not insert keyframes
if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
return false;
下面的条件看起来是针对论文中的条件一的实现，但又好象多了一些内容。首先，这里的成员变量mMaxFrames在Tracking的构造函数里的赋值是相机的帧率，它是可以通过配置文件中的字段"Camera.fps"调整的。 而且后面还有一个与的条件，看意思是说ORB-SLAM中的关键帧数量不宜过多，估计还是为了效率考虑的吧，所以设置了一个上限。如果关键帧数量可以一直增长，那么随着时间的流逝，地图不断的扩展，系统迟早要崩掉的。 更不用说什么实时性的问题了。
const int nKFs = mpMap->KeyFramesInMap();
if (mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && nKFs > mMaxFrames)
return false;
下面的语句获取参考帧中跟踪的地图点数量，至于这里的参数 nMinObs 表示地图点的最少观测次数。
int nMinObs = 3;
if (nKFs <= 2)
nMinObs = 2;
int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);
下面的c1a和c1b其实是对条件2的实现，c1c对于单目没有意义。c2是对条件4的实现，其中变量 thRefRatio 是当前帧与参考关键帧中地图点的比例系数，对于单目是 0.9，双目和深度相机则是0.75。 此外，变量 bNeedToInsertClose 并不是对单目的限制，我们放到针对深度相机的调整(TRACKING)中介绍。
bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
float thRefRatio = 0.75f;
if (nKFs < 2)
thRefRatio = 0.4f;
if (mSensor == System::MONOCULAR)
thRefRatio = 0.9f;
// 省略非单目相关的代码
const bool c1a = mCurrentFrame.mnId >= mnLastKeyFrameId + mMaxFrames;
const bool c1b = (mCurrentFrame.mnId >= mnLastKeyFrameId + mMinFrames && bLocalMappingIdle);
const bool c1c = mSensor != System::MONOCULAR && (mnMatchesInliers < nRefMatches*0.25 || bNeedToInsertClose);
const bool c2 = ((mnMatchesInliers < nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers > 15);
在满足了条件1,2,3,4后，就应当返回是否新建关键帧的判定结果了。如果LOCAL MAPPING线程并没有处于空闲状态的话，贸然插入新的关键帧可能对其局部BA优化有影响， 所以只是发送了一个信号要求LOCAL MAPPING暂停BA优化。29~31行只是打断了 LOCAL MAPPING 的 BA 优化，并不生成新的关键帧，当前帧将被抛弃掉，下一帧大概率是会生成一个的。
if ((c1a || c1b || c1c) && c2) {
if(bLocalMappingIdle)
return true;
else {
mpLocalMapper->InterruptBA();
// 省略非单目的语句
return false;
}
} else
return false;
}
3. 生成关键帧
生成一个关键帧的操作就比较直接了，如下面的代码所示。首先通知 LOCAL MAPPING 如果要关闭线程的话就先的等一等，再创建新的关键帧，并更新当前的参考关键帧和当前帧的参考关键帧。 然后通知mpLocalMapper有新的关键帧插入，并更新成员变量mnLastKeyFrameId和mpLastKeyFrame。
void Tracking::CreateNewKeyFrame() {
if(!mpLocalMapper->SetNotStop(true))
return;
KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
mpReferenceKF = pKF;
mCurrentFrame.mpReferenceKF = pKF;
if(mSensor!=System::MONOCULAR)
// 省略
mpLocalMapper->InsertKeyFrame(pKF);
mpLocalMapper->SetNotStop(false);
mnLastKeyFrameId = mCurrentFrame.mnId;
mpLastKeyFrame = pKF;
}
4. 其它收尾操作
我们现在再回到Tracking对象的成员函数Track中，完成了关键帧的生成操作后，就要结束TRACKING线程的一个循环了， 还有一些其它工作需要收尾。
首先是如果跟丢了，而且当前系统还没有足够多的关键帧，将重置系统，重新初始化。
if (mState == LOST) {
if(mpMap->KeyFramesInMap() <= 5) {
cout << "Track lost soon after initialisation, reseting..." << endl;
mpSystem->Reset();
return;
} }
然后是更新当前帧和上一帧的状态和对象，方便下次迭代。
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
mLastFrame = Frame(mCurrentFrame);
最后根据当前帧的位姿估计是否存在，更新Tracking对象的一些状态。
if(!mCurrentFrame.mTcw.empty()) {
cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
mlRelativeFramePoses.push_back(Tcr);
mlpReferences.push_back(mpReferenceKF);
mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
mlbLost.push_back(mState==LOST);
} else {
// This can happen if tracking is lost
mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
mlpReferences.push_back(mlpReferences.back());
mlFrameTimes.push_back(mlFrameTimes.back());
mlbLost.push_back(mState==LOST);
}
5. 完
根据局部地图进一步优化了相机位姿之后，如果位姿仍然没有跟丢，那么在满足一些限制条件的情况下就可以生成新的关键帧了。为了满足SLAM前端系统的实时性，Tracking对于生成关键帧的限制条件是比较宽泛的。 ORB-SLAM将在LOCAL MAPPING线程中对关键帧进行比较严格的筛选。