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:\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 提供稀疏图像对齐函数