跟踪线程（二）：基于匀速模型跟踪

本次主要讲解ORBSLAM2中的第二种跟踪方式，基于匀速模型跟踪，在ORBSLAM2的跟踪过程中，大部分情况下都会使用这个方式来进行跟踪，对应的函数为TrackWithMotionModel，在其中主要进行了以下工作

• 初始化特征点匹配器并更新上一帧的位姿，对于双目或者RGBD模式还会生成临时的地图点用于增强跟踪的稳定性

  // 最小距离 < 0.9*次小距离 匹配成功，检查旋转
  ORBmatcher matcher(0.9,true);
  
  // Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点
  UpdateLastFrame();
  

  函数展开为

  void Tracking::UpdateLastFrame()
  {
      // Update pose according to reference keyframe
      // Step 1：利用参考关键帧更新上一帧在世界坐标系下的位姿
      // 上一普通帧的参考关键帧，注意这里用的是参考关键帧（位姿准）而不是上上一帧的普通帧
      KeyFrame* pRef = mLastFrame.mpReferenceKF;  
      // ref_keyframe 到 lastframe的位姿变换
      cv::Mat Tlr = mlRelativeFramePoses.back();
  
      // 将上一帧的世界坐标系下的位姿计算出来
      // l:last, r:reference, w:world
      // Tlw = Tlr*Trw 
      mLastFrame.SetPose(Tlr*pRef->GetPose()); 
  
      // 如果上一帧为关键帧，或者单目的情况，则退出
      if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR)
          return;
  
      // Step 2：对于双目或rgbd相机，为上一帧生成新的临时地图点
      // 注意这些地图点只是用来跟踪，不加入到地图中，跟踪完后会删除
  
      // Create "visual odometry" MapPoints
      // We sort points according to their measured depth by the stereo/RGB-D sensor
      // Step 2.1：得到上一帧中具有有效深度值的特征点（不一定是地图点）
      vector<pair<float,int> > vDepthIdx;
      vDepthIdx.reserve(mLastFrame.N);
  
      for(int i=0; i<mLastFrame.N;i++)
      {
          float z = mLastFrame.mvDepth[i];
          if(z>0)
          {
              // vDepthIdx第一个元素是某个点的深度,第二个元素是对应的特征点id
              vDepthIdx.push_back(make_pair(z,i));
          }
      }
  
      // 如果上一帧中没有有效深度的点,那么就直接退出
      if(vDepthIdx.empty())
          return;
  
      // 按照深度从小到大排序
      sort(vDepthIdx.begin(),vDepthIdx.end());
  
      // We insert all close points (depth
      // If less than 100 close points, we insert the 100 closest ones.
      // Step 2.2：从中找出不是地图点的部分  
      int nPoints = 0;
      for(size_t j=0; j<vDepthIdx.size();j++)
      {
          int i = vDepthIdx[j].second;
  
          bool bCreateNew = false;
  
          // 如果这个点对应在上一帧中的地图点没有,或者创建后就没有被观测到,那么就生成一个临时的地图点
          MapPoint* pMP = mLastFrame.mvpMapPoints[i];
          if(!pMP)
              bCreateNew = true;
          else if(pMP->Observations()<1)      
          {
              // 地图点被创建后就没有被观测，认为不靠谱，也需要重新创建
              bCreateNew = true;
          }
  
          if(bCreateNew)
          {
              // Step 2.3：需要创建的点，包装为地图点。只是为了提高双目和RGBD的跟踪成功率，并没有添加复杂属性，因为后面会扔掉
              // 反投影到世界坐标系中
              cv::Mat x3D = mLastFrame.UnprojectStereo(i);
              MapPoint* pNewMP = new MapPoint(
                  x3D,            // 世界坐标系坐标
                  mpMap,          // 跟踪的全局地图
                  &mLastFrame,    // 存在这个特征点的帧(上一帧)
                  i);             // 特征点id
  
              // 加入上一帧的地图点中
              mLastFrame.mvpMapPoints[i]=pNewMP; 
  
              // 标记为临时添加的MapPoint，之后在CreateNewKeyFrame之前会全部删除
              mlpTemporalPoints.push_back(pNewMP);
              nPoints++;
          }
          else
          {
              // 因为从近到远排序，记录其中不需要创建地图点的个数
              nPoints++;
          }
  
          // Step 2.4：如果地图点质量不好，停止创建地图点
          // 停止新增临时地图点必须同时满足以下条件：
          // 1、当前的点的深度已经超过了设定的深度阈值（35倍基线）
          // 2、nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出
          if(vDepthIdx[j].first>mThDepth && nPoints>100)
              break;
      }
  }
  

• 根据上一帧计算出来的速度和上一帧的位姿来计算出当前帧的初始位姿，这也是匀速模型的名称由来

  // Step 2：根据之前估计的速度，用恒速模型得到当前帧的初始位姿。
  mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
  
  // 清空当前帧的地图点
  fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
  
  // Project points seen in previous frame
  // 设置特征匹配过程中的搜索半径
  int th;
  if(mSensor!=System::STEREO)
      th=15;//单目
  else
      th=7;//双目
  

• 根据上一帧和当前帧的位姿，使用PNP投影的方式来搜索匹配关系

  // Step 3：用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次
  int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
  
  // If few matches, uses a wider window search
  // 如果匹配点太少，则扩大搜索半径再来一次
  if(nmatches<20)
  {
      fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
      nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR); // 2*th
  }
  
  // 如果还是不能够获得足够的匹配点,那么就认为跟踪失败
  if(nmatches<20)
      return false;
  

  其中函数展开为

  int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)
  {
      int nmatches = 0;
  
      // Rotation Histogram (to check rotation consistency)
      // Step 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;
  
      // Step 2 计算当前帧和前一帧的平移向量
      //当前帧的相机位姿
      const cv::Mat Rcw = CurrentFrame.mTcw.rowRange(0,3).colRange(0,3);
      const cv::Mat tcw = CurrentFrame.mTcw.rowRange(0,3).col(3);
  
      //当前相机坐标系到世界坐标系的平移向量
      const cv::Mat twc = -Rcw.t()*tcw; 
  
      //上一帧的相机位姿
      const cv::Mat Rlw = LastFrame.mTcw.rowRange(0,3).colRange(0,3);
      const cv::Mat tlw = LastFrame.mTcw.rowRange(0,3).col(3); // tlw(l)
  
      // vector from LastFrame to CurrentFrame expressed in LastFrame
      // 当前帧相对于上一帧相机的平移向量
      const cv::Mat tlc = Rlw*twc+tlw; 
  
      // 判断前进还是后退
      const bool bForward = tlc.at<float>(2) > CurrentFrame.mb && !bMono;     // 非单目情况，如果Z大于基线，则表示相机明显前进
      const bool bBackward = -tlc.at<float>(2) > CurrentFrame.mb && !bMono;   // 非单目情况，如果-Z小于基线，则表示相机明显后退
  
      //  Step 3 对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标
      for(int i=0; i<LastFrame.N; i++)
      {
          MapPoint* pMP = LastFrame.mvpMapPoints[i];
  
          if(pMP)
          {
              if(!LastFrame.mvbOutlier[i])
              {
                  // 对上一帧有效的MapPoints投影到当前帧坐标系
                  cv::Mat x3Dw = pMP->GetWorldPos();
                  cv::Mat x3Dc = Rcw*x3Dw+tcw;
  
                  const float xc = x3Dc.at<float>(0);
                  const float yc = x3Dc.at<float>(1);
                  const float invzc = 1.0/x3Dc.at<float>(2);
  
                  if(invzc<0)
                      continue;
  
                  // 投影到当前帧中
                  float u = CurrentFrame.fx*xc*invzc+CurrentFrame.cx;
                  float v = CurrentFrame.fy*yc*invzc+CurrentFrame.cy;
  
                  if(u<CurrentFrame.mnMinX || u>CurrentFrame.mnMaxX)
                      continue;
                  if(v<CurrentFrame.mnMinY || v>CurrentFrame.mnMaxY)
                      continue;
  
                  // 上一帧中地图点对应二维特征点所在的金字塔层级
                  int nLastOctave = LastFrame.mvKeys[i].octave;
  
                  // Search in a window. Size depends on scale
                  // 单目：th = 7，双目：th = 15
                  float radius = th*CurrentFrame.mvScaleFactors[nLastOctave]; // 尺度越大，搜索范围越大
  
                  // 记录候选匹配点的id
                  vector<size_t> vIndices2;         
  
                  // Step 4 根据相机的前后前进方向来判断搜索尺度范围。
                  // 以下可以这么理解，例如一个有一定面积的圆点，在某个尺度n下它是一个特征点
                  // 当相机前进时，圆点的面积增大，在某个尺度m下它是一个特征点，由于面积增大，则需要在更高的尺度下才能检测出来
                  // 当相机后退时，圆点的面积减小，在某个尺度m下它是一个特征点，由于面积减小，则需要在更低的尺度下才能检测出来
                  if(bForward) // 前进,则上一帧兴趣点在所在的尺度nLastOctave<=nCurOctave
                      vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave);
                  else if(bBackward) // 后退,则上一帧兴趣点在所在的尺度0<=nCurOctave<=nLastOctave
                      vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, 0, nLastOctave);
                  else // 在[nLastOctave-1, nLastOctave+1]中搜索
                      vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave-1, nLastOctave+1);
  
                  if(vIndices2.empty())
                      continue;
  
                  const cv::Mat dMP = pMP->GetDescriptor();
  
                  int bestDist = 256;
                  int bestIdx2 = -1;
  
                  // Step 5 遍历候选匹配点，寻找距离最小的最佳匹配点 
                  for(vector<size_t>::const_iterator vit=vIndices2.begin(), vend=vIndices2.end(); vit!=vend; vit++)
                  {
                      const size_t i2 = *vit;
  
                      // 如果该特征点已经有对应的MapPoint了,则退出该次循环
                      if(CurrentFrame.mvpMapPoints[i2])
                          if(CurrentFrame.mvpMapPoints[i2]->Observations()>0)
                              continue;
  
                      if(CurrentFrame.mvuRight[i2]>0)
                      {
                          // 双目和rgbd的情况，需要保证右图的点也在搜索半径以内
                          const float ur = u - CurrentFrame.mbf*invzc;
                          const float er = fabs(ur - CurrentFrame.mvuRight[i2]);
                          if(er>radius)
                              continue;
                      }
  
                      const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);
  
                      const int dist = DescriptorDistance(dMP,d);
  
                      if(dist<bestDist)
                      {
                          bestDist=dist;
                          bestIdx2=i2;
                      }
                  }
  
                  // 最佳匹配距离要小于设定阈值
                  if(bestDist<=TH_HIGH)
                  {
                      CurrentFrame.mvpMapPoints[bestIdx2]=pMP; 
                      nmatches++;
  
                      // Step 6 计算匹配点旋转角度差所在的直方图
                      if(mbCheckOrientation)
                      {
                          float rot = LastFrame.mvKeysUn[i].angle-CurrentFrame.mvKeysUn[bestIdx2].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(bestIdx2);
                      }
                  }
              }
          }
      }
  
      //Apply rotation consistency
      //  Step 7 进行旋转一致检测，剔除不一致的匹配
      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++)
          {
              // 对于数量不是前3个的点对，剔除
              if(i!=ind1 && i!=ind2 && i!=ind3)
              {
                  for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
                  {
                      CurrentFrame.mvpMapPoints[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
                      nmatches--;
                  }
              }
          }
      }
  
      return nmatches;
  }
  

• 使用纯运动BA来优化当前帧的位姿

  // Step 4：利用3D-2D投影关系，优化当前帧位姿
  Optimizer::PoseOptimization(&mCurrentFrame);
  

• 剔除外点，并根据获得的匹配数量来判断是否跟踪成功

  // Discard outliers
  // 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++;
      }
  }    
  
  if(mbOnlyTracking)
  {
      // 纯定位模式下：如果成功追踪的地图点非常少,那么这里的mbVO标志就会置位
      mbVO = nmatchesMap<10;
      return nmatches>20;
  }
  
  // Step 6：匹配超过10个点就认为跟踪成功
  return nmatchesMap>=10;