SVO系列第二篇，正式开始介绍SVO代码

程序入口

test_pipeline.cpp

主函数

主函数BenchmarkNode的函数。

 svo::BenchmarkNode benchmark;benchmark.runFromFolder();

BenchmarkNode 类

BenchmarkNode 定义了什么？
class BenchmarkNode 中定义了2个类，3个函数。2个类分别是

vk::AbstractCamera* cam_;
svo::FrameHandlerMono* vo_;

vk表示vikit包，是svo编译依赖的一个包，同样出自uzh-rpg，所以svo编译需要下载vikit. vikit的作用：vikit contains camera models, some math and interpolation functions that SVO needs. 所以AbstractCamera这里是一个相机类，类的定义可以查看vikit/abstract_camera.h
FrameHandleMono表示处理帧的类，定义了SVO算法的主体函数，后面会详述。

3个函数分别是：
构造函数，析构函数，读图程序

BenchmarkNode();
~BenchmarkNode();
void runFromFolder();

BenchmarkNode 实现了什么？
构造函数完成了相机初始化定义，svo初始化。

BenchmarkNode::BenchmarkNode()
{cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0);vo_ = new svo::FrameHandlerMono(cam_);vo_->start(); //
}

关注第三步代码，是为了置位，void start() { set_start_ = true; }
true表示开始处理frame，在处理第一帧时会用到这个flag变量。
然后runFromFolder()读图循环，进入SVO算法的的入口。

vo_->addImage(img, 0.01*img_id);

接下来的处理就是在 class FrameHandlerMono中完成了，进入addImage函数，在函数内部会首先调用startFrameProcessingCommon函数，用到上面的flag变量，完成如下操作：

if(set_start_){resetAll();stage_ = STAGE_FIRST_FRAME;}

表示我们可以处理第一帧了，接下来就进入第一帧处理，开始初始化。

初始化

第一帧

拿到第一帧，第一帧位姿R为单位阵，t设为0.然后第一帧加入初始化队列，这里调用了class initialization的addFirstFrame函数，对于第一帧，只需要检测兴趣点，如下

InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
{reset();detectFeatures(frame_ref, px_ref_, f_ref_);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;}frame_ref_ = frame_ref;px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end());return SUCCESS;
}

需要注意的是，第一帧一定是关键帧，所以有如下代码：

new_frame_->setKeyframe();
map_.addKeyframe(new_frame_);
stage_ = STAGE_SECOND_FRAME;
SVO_INFO_STREAM("Init: Selected first frame.");
return RESULT_IS_KEYFRAME;

第二帧

光流跟踪

就是直接光流跟踪算法(开始时写错了，貌似没有构建金字塔的过程)，阈值可以自己调整，初始参数是这样的：

void trackKlt(FramePtr frame_ref,FramePtr frame_cur,vector<cv::Point2f>& px_ref,vector<cv::Point2f>& px_cur,vector<Vector3d>& f_ref,vector<Vector3d>& f_cur,vector<double>& disparities)
{const double klt_win_size = 30.0;const int klt_max_iter = 30;const double klt_eps = 0.001;vector<uchar> status;vector<float> error;vector<float> min_eig_vec;cv::TermCriteria termcrit(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, klt_max_iter, klt_eps);cv::calcOpticalFlowPyrLK(frame_ref->img_pyr_[0], frame_cur->img_pyr_[0],px_ref, px_cur,status, error,cv::Size2i(klt_win_size, klt_win_size),4, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW);......

实现跟踪。这里有几个判断条件：

如果特征数不足，则判定失败，特征数阈值由Config::initMinTracked() 决定；

if(disparities_.size() < Config::initMinTracked())return FAILURE;

如果视差不足，则判定不足以作为关键帧，等待下一帧，然后后面来的帧一直用第一帧的特征点做光流跟踪。

double disparity = vk::getMedian(disparities_);SVO_INFO_STREAM("Init: KLT "<<disparity<<"px average disparity.");if(disparity < Config::initMinDisparity())return NO_KEYFRAME;

个人觉得，因为SVO面向的是无人机(俯视)场景，所以这里求了视差的均值。

位姿恢复

根据光流跟踪并得到当前帧的T，会用到vikit包的相关类。可以看一下computeHomography函数的实现：

void computeHomography(const vector<Vector3d>& f_ref,const vector<Vector3d>& f_cur,double focal_length,double reprojection_threshold,vector<int>& inliers,vector<Vector3d>& xyz_in_cur,SE3& T_cur_from_ref)
{vector<Vector2d > uv_ref(f_ref.size());vector<Vector2d > uv_cur(f_cur.size());for(size_t i=0, i_max=f_ref.size(); i<i_max; ++i){uv_ref[i] = vk::project2d(f_ref[i]);uv_cur[i] = vk::project2d(f_cur[i]);}vk::Homography Homography(uv_ref, uv_cur, focal_length, reprojection_threshold);Homography.computeSE3fromMatches();vector<int> outliers;vk::computeInliers(f_cur, f_ref,Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(),reprojection_threshold, focal_length,xyz_in_cur, inliers, outliers);T_cur_from_ref = Homography.T_c2_from_c1;
}

关于位姿T的计算，在class Homography中有清晰的代码，基本过程就是先求单应矩阵H，对H矩阵奇异值分解得到T和t，这筛选的过程，提出错误解，因为H分解是有4个解，会有2个正解；然后再从这2个解里选择inliers多的，得到最优解，讲道理这里没看懂，为什么不直接用opencv提供的接口。

其他

处理完第二帧之后，有一个局部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;   ~~~

一步步解析SVO代码(二)---初始化相关推荐

一步步读懂Pytorch Chatbot Tutorial代码(二) - 数据处理
文章目录自述代码出处目录代码 Create formatted data file (为了方便理解,把代码的顺序略微改一下, 此章节略长.) 1. `loadLines` 将文件的每一行拆分为 ...
一步步教你为网站开发Android客户端---HttpWatch抓包，HttpClient模拟POST请求，Jsoup解析HTML代码，动态更新ListView...
本文面向Android初级开发者,有一定的Java和Android知识即可. 文章覆盖知识点:HttpWatch抓包,HttpClient模拟POST请求,Jsoup解析HTML代码,动态更新List ...
【SVO代码】（一）从头到尾
阅读SVO代码过程中主要参考的下面两篇博客: svo: semi-direct visual odometry 论文解析 SVO详细解读基于帧间4x4的图像块的灰度不变形来优化相机位姿,这与直接法很 ...
V-SLAM重读(3):SVO代码阅读和调试修改
在本文内容开始之前,看一段调试之后的SVO测试gif(gif图刚开始学会做,大家将就着看一下--) 个人经验(仅供参考): 在阅读一个大型库源代码的时候,遵循以下步骤: (1). 第一步看CMakeL ...
数据结构-二叉树（包含二叉树的层次建树、前中后序遍历、层次遍历解析及代码）
目录一.树与二叉树的原理解析 1.树的定义 2.树的结构和特点 3.二叉树的定义 4.树结点的数据结构二.二叉树的层次建树 1.二叉树层次建树的原理及分析 2.完整代码三.二叉树的前中后序遍历 ...
基于UDS的BootLoader上位机源代码，支持ISO15765通信，支持PeakCAN , ZJG CAN等CAN卡, 支持S-record格式的二进制文件解析；可二次开发或扩展应用
基于UDS的BootLoader上位机源代码(C#) 基于UDS的BootLoader上位机源代码,支持ISO15765通信,支持PeakCAN , ZJG CAN等CAN卡, 支持S-record格 ...
【JVM系列】一步步解析java执行内幕
[JVM系列]一步步解析java执行内幕对于任何一门语言,要想达到精通的水平,研究它的执行原理(或者叫底层机制)不失为一种良好的方式.在本篇文章中,将重点研究java源代码的执行原理,即从程序员编 ...
android 解析、生成二维码
android 解析.生成二维码 (1)ZXing是一个开源Java类库用于解析多种格式的1D/2D条形码.目标是能够对QR编码.Data Matrix.UPC的1D条形码进行解码. 其提供了多种平台 ...
Android解析WindowManagerService（二）WMS的重要成员和Window的添加过程
前言在本系列的上一篇文章中,我们学习了WMS的诞生,WMS被创建后,它的重要的成员有哪些?Window添加过程的WMS部分做了什么呢?这篇文章会给你解答. 1.WMS的重要成员所谓WMS的重要成员 ...

一步步解析SVO代码(二)---初始化

程序入口

主函数

BenchmarkNode 类

初始化

第一帧

第二帧

光流跟踪

位姿恢复

其他

一步步解析SVO代码(二)---初始化相关推荐

最新文章

热门文章