SVO 特征对齐代码分析

　　SVO稀疏图像对齐之后使用特征对齐，即通过地图向当前帧投影，并使用逆向组合光流以稀疏图像对齐的结果为初始值，得到更精确的特征位置。

　　主要涉及文件：

reprojector.cpp

matcher.cpp

feature_alignment.cpp

point.cpp

map.cpp

1.入口函数：

void Reprojector::reprojectMap(
    FramePtr frame,
    std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)
{
  resetGrid();

  // Identify those Keyframes which share a common field of view.
  SVO_START_TIMER("reproject_kfs");
  //计算当前地图中的关键帧与当前帧frame具有共视关系的关键帧，并返回两帧之间的距离；
  list< pair<FramePtr,double> > close_kfs;
  map_.getCloseKeyframes(frame, close_kfs);

  //按照距离进行排序；
  // Sort KFs with overlap according to their closeness
  close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
                 boost::bind(&std::pair<FramePtr, double>::second, _2));

  // Reproject all mappoints of the closest N kfs with overlap. We only store
  // in which grid cell the points fall.
  size_t n = 0;
  overlap_kfs.reserve(options_.max_n_kfs);    //关键帧共视关系数量限制；
  for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end();
      it_frame!=ite_frame && n<options_.max_n_kfs; ++it_frame, ++n)
  {
    FramePtr ref_frame = it_frame->first;

    ref_frame->debug_img_ = ref_frame->img().clone();   //every reproject iteration, the debug_img_ should be reinition

    overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));
    // Try to reproject each mappoint that the other KF observes
    for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end();
        it_ftr!=ite_ftr; ++it_ftr)
    {
      // check if the feature has a mappoint assigned
      if((*it_ftr)->point == NULL)
        continue;

      //判断该特征是否已经投影过了
      // make sure we project a point only once
      if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)
        continue;
      (*it_ftr)->point->last_projected_kf_id_ = frame->id_;
      //如果参考帧的一个特征在当前帧中投影成功，计数+1；
      if(reprojectPoint(frame, (*it_ftr)->point))
        overlap_kfs.back().second++;
    }
  }
  SVO_STOP_TIMER("reproject_kfs");// Now project all point candidates
  //投影候选点；
  SVO_START_TIMER("reproject_candidates");
  {
    boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
    auto it=map_.point_candidates_.candidates_.begin();

    while(it!=map_.point_candidates_.candidates_.end())
    {
      if(!reprojectPoint(frame, it->first))
      {
        //候选点如果一直都投影不上，说明这个点可能有问题，可以把它删除；
        it->first->n_failed_reproj_ += 3;
        if(it->first->n_failed_reproj_ > 30)
        {
          map_.point_candidates_.deleteCandidate(*it);
          it = map_.point_candidates_.candidates_.erase(it);
          continue;
        }
      }
      ++it;
    }
  } // unlock the mutex when out of scope
  SVO_STOP_TIMER("reproject_candidates");

  // Now we go through each grid cell and select one point to match.
  // At the end, we should have at maximum one reprojected point per cell.
  SVO_START_TIMER("feature_align");
  for(size_t i=0; i<grid_.cells.size(); ++i)
  {
    // we prefer good quality points over unkown quality (more likely to match)
    // and unknown quality over candidates (position not optimized)
    //随机的投影每个cell，每投影成功一次+1；
    if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
      ++n_matches_;

    if(n_matches_ > (size_t) Config::maxFts())
      break;

  }

  SVO_STOP_TIMER("feature_align");
}

2. 计算获得和frame具有共视关系的帧，并返回这些帧和与frame的距离：

//计算frame和当前地图关键帧中具有共视关系的帧，并计算两帧之间的距离；
void Map::getCloseKeyframes(
    const FramePtr& frame,
    std::list< std::pair<FramePtr,double> >& close_kfs) const
{
  //遍历所有关键帧；
  for(auto kf : keyframes_)
  {
    //遍历关键帧中的特征点，计算是否具有共视关系；
    // check if kf has overlaping field of view with frame, use therefore KeyPoints
    for(auto keypoint : kf->key_pts_)
    {
      if(keypoint == nullptr)
        continue;

      //如果有共视关系，则记录该关键帧和当前帧与该关键帧的距离；
      if(frame->isVisible(keypoint->point->pos_))
      {
        close_kfs.push_back(
            std::make_pair(
                kf, (frame->T_f_w_.translation()-kf->T_f_w_.translation()).norm()));
        break; // this keyframe has an overlapping field of view -> add to close_kfs
      }
    }
  }
}

3.把点投影到当前帧中对应的cell中：

bool Reprojector::reprojectPoint(FramePtr frame, Point* point)
{
  //世界坐标，变换到当前帧下，然后投影到像素坐标；
  Vector2d px(frame->w2c(point->pos_));
  //判断8*8的patch是否在帧内
  if(frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher
  {
    //判断该点落在了那个cell内，则对应的cell内增加候选点；
    const int k = static_cast<int>(px[1]/grid_.cell_size)*grid_.grid_n_cols
                + static_cast<int>(px[0]/grid_.cell_size);
    grid_.cells.at(k)->push_back(Candidate(point, px));
    return true;
  }
  return false;
}

4.投影每个cell，比较重要，

bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
{
  cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));//质量排序；
  Cell::iterator it=cell.begin();
  //遍历cell中的Candidate；
  while(it!=cell.end())
  {
    ++n_trials_;

    if(it->pt->type_ == Point::TYPE_DELETED)
    {
      it = cell.erase(it);
      continue;
    }

    //通过光流法估计patch的位置；
    bool found_match = true;
    if(options_.find_match_direct)
      found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
    if(!found_match)
    {
      it->pt->n_failed_reproj_++;
      if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
        map_.safeDeletePoint(it->pt);
      if(it->pt->type_ == Point::TYPE_CANDIDATE  && it->pt->n_failed_reproj_ > 30)
        map_.point_candidates_.deleteCandidatePoint(it->pt);
      it = cell.erase(it);

      continue;
    }

    //投影成功+1；
    it->pt->n_succeeded_reproj_++;
    if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
      it->pt->type_ = Point::TYPE_GOOD;

    //给当前帧中增加新的feature
    Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);
    frame->addFeature(new_feature);

    // Here we add a reference in the feature to the 3D point, the other way
    // round is only done if this frame is selected as keyframe.
    new_feature->point = it->pt;

    if(matcher_.ref_ftr_->type == Feature::EDGELET)
    {
      new_feature->type = Feature::EDGELET;
      new_feature->grad = matcher_.A_cur_ref_*matcher_.ref_ftr_->grad;
      new_feature->grad.normalize();
    }

    // If the keyframe is selected and we reproject the rest, we don't have to
    // check this point anymore.
    it = cell.erase(it);

    //每个cell里面最多有一个point；
    // Maximum one point per cell.
    return true;
  }
  return false;
}

5.光流法，逆向组合式：

patch_with_border_和patch_的区别是前者是带有border的patch，因为后面光流计算时需要计算梯度方向，使用的平均梯度，所以计算边界梯度时需要增加一行或一列；

bool Matcher::findMatchDirect(
    const Point& pt,
    const Frame& cur_frame,
    Vector2d& px_cur)
{
  //计算cur_frame与观察到pt该点的特征中视角最小的一帧；
  if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))
  {
          return false;
  }


  //判断是否在图像内部；
  if(!ref_ftr_->frame->cam_->isInFrame(
      ref_ftr_->px.cast<int>()/(1<<ref_ftr_->level), halfpatch_size_+2, ref_ftr_->level))
  {
          return false;
  }

  //计算仿射变换矩阵2*2；
  // warp affine
  warp::getWarpMatrixAffine(
      *ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f,
      (ref_ftr_->frame->pos() - pt.pos_).norm(),
      cur_frame.T_f_w_ * ref_ftr_->frame->T_f_w_.inverse(), ref_ftr_->level, A_cur_ref_);

  //获取搜索层，不明白函数原理；
  search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);

  //计算该特征的仿射变换，由当前帧到参考帧；
  warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
                   ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_);

  createPatchFromPatchWithBorder();

  // px_cur should be set变换到搜索尺度下
  Vector2d px_scaled(px_cur/(1<<search_level_));

  //前面已经计算了参考帧到当前帧的仿射变换，并且已经获得了对应的patch；
  bool success = false;
  if(ref_ftr_->type == Feature::EDGELET)
  {
    Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad);
    dir_cur.normalize();
    success = feature_alignment::align1D(
          cur_frame.img_pyr_[search_level_], dir_cur.cast<float>(),
          patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
  }
  else
  {
    //特征对齐；
    success = feature_alignment::align2D(
      cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
      options_.align_max_iter, px_scaled);
  }
  //返回原始尺度；
  px_cur = px_scaled * (1<<search_level_);
  return success;
}

6.计算获得与ftr视角最小的一帧，地图中关键帧不多，速度比较快：

bool Point::getCloseViewObs(const Vector3d& framepos, Feature*& ftr) const
{
  // TODO: get frame with same point of view AND same pyramid level!
  Vector3d obs_dir(framepos - pos_); obs_dir.normalize();
  auto min_it=obs_.begin();//观察到该点的关键帧列表；
  double min_cos_angle = 0;
  for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
  {
    //计算该关键帧与该点的距离；
    Vector3d dir((*it)->frame->pos() - pos_); dir.normalize();
    //计算余弦值；
    double cos_angle = obs_dir.dot(dir);
    //获取视角最小的关键帧；
    if(cos_angle > min_cos_angle)
    {
      min_cos_angle = cos_angle;
      min_it = it;
    }
  }
  ftr = *min_it;
  //不能大于60°
  if(min_cos_angle < 0.5) // assume that observations larger than 60° are useless
    return false;
  return true;
}

7.计算仿射变换：即patch因为视角的变换，应该具有一定的扭曲

//计算仿射变换；
void getWarpMatrixAffine(
    const svo::AbstractCamera& cam_ref,
    const svo::AbstractCamera& cam_cur,
    const Vector2d& px_ref,
    const Vector3d& f_ref,
    const double depth_ref,
    const SE3& T_cur_ref,
    const int level_ref,
    Matrix2d& A_cur_ref)
{
  // Compute affine warp matrix A_cur_ref
  const int halfpatch_size = 5;//5*5的窗口；
  const Vector3d xyz_ref(f_ref*depth_ref);//归一化的相机坐标乘以深度；

  //u方向的边界点和v方向的边界点，注意特征所在的金字塔level
  Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<<level_ref)));  //  patch tranfrom to the level0 pyr img
  Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref)));  //  px_ref is located at level0
                                                                                               //  attation!!!! so, A_cur_ref  is only used to affine warp patch at level0
  //因为xyz_du_ref返回的是归一化的3D坐标，所以要借助xyz_ref点的深度计算；
  xyz_du_ref *= xyz_ref[2]/xyz_du_ref[2];
  xyz_dv_ref *= xyz_ref[2]/xyz_dv_ref[2];

  //上面的三个点分别投影到当前帧；
  const Vector2d px_cur(cam_cur.world2cam(T_cur_ref*(xyz_ref)));
  const Vector2d px_du(cam_cur.world2cam(T_cur_ref*(xyz_du_ref)));
  const Vector2d px_dv(cam_cur.world2cam(T_cur_ref*(xyz_dv_ref)));

  //仿射变换，其实是一种在x和y方向的变化率；
  A_cur_ref.col(0) = (px_du - px_cur)/halfpatch_size;
  A_cur_ref.col(1) = (px_dv - px_cur)/halfpatch_size;
}

8.通过仿射变换计算patch值：

void warpAffine(
    const Matrix2d& A_cur_ref,
    const cv::Mat& img_ref,
    const Vector2d& px_ref,
    const int level_ref,
    const int search_level,
    const int halfpatch_size,
    uint8_t* patch)
{
  const int patch_size = halfpatch_size*2 ;
  const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();   //逆向组合法，所以计算的是有当前帧到参考帧之间的变换
  if(isnan(A_ref_cur(0,0)))
  {
    printf("Affine warp is NaN, probably camera has no translation
"); // TODO
    return;
  }

  // Perform the warp on a larger patch.
  uint8_t* patch_ptr = patch;
  const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref);
  for (int y=0; y<patch_size; ++y)
  {
    for (int x=0; x<patch_size; ++x, ++patch_ptr)
    {
      Vector2f px_patch(x-halfpatch_size, y-halfpatch_size);  // px_patch is locat at  pyr [ref_level ]
      px_patch *= (1<<search_level);//  1. patch tranform to level0,  because A_ref_cur is only used to affine warp level0 patch
      const Vector2f px(A_ref_cur*px_patch + px_ref_pyr);    //  2. then, use A_ref_cur  to affine warp the patch
      if (px[0]<0 || px[1]<0 || px[0]>=img_ref.cols-1 || px[1]>=img_ref.rows-1)
        *patch_ptr = 0;
      else
      {
        //双线性插值
        *patch_ptr = (uint8_t) interpolateMat_8u(img_ref, px[0], px[1]);   // img_ref  is the  img at pyr[level]
      }
    }
  }
}

9.给带边界的patch赋值：

//主要是给patch_填值；
void Matcher::createPatchFromPatchWithBorder()
{
  uint8_t* ref_patch_ptr = patch_;
  //为什么从1开始?因为横纵都+1，为什么横纵都+2，因为后面光流时要计算梯度，用到了patch外的一行或一列数据
  for(int y=1; y<patch_size_+1; ++y, ref_patch_ptr += patch_size_)
  {
    uint8_t* ref_patch_border_ptr = patch_with_border_ + y*(patch_size_+2) + 1;
    for(int x=0; x<patch_size_; ++x)
      ref_patch_ptr[x] = ref_patch_border_ptr[x];
  }
}

10.光流法的主要过程：

bool align2D(
    const cv::Mat& cur_img,
    uint8_t* ref_patch_with_border,
    uint8_t* ref_patch,
    const int n_iter,
    Vector2d& cur_px_estimate,
    bool no_simd)
{
  const int halfpatch_size_ = 4;
  const int patch_size_ = 8;
  const int patch_area_ = 64;
  bool converged=false;

  // compute derivative of template and prepare inverse compositional
  float __attribute__((__aligned__(16))) ref_patch_dx[patch_area_];
  float __attribute__((__aligned__(16))) ref_patch_dy[patch_area_];
  Matrix3f H; H.setZero();

  // compute gradient and hessian
  const int ref_step = patch_size_+2;
  float* it_dx = ref_patch_dx;
  float* it_dy = ref_patch_dy;
  for(int y=0; y<patch_size_; ++y)
  {
    uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
    for(int x=0; x<patch_size_; ++x, ++it, ++it_dx, ++it_dy)
    {
      Vector3f J;
      //计算梯度方向；
      J[0] = 0.5 * (it[1] - it[-1]);
      J[1] = 0.5 * (it[ref_step] - it[-ref_step]);
      J[2] = 1;
      //梯度赋值，即保存每个像素点的梯度信息，因为是逆向组合算法，所以计算的是参考帧上的梯度信息；
      *it_dx = J[0];
      *it_dy = J[1];
      H += J*J.transpose();
    }
  }
  Matrix3f Hinv = H.inverse();
  float mean_diff = 0;

  //估算当前帧中的位置，因为前面的直接法已经有了光流初始值；
  // Compute pixel location in new image:
  float u = cur_px_estimate.x();
  float v = cur_px_estimate.y();

  // termination condition
  const float min_update_squared = 0.03*0.03;   //  origin param: 0.03 * 0.03
  const int cur_step = cur_img.step.p[0];
//  float chi2 = 0;
  //开始迭代优化；
  Vector3f update; update.setZero();
  for(int iter = 0; iter<n_iter; ++iter)
  {
    int u_r = floor(u);
    int v_r = floor(v);
    //判断是否越界；应该是不会；
    if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_)
      break;

    if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
      return false;

    //双线性插值权重
    // compute interpolation weights
    float subpix_x = u-u_r;
    float subpix_y = v-v_r;
    float wTL = (1.0-subpix_x)*(1.0-subpix_y);
    float wTR = subpix_x * (1.0-subpix_y);
    float wBL = (1.0-subpix_x)*subpix_y;
    float wBR = subpix_x * subpix_y;

    // loop through search_patch, interpolate
    uint8_t* it_ref = ref_patch;
    float* it_ref_dx = ref_patch_dx;
    float* it_ref_dy = ref_patch_dy;
//    float new_chi2 = 0.0;
    Vector3f Jres; Jres.setZero();
    for(int y=0; y<patch_size_; ++y)
    {
      uint8_t* it = (uint8_t*) cur_img.data + (v_r+y-halfpatch_size_)*cur_step + u_r-halfpatch_size_;
      for(int x=0; x<patch_size_; ++x, ++it, ++it_ref, ++it_ref_dx, ++it_ref_dy)
      {
        float search_pixel = wTL*it[0] + wTR*it[1] + wBL*it[cur_step] + wBR*it[cur_step+1];
        //计算当前帧和参考帧patch像素点之间的残差；
        float res = search_pixel - *it_ref + mean_diff;
        //残差乘以梯度为b；
        Jres[0] -= res*(*it_ref_dx);
        Jres[1] -= res*(*it_ref_dy);
        Jres[2] -= res;
//        new_chi2 += res*res;
      }
    }

    //更新值为Hinv*b；
    update = Hinv * Jres;
    u += update[0];
    v += update[1];
    mean_diff += update[2];

    if(update[0]*update[0]+update[1]*update[1] < min_update_squared)
    {
      converged=true;
      break;
    }
  }

  cur_px_estimate << u, v;
  return converged;
}

　　通过以上函数，完成了基于patch的特征匹配，后续就是通过高斯牛顿法优化相机位姿了。