关键帧与地图点（二）：关键帧
本次我们要讲解的是ORBSLAM2中的关键帧，首先我们来看一下论文中关于关键帧的相关描述：每个关键帧 K i K_i Ki存储了以下内容：
- 相机的位姿 T i w T_{iw} Tiw，注意这里是从相机到世界系的变换矩阵
- 相机内参，包括主点和焦距
- 在这一帧中提取到的所有的去畸变后的ORB特征
地图点和关键帧的创建条件较为宽松，但是之后则会通过一个非常严格苛刻的删选机制负责剔除冗余的关键帧和错匹配的或不可跟踪的地图点。
对应于代码则是
// SE3位姿和相机光心的坐标
cv::Mat Tcw; // 当前相机的位姿，世界坐标系到相机坐标系
cv::Mat Twc; // 当前相机位姿的逆
cv::Mat Ow; // 相机光心(左目)在世界坐标系下的坐标,这里和普通帧中的定义是一样的
cv::Mat Cw; ///< Stereo middel point. Only for visualization
/// 关键帧观测到的地图点
std::vector<MapPoint*> mvpMapPoints;
// BoW词典
KeyFrameDatabase* mpKeyFrameDB;
// 视觉单词
ORBVocabulary* mpORBvocabulary;
// 用于加速特征匹配的网格
// 其实应该说是二维的,第三维的 vector中保存的是这个网格内的特征点的索引
std::vector< std::vector <std::vector<size_t> > > mGrid;
// 共视图
// 与该关键帧连接（至少15个共视地图点）的关键帧与权重
std::map<KeyFrame*,int> mConnectedKeyFrameWeights;
// 共视关键帧中权重从大到小排序后的关键帧
std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames;
// 共视关键帧中从大到小排序后的权重，和上面对应
std::vector<int> mvOrderedWeights;
// ===================== 生成树和闭环边 ========================
// std::set是集合，相比vector，进行插入数据这样的操作时会自动排序
bool mbFirstConnection; // 是否是第一次生成树
KeyFrame* mpParent; // 当前关键帧的父关键帧 （共视程度最高的）
std::set<KeyFrame*> mspChildrens; // 存储当前关键帧的子关键帧，这个一般不止一个
std::set<KeyFrame*> mspLoopEdges; // 和当前关键帧形成回环关系的关键帧
// Bad flags
bool mbNotErase; // 当前关键帧已经和其他的关键帧形成了回环关系，因此在各种优化的过程中不应该被删除
bool mbToBeErased; // 将要被删除的标志
bool mbBad; // 关键帧为Bad的标志
float mHalfBaseline; // 对于双目相机来说,双目相机基线长度的一半. Only for visualization
Map* mpMap; // 局部地图
/// 在对位姿进行操作时相关的互斥锁
std::mutex mMutexPose;
/// 在操作当前关键帧和其他关键帧的共视关系的时候使用到的互斥锁
std::mutex mMutexConnections;
/// 在操作和特征点有关的变量的时候的互斥锁
std::mutex mMutexFeatures;
接下来我们需要了解一下ORBSLAM2中对于关键帧的选择策略，要插入新的关键帧，以下条件必须满足：
- 距离上一次全局重定位要经过至少20帧
- 局部建图线程处于空闲状态，或者从上次插入关键帧起经过了至少20帧
- 当前帧跟踪了至少50个地图点
- 当前帧跟踪的地图点数少于参考关键帧 K r e f K_{ref} Kref地图点数量的90%
这里不使用与其他关键帧的距离标准作为判断是否插入关键帧的条件，而是使用视觉变化来判断（条件4），条件1确保良好的重定位，条件3确保良好的跟踪。如果在局部建图线程忙时插入关键帧（条件2的第二部分），则会发送一个信号来停止局部BA的进行，以便它能够尽快处理新的关键帧。
对应的代码为
- 判断是否需要插入关键帧
/** * @brief 判断当前帧是否需要插入关键帧 * * Step 1：纯VO模式下不插入关键帧，如果局部地图被闭环检测使用，则不插入关键帧 * Step 2：如果距离上一次重定位比较近，或者关键帧数目超出最大限制，不插入关键帧 * Step 3：得到参考关键帧跟踪到的地图点数量 * Step 4：查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧 * Step 5：对于双目或RGBD摄像头，统计可以添加的有效地图点总数 和 跟踪到的地图点数量 * Step 6：决策是否需要插入关键帧 * @return true 需要 * @return false 不需要 */ bool Tracking::NeedNewKeyFrame() { // Step 1：纯VO模式下不插入关键帧 if(mbOnlyTracking) return false; // If Local Mapping is freezed by a Loop Closure do not insert keyframes // Step 2：如果局部地图线程被闭环检测使用，则不插入关键帧 if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested()) return false; // 获取当前地图中的关键帧数目 const int nKFs = mpMap->KeyFramesInMap(); // Do not insert keyframes if not enough frames have passed from last relocalisation // mCurrentFrame.mnId是当前帧的ID // mnLastRelocFrameId是最近一次重定位帧的ID // mMaxFrames等于图像输入的帧率 // Step 3：如果距离上一次重定位比较近，并且关键帧数目超出最大限制，不插入关键帧 if( mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && nKFs>mMaxFrames) return false; // Tracked MapPoints in the reference keyframe // Step 4：得到参考关键帧跟踪到的地图点数量 // UpdateLocalKeyFrames 函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧 // 地图点的最小观测次数 int nMinObs = 3; if(nKFs<=2) nMinObs=2; // 参考关键帧地图点中观测的数目>= nMinObs的地图点数目 int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs); // Local Mapping accept keyframes? // Step 5：查询局部地图线程是否繁忙，当前能否接受新的关键帧 bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames(); // Check how many "close" points are being tracked and how many could be potentially created. // Step 6：对于双目或RGBD摄像头，统计成功跟踪的近点的数量，如果跟踪到的近点太少，没有跟踪到的近点较多，可以插入关键帧 int nNonTrackedClose = 0; //双目或RGB-D中没有跟踪到的近点 int nTrackedClose= 0; //双目或RGB-D中成功跟踪的近点（三维点） if(mSensor!=System::MONOCULAR) { for(int i =0; i<mCurrentFrame.N; i++) { // 深度值在有效范围内 if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth) { if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i]) nTrackedClose++; else nNonTrackedClose++; } } } // 双目或RGBD情况下：跟踪到的地图点中近点太少 同时 没有跟踪到的三维点太多，可以插入关键帧了 // 单目时，为false bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70); // Step 7：决策是否需要插入关键帧 // Step 7.1：设定比例阈值，当前帧和参考关键帧跟踪到点的比例，比例越大，越倾向于增加关键帧 float thRefRatio = 0.75f; // 关键帧只有一帧，那么插入关键帧的阈值设置的低一点，插入频率较低 if(nKFs<2) thRefRatio = 0.4f; //单目情况下插入关键帧的频率很高 if(mSensor==System::MONOCULAR) thRefRatio = 0.9f; // Condition 1a: More than "MaxFrames" have passed from last keyframe insertion // Step 7.2：很长时间没有插入关键帧，可以插入 const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames; // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle // Step 7.3：满足插入关键帧的最小间隔并且localMapper处于空闲状态，可以插入 const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle); // Condition 1c: tracking is weak // Step 7.4：在双目，RGB-D的情况下当前帧跟踪到的点比参考关键帧的0.25倍还少，或者满足bNeedToInsertClose const bool c1c = mSensor!=System::MONOCULAR && //只考虑在双目，RGB-D的情况 (mnMatchesInliers<nRefMatches*0.25 || //当前帧和地图点匹配的数目非常少 bNeedToInsertClose) ; //需要插入 // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches. // Step 7.5：和参考帧相比当前跟踪到的点太少 或者满足bNeedToInsertClose；同时跟踪到的内点还不能太少 const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15); if((c1a||c1b||c1c)&&c2) { // If the mapping accepts keyframes, insert keyframe. // Otherwise send a signal to interrupt BA // Step 7.6：local mapping空闲时可以直接插入，不空闲的时候要根据情况插入 if(bLocalMappingIdle) { //可以插入关键帧 return true; } else { mpLocalMapper->InterruptBA(); if(mSensor!=System::MONOCULAR) { // 队列里不能阻塞太多关键帧 // tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中， // 然后localmapper再逐个pop出来插入到mspKeyFrames if(mpLocalMapper->KeyframesInQueue()<3) //队列中的关键帧数目不是很多,可以插入 return true; else //队列中缓冲的关键帧数目太多,暂时不能插入 return false; } else //对于单目情况,就直接无法插入关键帧了 //? 为什么这里对单目情况的处理不一样? //回答：可能是单目关键帧相对比较密集 return false; } } else //不满足上面的条件,自然不能插入关键帧 return false; }这里的判断条件比较复杂，有些变量的含义还没有讲到，可以等到之后再仔细理解
- 插入关键帧：
/** * @brief 创建新的关键帧 * 对于非单目的情况，同时创建新的MapPoints * * Step 1：将当前帧构造成关键帧 * Step 2：将当前关键帧设置为当前帧的参考关键帧 * Step 3：对于双目或rgbd摄像头，为当前帧生成新的MapPoints */ void Tracking::CreateNewKeyFrame() { // 如果局部建图线程关闭了,就无法插入关键帧 if(!mpLocalMapper->SetNotStop(true)) return; // Step 1：将当前帧构造成关键帧 KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); // Step 2：将当前关键帧设置为当前帧的参考关键帧 // 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧 mpReferenceKF = pKF; mCurrentFrame.mpReferenceKF = pKF; // 这段代码和 Tracking::UpdateLastFrame 中的那一部分代码功能相同 // Step 3：对于双目或rgbd摄像头，为当前帧生成新的地图点；单目无操作 if(mSensor!=System::MONOCULAR) { // 根据Tcw计算mRcw、mtcw和mRwc、mOw mCurrentFrame.UpdatePoseMatrices(); // We sort points by the measured depth by the stereo/RGBD sensor. // We create all those MapPoints whose depth < mThDepth. // If there are less than 100 close points we create the 100 closest. // Step 3.1：得到当前帧有深度值的特征点（不一定是地图点） vector<pair<float,int> > vDepthIdx; vDepthIdx.reserve(mCurrentFrame.N); // 遍历当前帧的地图点 for(int i=0; i<mCurrentFrame.N; i++) { // 地图点深度 float z = mCurrentFrame.mvDepth[i]; if(z>0) { // 第一个元素是深度,第二个元素是对应的特征点的id vDepthIdx.push_back(make_pair(z,i)); } } if(!vDepthIdx.empty()) { // Step 3.2：按照深度从小到大排序 sort(vDepthIdx.begin(),vDepthIdx.end()); // Step 3.3：从中找出不是地图点的生成临时地图点 // 处理的近点的个数 int nPoints = 0; for(size_t j=0; j<vDepthIdx.size();j++) { // 地图点id int i = vDepthIdx[j].second; bool bCreateNew = false; // 如果这个点对应在上一帧中的地图点没有,或者创建后就没有被观测到,那么就生成一个临时的地图点 MapPoint* pMP = mCurrentFrame.mvpMapPoints[i]; if(!pMP) bCreateNew = true; else if(pMP->Observations()<1) { bCreateNew = true; mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL); } // 如果需要就新建地图点，这里的地图点不是临时的，是全局地图中新建地图点，用于跟踪 if(bCreateNew) { cv::Mat x3D = mCurrentFrame.UnprojectStereo(i); MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap); // 这些添加属性的操作是每次创建MapPoint后都要做的 pNewMP->AddObservation(pKF,i); pKF->AddMapPoint(pNewMP,i); pNewMP->ComputeDistinctiveDescriptors(); pNewMP->UpdateNormalAndDepth(); mpMap->AddMapPoint(pNewMP); mCurrentFrame.mvpMapPoints[i]=pNewMP; nPoints++; } else { // 因为从近到远排序，记录其中不需要创建地图点的个数 nPoints++; } // Step 3.4：停止新建地图点必须同时满足以下条件： // 1、当前的点的深度已经超过了设定的深度阈值（35倍基线） // 2、nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出 if(vDepthIdx[j].first>mThDepth && nPoints>100) break; } } } // Step 4：插入关键帧 // 关键帧插入到列表 mlNewKeyFrames中，等待local mapping线程临幸 mpLocalMapper->InsertKeyFrame(pKF); // 插入好了，允许局部建图停止 mpLocalMapper->SetNotStop(false); // 当前帧成为新的关键帧，更新 mnLastKeyFrameId = mCurrentFrame.mnId; mpLastKeyFrame = pKF; }
最后我们来看一下关键帧对应的比较重要的函数
-
更新连接关系
//KeyFrame.cc KeyFrame::UpdateConnections() { //省略... // Step 5 更新生成树的连接 if(mbFirstConnection && mnId!=0) { // 初始化该关键帧的父关键帧为共视程度最高的那个关键帧 mpParent = mvpOrderedConnectedKeyFrames.front(); // 建立双向连接关系,将当前关键帧作为其子关键帧 mpParent->AddChild(this); mbFirstConnection = false; } } // 添加子关键帧(即和子关键帧具有最大共视关系的关键帧就是当前关键帧) void KeyFrame::AddChild(KeyFrame *pKF) {unique_lock<mutex> lockCon(mMutexConnections); mspChildrens.insert(pKF); } // 删除某个子关键帧 void KeyFrame::EraseChild(KeyFrame *pKF) { unique_lock<mutex> lockCon(mMutexConnections); mspChildrens.erase(pKF); } // 改变当前关键帧的父关键帧 void KeyFrame::ChangeParent(KeyFrame *pKF) { unique_lock<mutex> lockCon(mMutexConnections); // 添加双向连接关系 mpParent = pKF; pKF->AddChild(this); } //获取当前关键帧的子关键帧 set<KeyFrame*> KeyFrame::GetChilds() { unique_lock<mutex> lockCon(mMutexConnections); return mspChildrens; } //获取当前关键帧的父关键帧 KeyFrame* KeyFrame::GetParent() { unique_lock<mutex> lockCon(mMutexConnections); return mpParent; } // 判断某个关键帧是否是当前关键帧的子关键帧 bool KeyFrame::hasChild(KeyFrame *pKF) { unique_lock<mutex> lockCon(mMutexConnections); return mspChildrens.count(pKF); } -
更新局部关键帧
void Tracking::UpdateLocalKeyFrames() { //省略... // 策略2.2:将自己的子关键帧作为局部关键帧(将邻居的子孙们拉拢入伙) const set<KeyFrame*> spChilds = pKF->GetChilds(); for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++) { KeyFrame* pChildKF = *sit; if(!pChildKF->isBad()) { if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId) { mvpLocalKeyFrames.push_back(pChildKF);pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId; //? 找到一个就直接跳出for循环? break; } } } // 策略2.3:自己的父关键帧(将邻居的父母们拉拢入伙) KeyFrame* pParent = pKF->GetParent(); if(pParent) { // mnTrackReferenceForFrame防止重复添加局部关键帧 if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId) { mvpLocalKeyFrames.push_back(pParent); pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId; //! 感觉是个bug!如果找到父关键帧会直接跳出整个循环 break; } } // 省略.... }其中大部分代码之后讲到三大线程时候会详细讲