手撕 视觉slam14讲 ch13 代码（4）VisualOdometry类和工程主函数

1.run_kitti_stereo.cpp

接下来我们开始进入工程主函数run_kitti_stereo.cpp，流程非常简单：

1. 首先实例化了一个VisualOdometry类的类指针vo，传入config的地址
2. 然后vo类指针调用 VisualOdometry 类中的 Init() 函数 ，进行VO的初始化。
3. 在初始化成功后运行VisualOdometry类的Run()函数，开始正常运行VO。

#include <gflags/gflags.h>
#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类

2. VisualOdometry类：

VO 类是一个对外接口，有以下功能：

•  1.读取配置文件
•  2.通过Dataset::Init()函数设置相机的内参数，以及双目相机的外参数
•  3.实例化并初始化frontend_，backend_，map_，viewer_四个对象。并且创建3个线程之间的联系

 visualodometry.h：

//  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_ptr<VisualOdometry>Ptr;  //智能指针

    // 构造函数:   参数是配置文件路径
    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_四个对象，因此我们需要再额外写出前端、后端、显示这三个类的构造函数

前端构造函数：Frontend::Frontend()

// 构造函数 ：根据配置文件（Config类） 的参数来创建GFTT的特征点提取器。
    // num_features_是每帧最多提取的特征点数量，此外还保存一个参数num_features_init_，在后面的地图初始化中会用到
Frontend::Frontend() {

// 以下是 cv::GFTTDetector 的构造函数：
//     static Ptr<GFTTDetector> 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<int>("num_features"),0.01, 20);
        num_features_init_=Config::Get<int>("num_features_init");
        num_features_= Config::Get<int>("num_features");
}

前端的初始化主要就是根据配置文件（Config类） 的参数来创建GFTT的特征点提取器。num_features_是每帧最多提取的特征点数量，此外还保存一个参数num_features_init_，这个参数在后面的地图初始化中会用到。

后端构造函数：Backend::Backend()

// 构造函数 启动优化线程并挂起
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()

Viewer::Viewer(){
    // 上锁,唤醒一个wait的线程
    viewer_thread_= std::thread( std::bind(& Viewer::ThreadLoop,this));
}

Viewer类的初始化里面 开启一个线程做显示，并绑定 Viewer::ThreadLoop() 函数。

另外，Map类的构造函数是空的，没有在构造函数里面设置任何东西。

然后我们来看 visualodometry.h中的函数：

 初始化函数 bool VisualOdometry::Init()：

// 初始化，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<std::string>("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;
}

运行函数：VisualOdometry::Run()：

// 开始运行 ：一直循环step函数
void VisualOdometry::Run(){
    while (1)
    {
        if (Step()==false)
        {
           break;
        }
    }
    // 关闭后端线程
    backend_->Stop();

    // 关闭显示线程
    viewer_->Close();

    LOG(INFO) << "VO exit";
}

在Run()函数里面一直循环 Step() 函数，直到 Step() 函数返回false，就break出去关闭后端、显示线程

Step() 函数：bool VisualOdometry::Step()：

// 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<std::chrono::duration<double>>(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()