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. IDEA使用笔记(七)——编辑器最大个数的设置

    我想一定有许多人和我一样在使用IDEA的时候,打开了许多的编辑器选项卡,但是打开的数量是有限的,我们想打开更多的页面,并且希望控制当再次达到大限的时候能关闭那些我们最久没有看的文件! 好吧!看下面的配 ...

  2. 详述socket编程之select()和poll()函数

    转自:http://www.cppblog.com/myjfm/archive/2011/10/26/159093.aspx select()函数和poll()函数均是主要用来处理多路I/O复用的情况 ...

  3. Effective Java 第三版——5. 使用依赖注入取代硬连接资源

    Tips <Effective Java, Third Edition>一书英文版已经出版,这本书的第二版想必很多人都读过,号称Java四大名著之一,不过第二版2009年出版,到现在已经将 ...

  4. Docker 以 docker 方式运行 jenkins

    https://testerhome.com/topics/5798 Docker 以 docker 方式运行 jenkins jmcn · 2016年08月26日 · 最后由 blueshark 回 ...

  5. 初识HTTPS

    初识HTTPS HTTPS也被叫做“基于TLS的HTTP,基于SSL的HTTP,HTTP安全“,它是一个在Internet上广泛使用的计算机网络安全通信协议. 即HTTPS是HTTP的安全版本,在了解 ...

  6. 【Python】Python的安装与个人使用记录

    下载 从官网上下载,目前,最新版是Python3,基于项目需求,我们使用的是Python2. 我是在CentOS上安装,下载的是Python-2.7.9.tgz. 安装 tar -zxvf Pytho ...

  7. 【Java】接口(interface)VS抽象类

    接口(interface)可以说成是抽象类的一种特例,接口中的所有方法都必须是抽象的.接口中的方法定义默认为public abstract类型,接口中的成员变量类型默认为public static f ...

  8. 【九天教您南方cass 9.1】 07 绘制与标注圆曲线和细部点的方法

    同学们大家好,欢迎收看由老王测量上班记出品的cass9.1视频课程 我是本节课主讲老师九天. 我们讲课的教程附件也是共享的,请注意索取测量空间中. [点击索取cass教程]5元立得 (给客服说暗号:“ ...

  9. Java编程的逻辑 (86) - 动态代理

    ​本系列文章经补充和完善,已修订整理成书<Java编程的逻辑>,由机械工业出版社华章分社出版,于2018年1月上市热销,读者好评如潮!各大网店和书店有售,欢迎购买,京东自营链接:http: ...

  10. python进行数据分析

    1.  python进行数据分析----线性回归 2. python进行数据分析------相关分析 3. python进行数据分析---python3卡方 4. 多重响应分析,多选题二分法思路 5. ...