pl-svo是在svo的基础上结合点和线特征的半直接法视觉里程计

程序启动通过app文件夹下的run_pipeline.cpp主程序启动,其它的函数文件统一放在src文件夹下,我们先从run_pipeline.cpp进行分析,了解整个算法流程。

首先定义了 svo_options的数据结构,里面包含的是程序的运行参数

struct svo_options {
int seq_offset;
int seq_step;
int seq_length;
bool has_points;
bool has_lines ;
bool is_tum;
string dataset_dir;
string images_dir;
string traj_out;
string map_out;
};

接下来在plsvo命名空间下声明了一个 ConvergedSeed 结构类型和BenchmarkNode类

struct ConvergedSeed {
int x_, y_;
Vector3d pos_;
cv::Vec3b col_;
ConvergedSeed(int x, int y, Vector3d pos, cv::Vec3b col) :
x_(x), y_(y), pos_(pos), col_(col)
{}
};

我们下面将详细介绍BenchmarkNode类,他包含了主程序的主要功能函数

4个私有成员

  vk::AbstractCamera* cam_;
FrameHandlerMono* vo_;
DepthFilter* depth_filter_;
std::list<ConvergedSeed> results_;

公开的成员函数

public:
  BenchmarkNode(vk::AbstractCamera *cam_);
  BenchmarkNode(vk::AbstractCamera *cam_, const plsvo::FrameHandlerMono::Options& handler_opts);
  ~BenchmarkNode();
  void depthFilterCbPt(plsvo::Point* point);
  void depthFilterCbLs(plsvo::LineSeg* ls);
  int runFromFolder(svo_options opts);
  int runFromFolder(vk::PinholeCamera* cam_, svo_options opts);
  int runFromFolder(vk::ATANCamera* cam_,    svo_options opts);
};
负责启动视觉里程计的同名构造函数

BenchmarkNode::BenchmarkNode(vk::AbstractCamera* cam_)
{
vo_ = new plsvo::FrameHandlerMono(cam_);
vo_->start();
} BenchmarkNode::BenchmarkNode(
vk::AbstractCamera* cam_,
const plsvo::FrameHandlerMono::Options& handler_opts)
{
vo_ = new plsvo::FrameHandlerMono(cam_, handler_opts);
vo_->start();
}

点和线特征的深度滤波器函数

void BenchmarkNode::depthFilterCbPt(plsvo::Point* point)
{
cv::Vec3b color = point->obs_.front()->frame->img_pyr_[].at<cv::Vec3b>(point->obs_.front()->px[], point->obs_.front()->px[]);
results_.push_back(ConvergedSeed(
point->obs_.front()->px[], point->obs_.front()->px[], point->pos_, color));
delete point->obs_.front();
} void BenchmarkNode::depthFilterCbLs(plsvo::LineSeg* ls)
{
cv::Vec3b color = ls->obs_.front()->frame->img_pyr_[].at<cv::Vec3b>(ls->obs_.front()->spx[], ls->obs_.front()->spx[]);
results_.push_back(ConvergedSeed(
ls->obs_.front()->spx[], ls->obs_.front()->spx[], ls->spos_, color)); // test only with spoint
delete ls->obs_.front();
}

int BenchmarkNode::runFromFolder是算法的主流程,读取数据文件并运行。

int BenchmarkNode::runFromFolder(vk::ATANCamera* cam_, svo_options opts)

获取img目录中的已排序文件列表

YAML::Node dset_config = YAML::LoadFile(dataset_dir+"/dataset_params.yaml");

获取img目录中的所有文件

    size_t max_len = ;
std::list<std::string> imgs;
boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
for (boost::filesystem::directory_iterator file(img_dir_path); file != end_itr; ++file)
{
boost::filesystem::path filename_path = file->path().filename();
if (boost::filesystem::is_regular_file(file->status()) &&
(filename_path.extension() == ".png" ||
filename_path.extension() == ".jpg" ||
filename_path.extension() == ".jpeg" ||
filename_path.extension() == ".tiff") )
{
std::string filename(filename_path.string());
imgs.push_back(filename);
max_len = max(max_len, filename.length());
}
}

按文件名排序; 如果需要,添加前导零以使文件名长度相等

    for (std::list<std::string>::iterator img = imgs.begin(); img != imgs.end(); ++img)
{
sorted_imgs[std::string(max_len - img->length(), '') + (*img)] = *img;
n_imgs++;
}

根据初始偏移,步长和数据长度把图像数据存储到

std::map<std::string, std::string> sorted_imgs;

场景初始化

sceneRepresentation scene("../app/scene_config.ini");

运行SVO进行姿态估计

进入循环读入图像进行处理

cv::Mat img(cv::imread(img_path.string(), CV_8UC1));

图像去畸变

cam_->undistortImage(img,img_rec);

开始对图像进行处理

vo_->addImage(img_rec, frame_counter / (double)fps_);
void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp)

Frame类初始化为new_frame_,创建图像金字塔(也作为img_pyr_存储在Frame中)

new_frame_.reset(new Frame(cam_, img.clone(), timestamp));

处理图像帧

  UpdateResult res = RESULT_FAILURE;
if(stage_ == STAGE_DEFAULT_FRAME)
res = processFrame();
else if(stage_ == STAGE_SECOND_FRAME)
res = processSecondFrame();
else if(stage_ == STAGE_FIRST_FRAME)
res = processFirstFrame();
else if(stage_ == STAGE_RELOCALIZING)
res = relocalizeFrame(SE3(Matrix3d::Identity(), Vector3d::Zero()),
map_.getClosestKeyframe(last_frame_));

先处理第一帧,设置第一帧以进行身份转换,第一帧视为关键帧,进行特征提取并添加关键帧

FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
{
// set first frame to identity transformation
new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero());
// for now the initialization is done with points and endpoints only (consider use lines)
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;
}
InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
{
reset();
detectFeatures(frame_ref, px_ref_, f_ref_);
if(px_ref_.size() < )
//if(px_ref_.size() < 80)
{
SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 80 features. Retry in more textured environment.");
return FAILURE;
}
frame_ref_ = frame_ref;
// initialize points in current frame (query or second frame) with points in ref frame
px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end());
return SUCCESS;
}

循环过来,对第二帧进行跟踪

FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
{
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
#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;
}

对第二帧进行klt光流跟踪

InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
{
trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
SVO_INFO_STREAM("Init: KLT tracked "<< disparities_.size() <<" features"); // check the number of points tracked is high enough
if(disparities_.size() < Config::initMinTracked())
return FAILURE; // check the median disparity is high enough to compute homography robustly
double disparity = vk::getMedian(disparities_);
SVO_INFO_STREAM("Init: KLT "<<disparity<<"px median disparity.");
if(disparity < Config::initMinDisparity())
return NO_KEYFRAME; computeHomography(
f_ref_, f_cur_,
frame_ref_->cam_->errorMultiplier2(), Config::poseOptimThresh(),
inliers_, xyz_in_cur_, T_cur_from_ref_);
SVO_INFO_STREAM("Init: Homography RANSAC "<<inliers_.size()<<" inliers."); if(inliers_.size() < Config::initMinInliers())
{
SVO_WARN_STREAM("Init WARNING: "<<Config::initMinInliers()<<" inliers minimum required.");
return FAILURE;
} // Rescale the map such that the mean scene depth is equal to the specified scale
vector<double> depth_vec;
for(size_t i=; i<xyz_in_cur_.size(); ++i)
depth_vec.push_back((xyz_in_cur_[i]).z());
double scene_depth_median = vk::getMedian(depth_vec);
double scale = Config::mapScale()/scene_depth_median;
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())); // For each inlier create 3D point and add feature in both frames
SE3 T_world_cur = frame_cur->T_f_w_.inverse();
for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it)
{
Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y);
Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y);
// add 3D point (in (w)olrd coordinates) and features if
// BOTH ref and cur points lie within the image (with a margin)
// AND the 3D point lies in front of the camera
if(frame_ref_->cam_->isInFrame(px_cur.cast<int>(), ) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), ) && xyz_in_cur_[*it].z() > )
{
Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale);
Point* new_point = new Point(pos); PointFeat* ftr_cur(new PointFeat(frame_cur.get(), new_point, px_cur, f_cur_[*it], ));
frame_cur->addFeature(ftr_cur);
new_point->addFrameRef(ftr_cur); PointFeat* ftr_ref(new PointFeat(frame_ref_.get(), new_point, px_ref, f_ref_[*it], ));
frame_ref_->addFeature(ftr_ref);
new_point->addFrameRef(ftr_ref);
}
}
return SUCCESS;
}

从第三帧开始进入正常处理

FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
{
  // Set initial pose TODO use prior
  new_frame_->T_f_w_ = last_frame_->T_f_w_;   // sparse image align
  SVO_START_TIMER("sparse_img_align");
  bool display = false;
  bool verbose = false;
  SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
                           30, SparseImgAlign::GaussNewton, display, verbose);
  size_t img_align_n_tracked = img_align.run(last_frame_, new_frame_);
  SVO_STOP_TIMER("sparse_img_align");
  SVO_LOG(img_align_n_tracked);
  SVO_DEBUG_STREAM("Img Align:\t Tracked = " << img_align_n_tracked);   // show reference features
  cv::cvtColor(last_frame_->img(), FrameHandlerMono::debug_img, cv::COLOR_GRAY2BGR);
  {
    // draw point features
    {
      auto fts = last_frame_->pt_fts_;
      Patch patch( 4, debug_img );
      for(auto it=fts.begin(); it!=fts.end(); ++it)
      {
        patch.setPosition((*it)->px);
        patch.setRoi();
        cv::rectangle(debug_img,patch.rect,cv::Scalar(0,255,0));
      }
    }
    // draw segment features
    {
      auto fts = last_frame_->seg_fts_;
      std::for_each(fts.begin(), fts.end(), [&](plsvo::LineFeat* i){
          if( i->feat3D != NULL )
            cv::line(debug_img,cv::Point2f(i->spx[0],i->spx[1]),cv::Point2f(i->epx[0],i->epx[1]),cv::Scalar(0,255,0));
      });
    }
    //cv::imshow("cv: Ref image", debug_img);
    //cv::waitKey(30);
  }

稀疏图像对齐

sparse_img_align.cpp/h 提供稀疏图像对齐函数

pl-svo代码解读的更多相关文章

  1. SVO详细解读

    SVO详细解读 极品巧克力 前言 接上一篇文章<深度滤波器详细解读>. SVO(Semi-Direct Monocular Visual Odometry)是苏黎世大学Scaramuzza ...

  2. Android MVP模式 谷歌官方代码解读

    Google官方MVP Sample代码解读 关于Android程序的构架, 当前(2016.10)最流行的模式即为MVP模式, Google官方提供了Sample代码来展示这种模式的用法. Repo ...

  3. 优秀开源代码解读之JS与iOS Native Code互调的优雅实现方案

    简介 本篇为大家介绍一个优秀的开源小项目:WebViewJavascriptBridge. 它优雅地实现了在使用UIWebView时JS与ios 的ObjC nativecode之间的互调,支持消息发 ...

  4. SoftmaxLayer and SoftmaxwithLossLayer 代码解读

    SoftmaxLayer and SoftmaxwithLossLayer 代码解读 Wang Xiao 先来看看 SoftmaxWithLoss 在prototext文件中的定义: layer { ...

  5. 同样的一句SQL语句在pl/sql 代码块中count 没有数据,但是直接用SQl 执行却可以count 得到结果

    pl/sql 代码块: SELECT count(distinct t2.so_nbr) INTO v_count2 FROM KFGL_YW_STEP_qd t2 WHERE t2.partitio ...

  6. 将PL/SQL代码封装在机灵的包中

    将代码封装在机灵的包中 http://www.oracle.com/technetwork/issue-archive/2013/13-jan/o13plsql-1872456.html 绝大多数基于 ...

  7. Hybrid----优秀开源代码解读之JS与iOS Native Code互调的优雅实现方案-备

    本篇为大家介绍一个优秀的开源小项目:WebViewJavascriptBridge. 它优雅地实现了在使用UIWebView时JS与ios 的ObjC nativecode之间的互调,支持消息发送.接 ...

  8. Jsoup代码解读之六-防御XSS攻击

    Jsoup代码解读之八-防御XSS攻击 防御XSS攻击的一般原理 cleaner是Jsoup的重要功能之一,我们常用它来进行富文本输入中的XSS防御. 我们知道,XSS攻击的一般方式是,通过在页面输入 ...

  9. Jsoup代码解读之五-实现一个CSS Selector

    Jsoup代码解读之七-实现一个CSS Selector 当当当!终于来到了Jsoup的特色:CSS Selector部分.selector也是我写的爬虫框架webmagic开发的一个重点.附上一张s ...

  10. Jsoup代码解读之四-parser

    Jsoup代码解读之四-parser 作为Java世界最好的HTML 解析库,Jsoup的parser实现非常具有代表性.这部分也是Jsoup最复杂的部分,需要一些数据结构.状态机乃至编译器的知识.好 ...

随机推荐

  1. SpringCloud Stream生产者配置RabbitMq的动态路由键

    在写这个文章前不得不吐槽目前国内一些blog的文章,尽是些复制粘贴的文章,提到点上但没任何的深入和例子.......... 经过测试下来总结一下RabbitMQ的Exchange的特性: 1.dire ...

  2. nodeJS服务器的创建和重新启动

    一: 首先在nodejs项目里创建一个server.js文件,输入下面代码 var http = require("http"); http.createServer(functi ...

  3. 适合初学者的python实际例子

    最近在github上发现了一个有意思的项目,很适合初学者学习python代码. 学习一门语言刚开始的时候是很枯燥的,各种概念语法以及无聊的打印都会让人失去更进一步学习的动力. 很多同学在学习了一段时间 ...

  4. MAC EI Capitan上更新系统自带SVN版本号(关闭SIP方能sudo rm)

    继昨晚之后.决定更新系统自带的svn.自带的svn版本号是1.7.看官网svn:http://www.wandisco.com/subversion/download#osx 最新版本号是1.9.13 ...

  5. python gb2312 转换为 utf-8

    url = "" headers = { "Host":"www.chinanews.com", "User-Agent" ...

  6. openwrt 水星mw4530r-v1 搞搞搞

    感觉周围最实用的搞硬件非路由器莫属,可惜配置都不咋的高,选择水星这款就是看中它的性价比和openwrt的支持,真乃刷机神器啊,还可以挂载usb就更绝了,价格没得说. 另外128mb的rom对大部分功能 ...

  7. 透彻理解Spring事务设计思想之手写实现

    前言 事务,是描述一组操作的抽象,比如对数据库的一组操作,要么全部成功,要么全部失败.事务具有4个特性:Atomicity(原子性),Consistency(一致性),Isolation(隔离性),D ...

  8. rtrim() 函数 从字符串的末端开始删除空白字符!

    例子 在本例中,我们将使用 rtrim() 函数从字符串右端删除字符: <?php $str = "Hello World!\n\n"; echo $str; echo rt ...

  9. CAS (15) — CAS 线上环境 Ehcache Replication 的非稳定重现错误 java.util.ConcurrentModificationException

    CAS (15) - CAS 线上环境 Ehcache Replication 的非稳定重现错误 摘要 线上环境在 EhCache Replication 过程中出现 java.util.Concur ...

  10. Go指南练习_错误

    源地址 https://tour.go-zh.org/methods/20 一.题目描述 从之前的练习中复制 Sqrt 函数,修改它使其返回 error 值. Sqrt 接受到一个负数时,应当返回一个 ...