接下来我们开始进入工程主函数run_kitti_stereo.cpp,流程非常简单:
#include
#include"MYSLAM/visual_odometry.h"
//DEFINE_string参数1是一个变量;参数2是用户指定,给到参数1的;参数3,类似一个说明为这个变量的提示信息
//在程序中使用DEFINE_XXX函数定义的变量时,需要在每个变量前加上FLAGS_前缀。
DEFINE_string (config_file, "./config/default.yaml" ,"config file path");
int main (int argc , char **argv){
google::ParseCommandLineFlags(&argc, &argv, true);
MYSLAM::VisualOdometry::Ptr vo (new MYSLAM::VisualOdometry(FLAGS_config_file));
// 初始化
vo->Init();
// 开始运行
vo->Run();
return 0;
}
所以在写主函数之前,我们需要先实现VisualOdometry类
VO 类是一个对外接口,有以下功能:
// VO 对外接口
// 1.读取配置文件
// 2.通过Dataset::Init()函数设置相机的内参数,以及双目相机的外参数
// 3.实例化并初始化frontend_,backend_,map_,viewer_四个对象。并且创建3个线程之间的联系
#pragma once
#ifndef MYSLAM_VISUAL_ODOMETRY_H
#define MYSLAM_VISUAL_ODOMETRY_H
#include"MYSLAM/frontend.h"
#include"MYSLAM/backend.h"
#include"MYSLAM/viewer.h"
#include"MYSLAM/common_include.h"
#include"MYSLAM/dataset.h"
namespace MYSLAM{
//VO类
class VisualOdometry{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; //内存对齐
typedef std::shared_ptrPtr; //智能指针
// 构造函数: 参数是配置文件路径
VisualOdometry(std::string &config_path);
// 初始化,return true if success
bool Init();
// 开始运行 start vo in the dataset
void Run();
// 从dataset开始逐帧读取、运行
bool Step();
// 获取前端状态
FrontendStatus GetFrontendStatus() const {return frontend_->GetStatus();}
private:
bool inited_=false;// 是否初始化
std::string config_file_path_;// 参数文件路径
//实例化并初始化frontend_,backend_,map_,viewer_四个对象
Frontend::Ptr frontend_=nullptr;
Backend::Ptr backend_=nullptr;
Map::Ptr map_=nullptr;
Viewer::Ptr viewer_=nullptr;
//实例化并初始化dataset对象
Dataset::Ptr dataset_=nullptr;
};
}//namespace MYSLAM
#endif // MYSLAM_VISUAL_ODOMETRY_H
因为需要实例化并初始化frontend_,backend_,map_,viewer_四个对象,因此我们需要再额外写出前端、后端、显示这三个类的构造函数
// 构造函数 :根据配置文件(Config类) 的参数来创建GFTT的特征点提取器。
// num_features_是每帧最多提取的特征点数量,此外还保存一个参数num_features_init_,在后面的地图初始化中会用到
Frontend::Frontend() {
// 以下是 cv::GFTTDetector 的构造函数:
// static Ptr create( int maxCorners=1000, double qualityLevel=0.01, double minDistance=1,
// int blockSize=3, bool useHarrisDetector=false, double k=0.04 );
// 其中,maxCorners 决定提取关键点最大个数; qualityLevel 表示关键点强度阈值,只保留大于该阈值的关键点(阈值 = 最强关键点强度 * qualityLevel);
// minDistance 决定关键点间的最小距离;blockSize 决定自相关函数累加区域,也决定了 Sobel 梯度时窗口尺寸;
// useHarrisDetector 决定使用 Harris 判定依据还是 Shi-Tomasi 判定依据;k 仅在使用 Harris 判定依据时有效;
gftt_=
cv::GFTTDetector::create(Config::Get("num_features"),0.01, 20);
num_features_init_=Config::Get("num_features_init");
num_features_= Config::Get("num_features");
}
前端的初始化主要就是根据配置文件(Config类) 的参数来创建GFTT的特征点提取器。num_features_是每帧最多提取的特征点数量,此外还保存一个参数num_features_init_,这个参数在后面的地图初始化中会用到。
// 构造函数 启动优化线程并挂起
Backend::Backend(){
backend_running_.store(true);
backend_thread_ = std::thread(std::bind(&Backend::BackendLoop, this));//上锁,唤醒一个wait的线程
}
在后端初始化中,主要是新开一个线程backend_thread_,然后把这个线程中运行的函数设置为Backend::BackendLoopI()函数。线程运行状态 backend_running_ 设置为true
Viewer::Viewer(){
// 上锁,唤醒一个wait的线程
viewer_thread_= std::thread( std::bind(& Viewer::ThreadLoop,this));
}
Viewer类的初始化里面 开启一个线程做显示,并绑定 Viewer::ThreadLoop() 函数。
另外,Map类的构造函数是空的,没有在构造函数里面设置任何东西。
然后我们来看 visualodometry.h中的函数:
// 初始化,return true if success
bool VisualOdometry::Init(){
// 1. 调用Config::SetParameterFile()函数读取配置文件,如果找不到对应文件就返回false。
if ( Config::SetParameterFile(config_file_path_)==false)
{
return false;
}
// 2. 通过Dataset::Init()函数设置相机的内参数,以及双目相机的外参数。
dataset_=Dataset::Ptr(new Dataset(Config::Get("dataset_dir")));
CHECK_EQ(dataset_->Init(), true);
// 3. 实例化并初始化frontend_,backend_,map_,viewer_四个对象。并且创建3个线程之间的联系,
frontend_=Frontend::Ptr(new Frontend);
backend_=Backend::Ptr(new Backend);
map_=Map::Ptr(new Map);
viewer_=Viewer::Ptr(new Viewer);
frontend_->SetBackend(backend_);
frontend_->SetMap(map_);
frontend_->SetViewer(viewer_);
frontend_->SetCameras(dataset_->GetCamera(0),dataset_->GetCamera(1));
backend_->SetMap(map_);
backend_->SetCameras(dataset_->GetCamera(0), dataset_->GetCamera(1));
viewer_->SetMap(map_);
return true;
}
// 开始运行 :一直循环step函数
void VisualOdometry::Run(){
while (1)
{
if (Step()==false)
{
break;
}
}
// 关闭后端线程
backend_->Stop();
// 关闭显示线程
viewer_->Close();
LOG(INFO) << "VO exit";
}
在Run()函数里面一直循环 Step() 函数,直到 Step() 函数返回false,就break出去关闭后端、显示线程
// Step() 函数: 其实也一直在循环两个函数 Dataset::NextFrame() 和 Frontend::AddFrame() 。
bool VisualOdometry::Step(){
// 从数据集里面读取一下帧图像,然后调用 Frame::CreateFrame() 函数为每一帧赋一个id号
Frame::Ptr new_frame = dataset_->NextFrame();
if (new_frame == nullptr) return false;
// 调用AddFrame()函数:去根据前端状态变量FrontendStatus,运行不同的三个函数,StereoInit(),Track()和Reset()
auto t1 = std::chrono::steady_clock::now(); //计时 时间戳t1
bool success = frontend_->AddFrame(new_frame);
auto t2 = std::chrono::steady_clock::now(); //计时 时间戳t2
auto time_used = std::chrono::duration_cast>(t2 - t1); //计算用时 (t2-t1)
LOG(INFO) << "VO cost time: " << time_used.count() << " seconds.";
return success;
}
之后就会进入前端主函数 Frontend::AddFrame(),开始SLAM的过程...
Frontend::AddFrame()函数:
// 外部接口,添加一个帧并计算其定位结果
bool Frontend::AddFrame(Frame::Ptr frame ){//用frame接收传入的new_frame
current_frame_ = frame;//令当前帧等于上一行的frame
// 判断跟踪状态
switch(status_){
// 如果状态为初始化,进入双目初始化
case FrontendStatus::INITING:
StereoInit();
break;
// 跟踪状态好 或者 跟踪状态差,进入跟踪
case FrontendStatus::TRACKING_GOOD:
case FrontendStatus::TRACKING_BAD:
Track();
break;
// 如果跟踪丢失, 重定位
case FrontendStatus::LOST:
Reset();
break;
}
//对当前帧操作完毕,更新帧:令当前帧变为上一帧
last_frame_ =current_frame_;
return true;
}
根据前端状态变量FrontendStatus,运行不同的三个函数,StereoInit(),Track()和Reset()