SVO 初始化代码详解

个人认为的SVO 的初始化主要是指 <svo/frame_handler_mono.cpp>中的processFirstFrame()和 processSecondFrame()函数。

这两个函数主要完成了对于前两个关键帧的处理，得到前两个关键帧的单应矩阵H，并从中计算出前两帧的R和t，并对计算出来的t进行一个尺度变换，并建立初始地图。其主要是实现是在<svo/initialization.cpp>

首先先放一张SVO 的示意图，一些基本符号的解释以及相机位姿的获取

一、处理第一帧 FrameHandlerMono::processFirstFrame（）函数

//processFirstFrame()函数 作用是处理第1帧（ 如果关键点数量>100 ）就将其设置为关键帧；
FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
{
   
 // 把第一帧的位姿设置为起点，也是世界坐标系的原点
  new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero()); //  新建一个变换矩阵SE3然后赋给new_frame_的T_f_w_（用于表示从世界坐标到相机坐标的变换矩阵）
 //然后判断函数klt_homography_init_.addFirstFrame(new_frame_)返回值是否为initialization::FAILURE，如果是，则结束processFirstFrame并返回RESULT_NO_KEYFRAME（意思是当前帧非关键帧）。
 //其中klt_homography_init_是KltHomographyInit（FrameHandlerMono的友元类）类型的类，用于计算单应性矩阵（根据前两个关键帧）来初始化位姿。
 //其中addFirstFrame(new_frame_)（定义在initialization.cpp中）
 if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)
    return RESULT_NO_KEYFRAME;
 //如果上一步未执行return，说明当前帧是关键帧，执行函数new_frame_->setKeyframe()将其设置为关键帧（即将is_keyframe_设置为true，并执行setKeyPoints()）
 
 // ***************记录一个问题，第一帧的fts_是在什么时候被赋值的？？？？？？？？****************
 // 这个第一帧的fts_,应该是在processSecondFrame的addSecondFrame里赋值的
  new_frame_->setKeyframe();
  map_.addKeyframe(new_frame_);
  stage_ = STAGE_SECOND_FRAME;
  SVO_INFO_STREAM("Init: Selected first frame.");
  return RESULT_IS_KEYFRAME;
}

其中，

initialization::KltHomographyInit klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography.`

klt_homography_init_.addFirstFrame(new_frame_)函数定义在initialization.cpp中。

enum InitResult {
    FAILURE, NO_KEYFRAME, SUCCESS };
InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
{
   
  // 先执行reset()函数（定义在同文件中）：清空px_cur_（一个二维点向量，存储当前帧中被用于跟踪的关键点坐标）和重置 frame_ref_（一个frame型智能指针）。
  reset();
  // 执行detectFeatures函数，对new_frame进行Fast特征检测（调用FastDetector::detect函数），（？？？检测到的特征点在new_frame_ 的 fts_ 中？？？？）。
  // 并将其关键点坐标集和对应的向量集（关键特征点对应的世界坐标系下的归一化向量）分别赋给px_ref_和f_ref_。
  detectFeatures(frame_ref, px_ref_, f_ref_);
  //  判断特征数是否小于100，如果是，就结束addFirstFrame并返回FAILURE。
  if(px_ref_.size() < 100)
  {
   
    SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment.");
    return FAILURE;
  }
  // 若上一步没执行return，则继续执行程序，将传入的new_frame_赋值给frame_ref_，将px_ref_的值赋给px_cur_，并结束addFirstFrame返回SUCCESS。
  frame_ref_ = frame_ref;
  px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end());  //vector的insert函数，有三个参数，意义是：要插入的位置，要插入的另一个vector的开始值和结束值
  return SUCCESS;
}

在processFirstFrame()函数处理完之后，系统把特征点超过100个的图像作为第一个关键帧，并把得到的特征点存在px_ref中。

二、处理第二帧 FrameHandlerMono::processSecondFrame（）函数

FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
{
   
  //调用klt_homography_init_子函数-addSecondFrame(new_frame_)同样定义在initialization.cpp中，
  //不过这次调用的是addSecondFrame(new_frame_)
  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;

  // two-frame bundle adjustment
  // 条件编译，如果定义了USE_BUNDLE_ADJUSTMENT，就进行BA优化，通过调用ba::twoViewBA函数，这里就不展开了。
#ifdef USE_BUNDLE_ADJUSTMENT
  ba::twoViewBA(new_frame_.get(), map_.lastKeyframe().get(), Config::lobaThresh(), &map_);
#endif

 // 执行函数new_frame_->setKeyframe()将其设置为关键帧（即将is_keyframe_设置为true，并执行setKeyPoints()）。
 // setKeyPoints函数中通过调用checkKeyPoints函数对当前图像中每个特征点进行遍历和比较，最终选出最具有代表性的5个作为关键点。
 // 实质上是1个靠近图像中心的点和4个靠近图像四个角的点。
  new_frame_->setKeyframe();
  
  //通过函数getSceneDepth获取场景平均深度（depth_mean）最小深度（depth_min）。
  double depth_mean, depth_min;
  frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
  // 向深度滤波器depth_filter中添加关键帧（当前帧），传入参数depth_mean、0.5 * depth_min（不知道为啥除以2）进行初始化。
  depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);

  // add frame to map
  // 向map_中添加关键帧
  map_.addKeyframe(new_frame_);
  stage_ = STAGE_DEFAULT_FRAME;
  //调用klt_homography_init_.reset()，初始化px_cur_和frame_ref_。
  klt_homography_init_.reset();
  SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map.");
  return RESULT_IS_KEYFRAME;
}

首先调用的是klt_homography_init_.addSecondFrame(new_frame_)函数。

// addSecondFrame（）函数
// 调用trackKlt对px_ref进行跟踪。跟踪数量小于一定值，返回FAILURE; disparity的均值小于一定值，说明两帧太近，返回NO_KEYFRAME。 
// 调用computeHomography。如果inliers数量小于一定值，返回FAULURE。 以上筛选条件都通过后，用xyz_in_cur来计算在当前帧中的点的平均深度，用于求尺度因子，然后算出当前帧的位姿。
// 对每一个inliers，计算它在世界坐标系下的3D点坐标，并将对应的feature加入到ref_frame和cur_frame中，同时将feature加入point->obs_中， 返回SUCCESS。

InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
{
   
  //首先调用trackKlt函数跟踪特征（LK光流法）
  trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
  SVO_INFO_STREAM("Init: KLT tracked "<< disparities_