本次我们主要讲解ORBSLAM2中跟踪模式的一种,基于参考关键帧的跟踪,这种跟踪模式主要用于没有速度信息,刚完成重定位或者恒速模型跟踪失败后进行,主要的函数为TrackReferenceKeyFrame()
,其中主要进行了以下工作:
-
将当前帧的描述子转化为BoW向量
// Step 1:将当前帧的描述子转化为BoW向量 mCurrentFrame.ComputeBoW();
这个函数之前已经提到过
-
在当前帧和参考关键帧之间利用DBOW加速匹配来寻找匹配关系
// We perform first an ORB matching with the reference keyframe // If enough matches are found we setup a PnP solver ORBmatcher matcher(0.7,true); vector<MapPoint*> vpMapPointMatches; // Step 2:通过词袋BoW加速当前帧与参考帧之间的特征点匹配 int nmatches = matcher.SearchByBoW( mpReferenceKF, //参考关键帧 mCurrentFrame, //当前帧 vpMapPointMatches); //存储匹配关系 // 匹配数目小于15,认为跟踪失败 if(nmatches<15) return false;
其中,
SearchByBoW
为另外一个重载函数,具体实现如下int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches) { // 获取该关键帧的地图点 const vector<MapPoint*> vpMapPointsKF = pKF->GetMapPointMatches(); // 和普通帧F特征点的索引一致 vpMapPointMatches = vector<MapPoint*>(F.N,static_cast<MapPoint*>(NULL)); // 取出关键帧的词袋特征向量 const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec; int nmatches=0; // 特征点角度旋转差统计用的直方图 vector<int> rotHist[HISTO_LENGTH]; for(int i=0;i<HISTO_LENGTH;i++) rotHist[i].reserve(500); // 将0~360的数转换到0~HISTO_LENGTH的系数 // !原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的,更改为下面代码 const float factor = HISTO_LENGTH/360.0f; // We perform the matching over ORB that belong to the same vocabulary node (at a certain level) // 将属于同一节点(特定层)的ORB特征进行匹配 DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin(); DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin(); DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end(); DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end(); while(KFit != KFend && Fit != Fend) { // Step 1:分别取出属于同一node的ORB特征点(只有属于同一node,才有可能是匹配点) // first 元素就是node id,遍历 if(KFit->first == Fit->first) { // second 是该node内存储的feature index const vector<unsigned int> vIndicesKF = KFit->second; const vector<unsigned int> vIndicesF = Fit->second; // Step 2:遍历KF中属于该node的特征点 for(size_t iKF=0; iKF<vIndicesKF.size(); iKF++) { // 关键帧该节点中特征点的索引 const unsigned int realIdxKF = vIndicesKF[iKF]; // 取出KF中该特征对应的地图点 MapPoint* pMP = vpMapPointsKF[realIdxKF]; if(!pMP) continue; if(pMP->isBad()) continue; const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF); // 取出KF中该特征对应的描述子 int bestDist1=256; // 最好的距离(最小距离) int bestIdxF =-1 ; int bestDist2=256; // 次好距离(倒数第二小距离) // Step 3:遍历F中属于该node的特征点,寻找最佳匹配点 for(size_t iF=0; iF<vIndicesF.size(); iF++) { // 和上面for循环重名了,这里的realIdxF是指普通帧该节点中特征点的索引 const unsigned int realIdxF = vIndicesF[iF]; // 如果地图点存在,说明这个点已经被匹配过了,不再匹配,加快速度 if(vpMapPointMatches[realIdxF]) continue; const cv::Mat &dF = F.mDescriptors.row(realIdxF); // 取出F中该特征对应的描述子 // 计算描述子的距离 const int dist = DescriptorDistance(dKF,dF); // 遍历,记录最佳距离、最佳距离对应的索引、次佳距离等 // 如果 dist < bestDist1 < bestDist2,更新bestDist1 bestDist2 if(dist<bestDist1) { bestDist2=bestDist1; bestDist1=dist; bestIdxF=realIdxF; } // 如果bestDist1 < dist < bestDist2,更新bestDist2 else if(dist<bestDist2) { bestDist2=dist; } } // Step 4:根据阈值 和 角度投票剔除误匹配 // Step 4.1:第一关筛选:匹配距离必须小于设定阈值 if(bestDist1<=TH_LOW) { // Step 4.2:第二关筛选:最佳匹配比次佳匹配明显要好,那么最佳匹配才真正靠谱 if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2)) { // Step 4.3:记录成功匹配特征点的对应的地图点(来自关键帧) vpMapPointMatches[bestIdxF]=pMP; // 这里的realIdxKF是当前遍历到的关键帧的特征点id const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF]; // Step 4.4:计算匹配点旋转角度差所在的直方图 if(mbCheckOrientation) { // angle:每个特征点在提取描述子时的旋转主方向角度,如果图像旋转了,这个角度将发生改变 // 所有的特征点的角度变化应该是一致的,通过直方图统计得到最准确的角度变化值 float rot = kp.angle-F.mvKeys[bestIdxF].angle;// 该特征点的角度变化值 if(rot<0.0) rot+=360.0f; int bin = round(rot*factor);// 将rot分配到bin组, 四舍五入, 其实就是离散到对应的直方图组中 if(bin==HISTO_LENGTH) bin=0; assert(bin>=0 && bin<HISTO_LENGTH); rotHist[bin].push_back(bestIdxF); // 直方图统计 } nmatches++; } } } KFit++; Fit++; } else if(KFit->first < Fit->first) { // 对齐 KFit = vFeatVecKF.lower_bound(Fit->first); } else { // 对齐 Fit = F.mFeatVec.lower_bound(KFit->first); } } // Step 5 根据方向剔除误匹配的点 if(mbCheckOrientation) { // index int ind1=-1; int ind2=-1; int ind3=-1; // 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引 ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3); for(int i=0; i<HISTO_LENGTH; i++) { // 如果特征点的旋转角度变化量属于这三个组,则保留 if(i==ind1 || i==ind2 || i==ind3) continue; // 剔除掉不在前三的匹配对,因为他们不符合“主流旋转方向” for(size_t j=0, jend=rotHist[i].size(); j<jend; j++) { vpMapPointMatches[rotHist[i][j]]=static_cast<MapPoint*>(NULL); nmatches--; } } } return nmatches; }
-
将上一帧的位姿态作为当前帧位姿的初始值,以加速优化过程的收敛
// Step 3:将上一帧的位姿态作为当前帧位姿的初始值 mCurrentFrame.mvpMapPoints = vpMapPointMatches; mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw设置初值,在PoseOptimization可以收敛快一些
-
剔除优化后匹配点中的外点
// Step 5:剔除优化后的匹配点中的外点 //之所以在优化之后才剔除外点,是因为在优化的过程中就有了对这些外点的标记 int nmatchesMap = 0; for(int i =0; i<mCurrentFrame.N; i++) { if(mCurrentFrame.mvpMapPoints[i]) { //如果对应到的某个特征点是外点 if(mCurrentFrame.mvbOutlier[i]) { //清除它在当前帧中存在过的痕迹 MapPoint* pMP = mCurrentFrame.mvpMapPoints[i]; mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL); mCurrentFrame.mvbOutlier[i]=false; pMP->mbTrackInView = false; pMP->mnLastFrameSeen = mCurrentFrame.mnId; nmatches--; } else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) //匹配的内点计数++ nmatchesMap++; } } // 跟踪成功的数目超过10才认为跟踪成功,否则跟踪失败 return nmatchesMap>=10;