ORB_SLAM2原理及代码解析：Tracking::NeedNewKeyFrame() Tracking::CreateNewKeyFrame()函数
目录
1 NeedNewKeyFrame() 函数
1.1 作用
1.2 参数及含义
1.3 代码
1.4 解析
1.4.1 判断系统状态
1.4.2 基于关键帧数量与重定位判断
1.4.3 基于参考关键帧匹配点数判断
1.4.4 Local Mapping是否空闲
1.4.5 跟踪近距离点（仅双目 / RGB-D）
1.4.6 匹配比例阀值
1.4.7 条件组合
1.4.8 Local Mapping忙碌时处理
2 CreateNewKeyFrame() 函数
2.1 作用
2.2 参数及含义
2.3 代码
2.4 解析
2.4.1 设置Local Mapping状态
2.4.2 创建关键帧
2.4.3 双目/RGB-D创建近距离MapPoints
2.4.3.1 UpdatePoseMatrices()
2.4.3.2 UnprojectStereo()
2.4.3.3 AddObservation()
2.4.3.4 ComputeDistinctiveDescriptors()
2.4.3.5 UpdateNormalAndDepth()
2.4.4 创建关键帧到局部地图
2.4.4.1 SetNotStop()
2.4.5 更新关键帧信息
1 NeedNewKeyFrame() 函数
1.1 作用
判断当前帧是否需要作为新的关键帧插入到地图中。
1.2 参数及含义
1.3 代码
bool Tracking::NeedNewKeyFrame() {if(mbOnlyTracking)return false;// If Local Mapping is freezed by a Loop Closure do not insert keyframesif(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())return false;const int nKFs = mpMap->KeyFramesInMap();// Do not insert keyframes if not enough frames have passed from last relocalisationif(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)return false;// Tracked MapPoints in the reference keyframeint nMinObs = 3;if(nKFs<=2)nMinObs=2;int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);// Local Mapping accept keyframes?bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();// Check how many "close" points are being tracked and how many could be potentially created.int nNonTrackedClose = 0;int nTrackedClose= 0;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++;elsenNonTrackedClose++;}}}bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);// Thresholdsfloat 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 insertionconst bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;// Condition 1b: More than "MinFrames" have passed and Local Mapping is idleconst bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);//Condition 1c: tracking is weakconst bool c1c = mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || bNeedToInsertClose) ;// Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.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 BAif(bLocalMappingIdle){return true;}else{mpLocalMapper->InterruptBA();if(mSensor!=System::MONOCULAR){if(mpLocalMapper->KeyframesInQueue()<3)return true;elsereturn false;}elsereturn false;}}elsereturn false; }
1.4 解析
1.4.1 判断系统状态
if(mbOnlyTracking)return false;// If Local Mapping is freezed by a Loop Closure do not insert keyframesif(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())return false;
1.4.2 基于关键帧数量与重定位判断
//地图中关键帧数量const int nKFs = mpMap->KeyFramesInMap();// Do not insert keyframes if not enough frames have passed from last relocalisation//当前帧距离上一次重定位的帧很近 且 地图中关键帧数量大于重定位后连续跟踪的帧数阈值if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)return false;
1.4.3 基于参考关键帧匹配点数判断
// Tracked MapPoints in the reference keyframe//参考关键帧中地图点的最小观测次数阈值int nMinObs = 3;//如果地图点中关键帧<=2，最小观测次数阈值改为2if(nKFs<=2)nMinObs=2;//该关键帧中被跟踪的地图点数量（至少被nMinObs个关键帧观测）int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);
1.4.4 Local Mapping是否空闲
// Local Mapping accept keyframes?bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();空闲，可以插入新关键帧；
忙碌，不可。
1.4.5 跟踪近距离点（仅双目 / RGB-D）
// Check how many "close" points are being tracked and how many could be potentially created.int nNonTrackedClose = 0;int nTrackedClose= 0;if(mSensor!=System::MONOCULAR){for(int i =0; i<mCurrentFrame.N; i++){//当前帧第i个特征点深度值>0 且 <近距离深度阀值if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth){if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])//有效跟踪近距离点+1nTrackedClose++;elsenNonTrackedClose++;}}}
1.4.6 匹配比例阀值
//bNeedToInsertClose：需要插入新关键帧，nTrackedClose：当前帧成功跟踪的近距离点数bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);// Thresholds//thRefRatio：参考关键帧的匹配点比例阈值，用于判断是否需要插入关键帧float thRefRatio = 0.75f;//地图中关键帧<2if(nKFs<2)thRefRatio = 0.4f;if(mSensor==System::MONOCULAR)thRefRatio = 0.9f;
1.4.7 条件组合
// Condition 1a: More than "MaxFrames" have passed from last keyframe insertionconst bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;// Condition 1b: More than "MinFrames" have passed and Local Mapping is idle//bLocalMappingIdle：局部建图空闲const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);//Condition 1c: tracking is weak//bNeedToInsertClose：近距离跟踪不佳const bool c1c = mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || bNeedToInsertClose) ;// Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.//nRefMatches * thRefRatio：参考关键帧阀值比例const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);
1.4.8 Local Mapping忙碌时处理
if((c1a||c1b||c1c)&&c2){// If the mapping accepts keyframes, insert keyframe.// Otherwise send a signal to interrupt BA//如果局部建图线程空闲，直接接受关键帧if(bLocalMappingIdle){return true;}else{//如果局部建图线程正在忙，先尝试中断 BAmpLocalMapper->InterruptBA();if(mSensor!=System::MONOCULAR){//如果局部建图队列中待处理关键帧少于3个，允许插入关键帧if(mpLocalMapper->KeyframesInQueue()<3)return true;elsereturn false;}elsereturn false;}}elsereturn false;
2 CreateNewKeyFrame() 函数
2.1 作用
创建新关键帧。
2.2 参数及含义
2.3 代码
void Tracking::CreateNewKeyFrame() {if(!mpLocalMapper->SetNotStop(true))return;KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);mpReferenceKF = pKF;mCurrentFrame.mpReferenceKF = pKF;if(mSensor!=System::MONOCULAR){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.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){vDepthIdx.push_back(make_pair(z,i));}}if(!vDepthIdx.empty()){sort(vDepthIdx.begin(),vDepthIdx.end());int nPoints = 0;for(size_t j=0; j<vDepthIdx.size();j++){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);pNewMP->AddObservation(pKF,i);pKF->AddMapPoint(pNewMP,i);pNewMP->ComputeDistinctiveDescriptors();pNewMP->UpdateNormalAndDepth();mpMap->AddMapPoint(pNewMP);mCurrentFrame.mvpMapPoints[i]=pNewMP;nPoints++;}else{nPoints++;}if(vDepthIdx[j].first>mThDepth && nPoints>100)break;}}}mpLocalMapper->InsertKeyFrame(pKF);mpLocalMapper->SetNotStop(false);mnLastKeyFrameId = mCurrentFrame.mnId;mpLastKeyFrame = pKF; }
2.4 解析
2.4.1 设置Local Mapping状态
if(!mpLocalMapper->SetNotStop(true))return;局部建图开起，否则返回。
2.4.2 创建关键帧
KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);mpReferenceKF = pKF;mCurrentFrame.mpReferenceKF = pKF;
2.4.3 双目/RGB-D创建近距离MapPoints
if(mSensor!=System::MONOCULAR){//更新当前帧的位姿矩阵，详见2.4.3.1mCurrentFrame.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.//按深度排序所有可用特征点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){vDepthIdx.push_back(make_pair(z,i));}}if(!vDepthIdx.empty()){//按深度排序（近点优先）sort(vDepthIdx.begin(),vDepthIdx.end());int nPoints = 0;for(size_t j=0; j<vDepthIdx.size();j++){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){//将当前帧第i个特征点反投影到世界坐标系，详见2.4.3.2cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);//地图点被关键帧观测次数统计，详见2.4.3.3pNewMP->AddObservation(pKF,i);pKF->AddMapPoint(pNewMP,i);//寻找地图点最佳描述子 ，详见2.4.3.4pNewMP->ComputeDistinctiveDescriptors();//更新地图点法向量，可观测距离范围，详见2.4.3.5pNewMP->UpdateNormalAndDepth();mpMap->AddMapPoint(pNewMP);mCurrentFrame.mvpMapPoints[i]=pNewMP;nPoints++;}else{nPoints++;}if(vDepthIdx[j].first>mThDepth && nPoints>100)break;}}}
2.4.3.1 UpdatePoseMatrices()
（1）声明：Frame.h
// Computes rotation, translation and camera center matrices from the camera pose.void UpdatePoseMatrices();（2）定义：Frame.cc
void Frame::UpdatePoseMatrices() { mRcw = mTcw.rowRange(0,3).colRange(0,3);mRwc = mRcw.t();mtcw = mTcw.rowRange(0,3).col(3);mOw = -mRcw.t()*mtcw; }
2.4.3.2 UnprojectStereo()
详见：
https://blog.csdn.net/weixin_45728280/article/details/152324125?spm=1001.2014.3001.5502
2.4.3.3 AddObservation()
（1）声明：MapPoint.h
void AddObservation(KeyFrame* pKF,size_t idx);（2）定义：MapPoint.cc
void MapPoint::AddObservation(KeyFrame* pKF, size_t idx) {unique_lock<mutex> lock(mMutexFeatures);//如果关键帧已经对地图点观测过，则返回if(mObservations.count(pKF))return;mObservations[pKF]=idx;//如果右目也有观测，观测值+2if(pKF->mvuRight[idx]>=0)nObs+=2;elsenObs++; }（3）作用：
地图点被关键帧观测次数统计。
2.4.3.4 ComputeDistinctiveDescriptors()
详见：
https://blog.csdn.net/weixin_45728280/article/details/152320324?spm=1011.2415.3001.5331
2.4.3.5 UpdateNormalAndDepth()
详见：
https://blog.csdn.net/weixin_45728280/article/details/152320646?spm=1011.2415.3001.5331
2.4.4 创建关键帧到局部地图
//把刚创建的关键帧给局部建图线程mpLocalMapper->InsertKeyFrame(pKF);//局部建图线程关闭mpLocalMapper->SetNotStop(false);
2.4.4.1 SetNotStop()
（1）声明：LocalMapping.h
bool SetNotStop(bool flag);（2）定义：LocalMapping.cc
bool LocalMapping::SetNotStop(bool flag) {unique_lock<mutex> lock(mMutexStop);if(flag && mbStopped)return false;mbNotStop = flag;return true; }
2.4.5 更新关键帧信息
mnLastKeyFrameId = mCurrentFrame.mnId;mpLastKeyFrame = pKF;