特征匹配（三）：根据词典进行匹配

本次主要讲解ORBSLAM2中使用词袋来加速度特征点匹配并使用对极约束剔除外点的函数SearchForTriangulation，这个函数中主要进行了以下工作：

• 计算KF2中的极点坐标并预分配内存

  // Step 1 计算KF1的相机中心在KF2图像平面的二维像素坐标
  // KF1相机光心在世界坐标系坐标Cw
  cv::Mat Cw = pKF1->GetCameraCenter(); 
  // KF2相机位姿R2w,t2w,是世界坐标系到相机坐标系
  cv::Mat R2w = pKF2->GetRotation();    
  cv::Mat t2w = pKF2->GetTranslation(); 
  // KF1的相机光心转化到KF2坐标系中的坐标
  cv::Mat C2 = R2w*Cw+t2w; 
  const float invz = 1.0f/C2.at<float>(2);
  // 得到KF1的相机光心在KF2中的坐标，也叫极点，这里是像素坐标
  const float ex =pKF2->fx*C2.at<float>(0)*invz+pKF2->cx;
  const float ey =pKF2->fy*C2.at<float>(1)*invz+pKF2->cy;
  
  
  int nmatches=0;
  // 记录匹配是否成功，避免重复匹配
  vector<bool> vbMatched2(pKF2->N,false);        
  vector<int> vMatches12(pKF1->N,-1);
  // 用于统计匹配点对旋转差的直方图
  vector<int> rotHist[HISTO_LENGTH];
  for(int i=0;i<HISTO_LENGTH;i++)
      rotHist[i].reserve(500);
  
   //! 原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的，更改为下面代码   
  const float factor = HISTO_LENGTH/360.0f;
  

• 利用词袋加速匹配，只有属于同一个Node下的特征点才有可能相互匹配

  // Step 2.1：遍历pKF1和pKF2中的node节点
  while(f1it!=f1end && f2it!=f2end)
  {
      // 如果f1it和f2it属于同一个node节点才会进行匹配，这就是BoW加速匹配原理
      if(f1it->first == f2it->first)
      {
          // Step 2.2：遍历属于同一node节点(id：f1it->first)下的所有特征点
          for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++)
          {
              // 获取pKF1中属于该node节点的所有特征点索引
              const size_t idx1 = f1it->second[i1];
              
              // Step 2.3：通过特征点索引idx1在pKF1中取出对应的MapPoint
              MapPoint* pMP1 = pKF1->GetMapPoint(idx1);
              
              // If there is already a MapPoint skip
              // 由于寻找的是未匹配的特征点，所以pMP1应该为NULL
              if(pMP1)
                  continue;
  
              // 如果mvuRight中的值大于0，表示是双目，且该特征点有深度值
              const bool bStereo1 = pKF1->mvuRight[idx1]>=0;
  
              if(bOnlyStereo)
                  if(!bStereo1)
                      continue;
              
              // Step 2.4：通过特征点索引idx1在pKF1中取出对应的特征点
              const cv::KeyPoint &kp1 = pKF1->mvKeysUn[idx1];
              
              // 通过特征点索引idx1在pKF1中取出对应的特征点的描述子
              const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);
              
              int bestDist = TH_LOW;
              int bestIdx2 = -1;
              
              // Step 2.5：遍历该node节点下(f2it->first)对应KF2中的所有特征点
              for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++)
              {
                  // 获取pKF2中属于该node节点的所有特征点索引
                  size_t idx2 = f2it->second[i2];
                  
                  // 通过特征点索引idx2在pKF2中取出对应的MapPoint
                  MapPoint* pMP2 = pKF2->GetMapPoint(idx2);
                  
  
                  // 如果pKF2当前特征点索引idx2已经被匹配过或者对应的3d点非空，那么跳过这个索引idx2
                  if(vbMatched2[idx2] || pMP2)
                      continue;
  
                  const bool bStereo2 = pKF2->mvuRight[idx2]>=0;
  
                  if(bOnlyStereo)
                      if(!bStereo2)
                          continue;
                  
                  // 通过特征点索引idx2在pKF2中取出对应的特征点的描述子
                  const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);
                  
                  // Step 2.6 计算idx1与idx2在两个关键帧中对应特征点的描述子距离
                  const int dist = DescriptorDistance(d1,d2);
                  
                  if(dist>TH_LOW || dist>bestDist)
                      continue;
  
                  // 通过特征点索引idx2在pKF2中取出对应的特征点
                  const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
  
                  //? 为什么双目就不需要判断像素点到极点的距离的判断？
                  // 因为双目模式下可以左右互匹配恢复三维点
                  if(!bStereo1 && !bStereo2)
                  {
                      const float distex = ex-kp2.pt.x;
                      const float distey = ey-kp2.pt.y;
                      // Step 2.7 极点e2到kp2的像素距离如果小于阈值th,认为kp2对应的MapPoint距离pKF1相机太近，跳过该匹配点对
                      // 作者根据kp2金字塔尺度因子(scale^n，scale=1.2，n为层数)定义阈值th
                      // 金字塔层数从0到7，对应距离 sqrt(100*pKF2->mvScaleFactors[kp2.octave]) 是10-20个像素
                      //? 对这个阈值的有效性持怀疑态度
                      if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave])
                          continue;
                  }
  
                  // Step 2.8 计算特征点kp2到kp1对应极线的距离是否小于阈值
                  if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2))
                  {
                      // bestIdx2，bestDist 是 kp1 对应 KF2中的最佳匹配点 index及匹配距离
                      bestIdx2 = idx2;
                      bestDist = dist;
                  }
              }
  
              
              if(bestIdx2>=0)
              {
                  const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];
                  // 记录匹配结果
                  vMatches12[idx1]=bestIdx2;      
                  
                  vbMatched2[bestIdx2]=true;  // !记录已经匹配，避免重复匹配。原作者漏掉！
                  nmatches++;
  
                  // 记录旋转差直方图信息
                  if(mbCheckOrientation)
                  {
                      // angle：角度，表示匹配点对的方向差。
                      float rot = kp1.angle-kp2.angle;
                      if(rot<0.0)
                          rot+=360.0f;
                      int bin = round(rot*factor);
                      if(bin==HISTO_LENGTH)
                          bin=0;
                      assert(bin>=0 && bin<HISTO_LENGTH);   
                      rotHist[bin].push_back(idx1);
                  }
              }
          }
  
          f1it++;
          f2it++;
      }
      // 这两步的目的是使用泛型算法使得两个Nodeid尽可能接近，不要去一个一个遍历
      else if(f1it->first < f2it->first)
      {
          f1it = vFeatVec1.lower_bound(f2it->first);
      }
      else
      {
          f2it = vFeatVec2.lower_bound(f1it->first);
      }
  }
  

  这里用到了两次对极几何，一次用来判断匹配点对对应的3d点距离KF1光心的距离，一次用本质矩阵来验证特征点匹配的误差是否在给定阈值内

• 使用旋转直方图来剔除掉错误的匹配对

  // Step 3 用旋转差直方图来筛掉错误匹配对
  if(mbCheckOrientation)
  {
      int ind1=-1;
      int ind2=-1;
      int ind3=-1;
  
      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++)
          {              
              vbMatched2[vMatches12[rotHist[i][j]]] = false;  // !清除匹配关系。原作者漏掉！
              vMatches12[rotHist[i][j]]=-1;
              nmatches--;
          }
      }
  
  }
  

• 储存匹配关系

  // Step 4 存储匹配关系，下标是关键帧1的特征点id，存储的是关键帧2的特征点id
  vMatchedPairs.clear();
  vMatchedPairs.reserve(nmatches);
  
  for(size_t i=0, iend=vMatches12.size(); i<iend; i++)
  {
      if(vMatches12[i]<0)
          continue;
      vMatchedPairs.push_back(make_pair(i,vMatches12[i]));
  }