SVO学习笔记（一）

这篇文章

 一个很年轻的叔叔踩进了SLAM的坑，现在正在学习视觉SLAM中的SVO系统。本着好记性不如烂笔头的思想，我想记录下整个学习过程。希望这个笔记能加深自己对系统的理解，也希望它能给读者带来启发（水平一般，但还是要有这种梦想）。
 所谓一口吃不成胖子，啃代码也是一个道理。所以这次先记录Frame、Featuredetection和Featurematcher三部分的内容。为了控制篇幅，只针对我认为比较重要和特殊的部分进行记录。

Frame

 Frame文件中比较特殊的是存储了五个特殊的关键点的容器。这五个点用于判断图像间的共视情况(看有没有重叠区域)。这部分的代码如图：

void Frame::setKeyPoints()
{
  //该循环确定图中特征点（把不好的去掉）
  for(size_t i = 0; i < 5; ++i)
    if(key_pts_[i] != NULL)
      if(key_pts_[i]->point == NULL)
      //如果某个KeyPoints没有指向任何地图点，则重新寻找新的KeyPoints
      std::for_each(fts_.begin(), fts_.end(), [&](Feature* ftr){ if(ftr->point != NULL) checkKeyPoints(ftr); });
      //checkKeyPoints函数用于找出那五个特殊的特征点
}

&esmp;Frame中保存的五个特殊关键点分别是：1、距离图片中心最近的。用1)将图片分成四等分，在分好后的四个角落各找出一个点（2，3，4，5），它们越接近图片边界越好(这些点都要有对应的3D点)。这五个特殊点会不断地更新（我想这段话也能够解释checkKeyPoints函数了。这个函数就是不断地寻找更符上述条件的五个点）。

Featuredetection

 SVO中的特征对象是作者自己定义的(以FAST角点为基础)，即文件中的Corner类，它保存了角点的位置、得分、角度（梯度方向）和金字塔层数这些信息。
 不过特征点的提取策略有些特别，我们一步一步地来看。

void FastDetector::detect(
    Frame* frame,
    const ImgPyr& img_pyr,
    const double detection_threshold,
    Features& fts)
{
//初始化角点存储容器，其下标与网格的数量对应
  Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f));
  //遍历金字塔中的每幅图像，提取FAST角点：
  for(int L=0; L<n_pyr_levels_; ++L)
  {
    const int scale = (1<<L);
    vector<fast::fast_xy> fast_corners;
    //提取FAST角点（可以使用不同的方法）
    //代码就不贴了，劳烦读者参考源码。
    //..................................
     vector<int> scores, nm_corners;
     //计算各角点的得分
    fast::fast_corner_score_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, fast_corners, 20, scores);
    //对提取的角点进行非极大值抑制，nmcorner记录那些被选出来的极大值的角点的下标
    fast::fast_nonmax_3x3(fast_corners, scores, nm_corners);
    //整个过程中最重要的一步：控制特征点的分布，保证每个网格中只有一个特征点。
    for(auto it=nm_corners.begin(), ite=nm_corners.end(); it!=ite; ++it)
    {
      fast::fast_xy& xy = fast_corners.at(*it);
      const int k = static_cast<int>((xy.y*scale)/cell_size_)*grid_n_cols_
                  + static_cast<int>((xy.x*scale)/cell_size_);
      if(grid_occupancy_[k])
      //只对那些还没被占据的网格设置角点
        continue;
      const float score = vk::shiTomasiScore(img_pyr[L], xy.x, xy.y);
      if(score > corners.at(k).score)
      //每个网格中只能有一个角点，所以网格只存放出现在这里得分最高的角点
        corners.at(k) = Corner(xy.x*scale, xy.y*scale, score, L, 0.0f);
    }
  }
  // 对网格中所有的特征点进行筛选，只保留得分大于阈值的点
  std::for_each(corners.begin(), corners.end(), [&](Corner& c) {
    if(c.score > detection_threshold)
      fts.push_back(new Feature(frame, Vector2d(c.x, c.y), c.level));
  });
   resetGrid();
}

 特征提取的特点就在于将图片用网格进行了划分，且每个网格中只保留一个特征点。这样能够让特征点分布的比较均匀。

Featrue_matcher

 这个文件要讲的是三角测量求深度、非线性优化提炼精确匹配特征、极线搜索匹配特征。这几个是我认为比较重要的内容，其余部分，比如仿射变换等，都是比较容易明白的。

三角测量求深度

//功能：通过两帧间匹配特征点的归一化坐标，进行三角测量来获得对应地图点的坐标。
bool depthFromTriangulation(
    const SE3& T_search_ref,//从参考帧到当前帧的位姿变换矩阵
    const Vector3d& f_ref,//归一化平面的坐标
    const Vector3d& f_cur,
    double& depth)
{
  Matrix<double,3,2> A; A << T_search_ref.rotation_matrix() * f_ref, f_cur;
  const Matrix2d AtA = A.transpose()*A;
  if(AtA.determinant() < 0.000001)
    return false;
  AtA.col(1) = -1*AtA.col(1);//这里源码中没有，但是我觉得应该是要加上的
//深度计算的公式是：d = - (ATA)^(-1) * AT * t
  const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation();
  depth = fabs(depth2[0]);//返回的是参考帧中的深度
  return true;
}

 代码中需要讲解的就是深度的求解公式。它的具体推到过程可参考这个博客

特征匹配

 特征匹配是由最小化以关键点为中心的一个像素块的光度误差的方法实现的。这部分要注意的是像素块的选取。将当前帧中的像素块定义为curpatch(默认为一个正方形)，参考帧的定义为refpatch。考虑到两帧间的仿射变换，与curpatch相对应的refpatch在形状和方向可能与curpatch不同，比如下图这个例子：

 P1和P2本是匹配的特征，但在旋转作用下，它们的像素块内像素刚好相反。这就导致它们的光度误差较大，无法被判定为匹配。另一种情况是两个匹配像素块的形状不一定相同（当前帧中正方形的像素块在参考帧中的匹配可能是只是平行四边形），若此时两个像素块仍取正方形，那么块中的像素可能不具有匹配关系。
 上述两种情况都会降低匹配的效果。因此要通过仿射变换，找到两个正确的像素块，以保证其中像素的正确匹配。这是最重要的前提条件（不过一般在论文中的featurealign部分才使用。因为sparseimgalign部分是针对前后两帧匹配，所以两个像素块的形状差异较小）。说完前提，接下来介绍各种匹配方法的函数。首先介绍非线性优化寻找匹配特征的函数。

非线性优化寻找匹配特征

//这个是论文当中的Relaxation Through Feature Alignment部分
//使用非线性优化的方式寻找匹配结果：px_cur是先前由极线搜索获得的匹配点坐标，此时用高斯牛顿法将像素块中的光度误差最小化
//函数中会寻找与pt到cur的观测方向间夹角最小（共视程度最高）的一个KF作为参考帧，
bool Matcher::findMatchDirect(
    const Point& pt,//匹配特征点对所对应的地图点
    const Frame& cur_frame,//当前帧
    Vector2d& px_cur)//地图点当前帧的匹配点坐标的初始估计
{
//找到观测角度与当前帧很接近的参考帧（求地图点到当前帧的向量和到参考帧的向量，这两个向量间的夹角要最小）
  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;
  warp::getWarpMatrixAffine(//你懂的
      *ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f,
      (ref_ftr_->frame->pos() - pt.pos_).norm(),//地图点在ref系的深度
      cur_frame.T_f_w_ * ref_ftr_->frame->T_f_w_.inverse(), ref_ftr_->level, A_cur_ref_);
  //找出当前帧中的匹配特征点最可能出现在curframe中第几层金字塔上
  search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
  //在参考帧中某一层金字塔上的图像中寻找对应的refpatch
  //这里是为当前帧匹配特征点的patch在refframe中找到对应的像素块。不过找的是带边框的。
  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();
  Vector2d px_scaled(px_cur/(1<<search_level_));
  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//主要使用这一部分
  {
  //patch是参考关键帧中的像素块。在cur中搜索的像素块都认为是正方形
    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;
}

极线搜索匹配特征

 这部分是通过极限搜索的方式来找出参考帧与当前帧的匹配关系。

//极线搜索匹配点。搜索也是在某一层金字塔中进行
/*这一步的前提是已经获得了当前帧的初始位姿估计，然后再来寻找相应的匹配特征
1、计算出当前帧curframe和参考帧refframe间的相对位姿
2、为refframe中那些深度值较好的特征点（地图点）在curframe上进行极限搜索，寻找对应的匹配点（极线搜索是在某一层金字塔中完成）
3、找到匹配点后，进行三角测量，优化地图点的深度，以便给地图添加好点。*/
bool Matcher::findEpipolarMatchDirect(
    const Frame& ref_frame,
    const Frame& cur_frame,
    const Feature& ref_ftr,//参考帧中的特征点
    const double d_estimate,//该特征点的深度估计值
    const double d_min,//深度波动的范围（最小和最大的深度值）
    const double d_max,
    double& depth)//找到匹配点对后，新估计的地图点深度。用于之后的深度优化
{
  SE3 T_cur_ref = cur_frame.T_f_w_ * ref_frame.T_f_w_.inverse();
  int zmssd_best = PatchScore::threshold();//阈值
  //匹配方式是ZMSSD（Zero Mean Sum of Squared Differences）一种光度误差计算的方法
  Vector2d uv_best;//匹配的最终结果
  //由dmin和dmax来确定投影在cur上的极线（在dmin、dmax这一距离上的所有地图点都可能在ref_ftr上成像）
  Vector2d A = vk::project2d(T_cur_ref * (ref_ftr.f*d_min));
  Vector2d B = vk::project2d(T_cur_ref * (ref_ftr.f*d_max));
//注意：这里的A、B都是在归一化平面上的点坐标
 epi_dir_ = A - B;
 //极线向量，主要用这个来确定极线的方向
 warp::getWarpMatrixAffine(//你懂的
      *ref_frame.cam_, *cur_frame.cam_, ref_ftr.px, ref_ftr.f,
      d_estimate, T_cur_ref, ref_ftr.level, A_cur_ref_);

 此时就找到了极线的方向和两帧间仿射变换。之后分情况选择极线搜索或者是非线性优化。如果极线的长度很短，那么就可通过非线性优化就能够找到匹配特征点；否则就老老实实地在极线上一步步地找。同时搜索的方法还和特征的类型有关，此处主要讲点特征的搜索（对不起，太懒了）。

search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
Vector2d px_A(cur_frame.cam_->world2cam(A));//极线上两端的像素点
Vector2d px_B(cur_frame.cam_->world2cam(B));
  //在某层金字塔中的极线的长度（在这条极线上进行搜索）
epi_length_ = (px_A-px_B).norm() / (1<<search_level_);
warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,
                   ref_ftr.level, search_level_, halfpatch_size_+1, patch_with_border_);
createPatchFromPatchWithBorder();
//极线长度很小时就意味着距离最终结果只差一点像素偏移。所以这就直接使用直接法求解就行
if(epi_length_ < 2.0)
  {
    px_cur_ = (px_A+px_B)/2.0;//用极线的中点作为初始点
    Vector2d px_scaled(px_cur_/(1<<search_level_));
    bool res;
    if(options_.align_1d)
      res = feature_alignment::align1D(//边缘型特征
          cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
          patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
    else
      res = feature_alignment::align2D(//角点型特征
          cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
          options_.align_max_iter, px_scaled);//是在特征点出现的那层图像金字塔图像中进行极限搜索，px_scaled是那层图像中的像素坐标
    if(res)
    {
      px_cur_ = px_scaled*(1<<search_level_);//把像素坐标恢复到原始图像坐标系中
      if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))//找到匹配点对后，新估计的地图点深度。用于之后的深度优化
        return true;
    }
    return false;
  }
  size_t n_steps = epi_length_/0.7; 
  Vector2d step = epi_dir_/n_steps;//每一步需要增加的像素坐标变化量
  if(n_steps > options_.max_epi_search_steps)
  {
    printf("WARNING: skip epipolar search: %zu evaluations, px_lenght=%f, d_min=%f, d_max=%f.\n",
           n_steps, epi_length_, d_min, d_max);
    return false;
  }
  int pixel_sum = 0;
  int pixel_sum_square = 0;
  PatchScore patch_score(patch_);//用来计算两个patch间的误差的一个类
  Vector2d uv = B-step;//这个uv也是在当前帧的归一化平面上的坐标
  Vector2i last_checked_pxi(0,0);
  ++n_steps;
  //沿着极线方向，搜索匹配窗口和匹配特征点
  //step的方向是从B->A
  for(size_t i=0; i<n_steps; ++i, uv+=step)
  {
    //原图上的特征点的坐标
    Vector2d px(cur_frame.cam_->world2cam(uv));
   //在某个金字塔层图像上的坐标
    Vector2i pxi(px[0]/(1<<search_level_)+0.5,
                 px[1]/(1<<search_level_)+0.5);
    if(pxi == last_checked_pxi)//防止重复搜索
      continue;
    last_checked_pxi = pxi;
    // 看特征点和其周围的窗口这些元素是否都在图像的范围内
    if(!cur_frame.cam_->isInFrame(pxi, patch_size_, search_level_))
      continue;
    //找出pxi的像素块(curpatch)
    uint8_t* cur_patch_ptr = cur_frame.img_pyr_[search_level_].data
                             + (pxi[1]-halfpatch_size_)*cur_frame.img_pyr_[search_level_].cols
                             + (pxi[0]-halfpatch_size_);
    //计算当前选中的patch和参考帧中的patch的误差
    int zmssd = patch_score.computeScore(cur_patch_ptr, cur_frame.img_pyr_[search_level_].cols);
    if(zmssd < zmssd_best) {//选一个误差最小的
      zmssd_best = zmssd;
      uv_best = uv;
  }
  //判断是否成功匹配
  if(zmssd_best < PatchScore::threshold())
  {
    if(options_.subpix_refinement)//进行高斯牛顿非线性优化来优化
    {
      px_cur_ = cur_frame.cam_->world2cam(uv_best);
      Vector2d px_scaled(px_cur_/(1<<search_level_));
      bool res;
      if(options_.align_1d)
        res = feature_alignment::align1D(
            cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
            patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
      else
        res = feature_alignment::align2D(
            cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
            options_.align_max_iter, px_scaled);
    //在使用depthFromTriangulation函数求出地图点深度后
    //这深度值可以帮助更新地图点的状态，让地图中好点增加
      if(res)
      {
        px_cur_ = px_scaled*(1<<search_level_);
        if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))//每次找到匹配特征点后都进行三角化，获得参考中特征点深度
          return true;
      }
      return false;
    }
    px_cur_ = cur_frame.cam_->world2cam(uv_best);
    if(depthFromTriangulation(T_cur_ref, ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth))
      return true;
  }
  return false;
}

总结

 （长叹口气）以上就是这篇博客的全部内容。第一编用CSDN，所以写博客的过程还是有点辛苦(代码复制的时候还频繁卡壳，博友们有没有什么好的办法？)。但年轻人相信以后会越写越轻松！感谢聪明、美丽、有耐心的你看完这篇长文。如果文中有不正确的地方请在评论中指出（感谢博友的指点）。那么…就到这吧，祝大家开心每一天！