SVO 学习笔记(三)

这篇博客

 这篇博客将介绍SVO源代码中的frame_handler_mono、initialization两个文件的代码流程。前者是SVO系统类，展示了整个系统的执行过程；后者则是用于系统初始化的文件。首先介绍initialization部分代码。

Initialization

 初始化的主要任务是使用光流法完成前几帧的跟踪工作,设置第一、第二关键帧。addFirstFrame函数处理获得的第一帧，并提取其中特征点。如果特征点数量足够，就将这一帧看作第一关键帧。
  addSecondFrame函数用于判断能否构建第二个关键帧。它的步骤为：
 1、获得当前帧，对当前帧使用光流法：
 trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
 完成和参考帧的匹配（因为这时还没有地图点，所以那些图像对齐之类的方法还不能用）。其中disparities_保存的是各匹配点所对应的视差。
 2、double disparity = vk::getMedian(disparities_);
  if(disparity < Config::initMinDisparity())
  return NO_KEYFRAME;判断视差大小是否满足要求（视差足够大时才会把当前帧设为第二关键帧）。
 3、若当前帧被选为第二关键帧，则用computeHomography计算两关键帧的单应矩阵，根据内点数量判断是否跟踪成功。然后构建内点对应的3D地图点。
 4、跟踪成功后，计算地图点在当前帧中的深度中值。用这个中值表示场景的尺度（因为是单目，所以需要计算尺度）。然后设置当前帧的位姿，要考虑尺度的影响：
 frame_cur->T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_;
 旋转不用考虑地图尺度问题，平移需要考虑：
 frame_cur->T_f_w_.translation() =-frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));
 5、最后将当前帧看到的地图点转换到世界坐标系中，作为第一代地图点，并将前两个关键帧与这些地图点关联起来。

Frame_Handler_Mono

 主要介绍这个文件中的processFirstFrame、processSecondFrame、 processFrame和relocalizeFrame这四个函数。它们代表了这样的流程：
(获取第一关键帧) > （获取第二关键帧，完成地图初始化） > (处理之后关键帧，完成跟踪任务) > （跟踪丢失后进行重定位）
 这个流程在该文件中的addImage()函数（处理每次获得的图像）中进行。下面介绍这四个函数代码。

processFirstFrame

 这个函数用来处理最开始的图像，直到获得第一个关键帧为止。

//处理获得的第一个帧，并判断该帧能否成为关键帧
//这个会一直使用到第一个关键帧构建了为止（即函数中klt...中addFirstFrame成功）
//返回结果有三种：当前帧是KF，不是KF，结果无效
FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
{
    //设置当前帧的初始位姿为单位变换矩阵
    new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero());
   //将当前帧添加到初始化类中，看其是否能够作为第一关键帧
  if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)
    return RESULT_NO_KEYFRAME;
  new_frame_->setKeyframe();//将当前帧设置为关键帧
  map_.addKeyframe(new_frame_);
  //更新系统的状态，以便下次使用其他帧处理函数
  stage_ = STAGE_SECOND_FRAME;
  SVO_INFO_STREAM("Init: Selected first frame.");
  return RESULT_IS_KEYFRAME;
}

processSecondFrame

 这个函数在第一关键帧出现后开始使用，一直用到出现第二关键帧为止

//返回结果有三种：结果是KF，不是KF，结果无效
FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
{
//klt...是用于系统初始化的类对象
  initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_);
  if(res == initialization::FAILURE)//使用两帧初始化失败
    return RESULT_FAILURE;
  else if(res == initialization::NO_KEYFRAME)//第二帧不能作为关键帧
    return RESULT_NO_KEYFRAME;
  //条件编译：如果需要使用BA，就用BA优化头两个关键帧的位姿
  #ifdef USE_BUNDLE_ADJUSTMENT
  ba::twoViewBA(new_frame_.get(), map_.lastKeyframe().get(), Config::lobaThresh(), &map_);
  #endif
  new_frame_->setKeyframe();
  double depth_mean, depth_min;
  //此时第一代地图点以由前两个关键帧构建了
  //获得所有地图点在当前帧中的平均、最小深度值
  frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
  //将当前这个新的关键帧添加到深度滤波器中
  depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
  // add frame to map
  map_.addKeyframe(new_frame_);
  //更新系统状态
  stage_ = STAGE_DEFAULT_FRAME;
  klt_homography_init_.reset();
  SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map.");
  return RESULT_IS_KEYFRAME;
}

processFrame

 在成功添加了两个关键帧之后，使用该函数处理后续的帧，也就是论文中描述的那几个部分。它的大致步骤为：
 1、先用上一帧的位姿作为当前帧的初始位姿：
 new_frame_->T_f_w_ = last_frame_->T_f_w_;
 2、进行稀疏图像对齐：
 size_t img_align_n_tracked = img_align.run(last_frame_, new_frame_);。
 3、将地图点重投影到当前帧中，获得更加精确的特征匹配（关键帧和当前帧的特征匹配）。
 reprojector_.reprojectMap(new_frame_, overlap_kfs_);
 如果匹配特征的数量较少，那么就将当前帧的位姿设置为上一帧的位姿，并设置跟踪失败，直接结束函数。
（PS：上面这两步的代码解释可参考笔记（二））
 4、如果第3步成了，就对当前帧的估计位姿进行优化，通过最小化重投影误差实现。先是只优化位姿，也就是所谓的“motion-only BA“。

 pose_optimizer::optimizeGaussNewton(
      Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
      new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);

 5、之后就是结构优化，即优化地图点的坐标（structure-only BA），也是根据最小化重投影误差实现。这部分只优化地图点。

optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());

(PS：对位姿和地图点分别执行优化，能很大地减少优化时间。)
 6、完成位姿和结构性优化后，开始选择关键帧。先进行一个跟踪效果判断：

core_kfs_.insert(new_frame_);
  setTrackingQuality(sfba_n_edges_final);//跟踪效果判断
  if(tracking_quality_ == TRACKING_INSUFFICIENT)
  {//跟踪得不好，就返回“跟踪失败”
  //个人认为这样设置是为了方便在重定位的时候找参考关键帧，因为之后newframe的成员会给到lastframe中
  //然后使用与lastframe距离最近的一部分关键帧来实现重定位
    new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps
    return RESULT_FAILURE;
  }

 跟踪效果不错时，对当前帧进行关键帧的判断：

//关键帧的判断
  double depth_mean, depth_min;
  //获得当前帧中所有地图点深度的平均和最小值
  frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
  //根据两个条件判断当前帧能否做关键帧：
  //1、跟踪的质量好
  //2、当前帧距离其他共视帧的距离远
  //第二条由neddNewKF决定。这个函数由将共视关键帧的位置坐标投影到当前帧坐标系，并通过距离判断是否加入需要关键帧
  if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)
  {
    //虽然不能做关键帧，但还是能用于更新深度滤波器中点的深度
    depth_filter_->addFrame(new_frame_);
    return RESULT_NO_KEYFRAME;
  }

 7、如果当前帧被设置为关键帧，则增加相应地图点的观测次数和观测对象。

 for(Features::iterator it=new_frame_->fts_.begin(); it!=new_frame_->fts_.end(); ++it)
    if((*it)->point != NULL)
      (*it)->point->addFrameRef(*it);
  map_.point_candidates_.addCandidatePointToFrame(new_frame_);

 8、最后将当前帧作为关键帧添加到深度滤波器中。如果地图和深度滤波器中的关键帧数量过多，则剔除掉离新关键帧最远的关键帧（这一步之前有个条件编译：如果定义了USE_BUNDLE_ADJUSTMENT，则使用BA优化当前帧位姿）：

// init new depth-filters.添加新的关键帧能帮助滤波器产生新的地图点，以及优化已有地图点的深度信息
 depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
 // if limited number of keyframes, remove the one furthest apart
  if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs())
  {
  //获得离新关键帧最远的关键帧，并将其从深度滤波器和地图中剔除
    FramePtr furthest_frame = map_.getFurthestKeyframe(new_frame_->pos());
    depth_filter_->removeKeyframe(furthest_frame); 
    map_.safeDeleteFrame(furthest_frame);
  }
  map_.addKeyframe(new_frame_);

 至此整个函数结束，当前帧处理完毕，继续等待下一个图像的到来。

relocalizeFrame

//以关键帧作为参考，完成重定位操作
//使用的是与lastframe距离最近的关键帧最为参考关键帧
FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame(
    const SE3& T_cur_ref,
    FramePtr ref_keyframe)
{
  SVO_WARN_STREAM_THROTTLE(1.0, "Relocalizing frame");
  if(ref_keyframe == nullptr)
  {
    SVO_INFO_STREAM("No reference keyframe.");
    return RESULT_FAILURE;
  }
  SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
                           30, SparseImgAlign::GaussNewton, false, false);
  size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_);
  //若匹配上的特征个数超过30则认为重定位成功
   if(img_align_n_tracked > 30)
  {
    SE3 T_f_w_last = last_frame_->T_f_w_;
    //用参考关键帧作为上一帧，以便进行processFrame()
    last_frame_ = ref_keyframe;
    //用关键帧做参考，进行定位操作
    FrameHandlerMono::UpdateResult res = processFrame();
    if(res != RESULT_FAILURE)
    {
      stage_ = STAGE_DEFAULT_FRAME;
      SVO_INFO_STREAM("Relocalization successful.");
    }
    else//重定位失败，用上一帧位姿作为当前帧位姿，以便继续重定位
      new_frame_->T_f_w_ = T_f_w_last; // reset to last well localized pose
     //（其实之后很大概率还是使用这个参考关键帧作为重定位的参考）
    return res;
  }
  return RESULT_FAILURE;
}

结尾

 总算是将SVO的代码笔记延续到了第三期，可喜可贺X3。感谢屏幕前的你耐心地看完，也感谢自己坚持了下来。如果文中有什么不足，劳烦各位博友指出，十分感谢。