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