pl-svo代码解读

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_[0].at<cv::Vec3b>(point->obs_.front()->px[0], point->obs_.front()->px[1]);
  results_.push_back(ConvergedSeed(
      point->obs_.front()->px[0], point->obs_.front()->px[1], point->pos_, color));
  delete point->obs_.front();
}

void BenchmarkNode::depthFilterCbLs(plsvo::LineSeg* ls)
{
  cv::Vec3b color = ls->obs_.front()->frame->img_pyr_[0].at<cv::Vec3b>(ls->obs_.front()->spx[0], ls->obs_.front()->spx[1]);
  results_.push_back(ConvergedSeed(
      ls->obs_.front()->spx[0], ls->obs_.front()->spx[1], 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 = 0;
    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(), '0') + (*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() < 100)
  //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=0; 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>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 10) && xyz_in_cur_[*it].z() > 0)
    {
      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], 0));
      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], 0));
      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:	 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 提供稀疏图像对齐函数