跟踪线程（一）：基于关键帧跟踪

本次我们主要讲解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;