这篇博客

这篇博客将介绍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_ADJUSTMENTba::twoViewBA(new_frame_.get(), map_.lastKeyframe().get(), Config::lobaThresh(), &map_);#endifnew_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 mapmap_.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 jumpsreturn 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 apartif(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。感谢屏幕前的你耐心地看完，也感谢自己坚持了下来。如果文中有什么不足，劳烦各位博友指出，十分感谢。