slam14讲之设计slam系统（第二版ch13）代码注释二（后端及VO接口）

这一部分相对于前端而言，代码量更小，可是逻辑更加复杂，设计到很多c++的新特性，是编程技巧很强的一部分.

一、后端 backend.cpp

//
// Created by gaoxiang on 19-5-2.
//

#include "myslam/backend.h"
#include "myslam/algorithm.h"
#include "myslam/feature.h"
#include "myslam/g2o_types.h"
#include "myslam/map.h"
#include "myslam/mappoint.h"

namespace myslam {

Backend::Backend() {
    /*
    原子操作的数据类型（atomic_bool,atomic_int,atomic_long等等），
    对于这些原子数据类型的共享资源的访问，无需借助mutex等锁机制，
    也能够实现对共享资源的正确访问。
    */
   // 构造函数中启动优化线程并挂起:通过原子操作实现
    backend_running_.store(true);
    /*
    如果回调函数是一个类的成员函数。
    这时想把成员函数设置给一个回调函数指针往往是不行的
    因为类的成员函数，多了一个隐含的参数this。 
    所以直接赋值给函数指针肯定会引起编译报错。这时候就要用到bind
    bind函数的用法和详细参考：
    https://www.cnblogs.com/jialin0x7c9/p/12219239.html
    thread用法参考：
    https://blog.csdn.net/weixin_44156680/article/details/116260129
    以下代码即是实现了创建一个回调函数为BackendLoop（）的线程，并传入类参数
    */
    backend_thread_ = std::thread(std::bind(&Backend::BackendLoop, this));
}

void Backend::UpdateMap() {
    //unique_lock用法参考：
    //https://blog.csdn.net/weixin_44156680/article/details/116260129
    std::unique_lock<std::mutex> lock(data_mutex_);
    map_update_.notify_one();
}

void Backend::Stop() {
    backend_running_.store(false);
    map_update_.notify_one();
    //等待该线程终止，例如，在子线程调用了join（time）方法后，
    //主线程只有等待子线程time时间后才能执行子线程后面的代码。
    backend_thread_.join();
}

void Backend::BackendLoop() {
    while (backend_running_.load()) {
        std::unique_lock<std::mutex> lock(data_mutex_);
        /*
        std::condition_variable实际上是一个类，是一个和条件相关的类，说白了就是等待一个条件达成。
        wait()用来等一个东西：
        一、有除互斥量以外的第二个参数时：
        如果第二个参数的lambda表达式返回值是false，那么wait()将解锁互斥量，并阻塞到本行
        如果第二个参数的lambda表达式返回值是true，那么wait()直接返回并继续执行。（此时对互斥量上锁！）
        阻塞到其他某个线程调用notify_one()成员函数为止；

        二、无除互斥量以外的第二个参数时：
        如果没有第二个参数，那么效果跟第二个参数lambda表达式返回false效果一样
        wait()将解锁互斥量，并阻塞到本行，阻塞到其他某个线程调用notify_one()成员函数为止。

        重点：显然，阻塞在这里的原因是没有获得互斥量的访问权

        三、当其他线程用notify_one()将本线程wait()唤醒后，这个wait恢复后
        1、wait()不断尝试获取互斥量锁，如果获取不到那么流程就卡在wait()这里等待获取，如果获取到了，那么wait()就继续执行，获取到了锁
        2、如果wait有第二个参数就判断这个lambda表达式。
            a)如果表达式为false，那wait又对互斥量解锁，然后又休眠，等待再次被notify_one()唤醒
            b)如果lambda表达式为true，则wait返回，流程可以继续执行（此时互斥量已被锁住）。
        3、如果wait没有第二个参数，则wait返回，流程走下去（直接锁住互斥量）。

        注意：流程只要走到了wait()下面则互斥量一定被锁住了。

        下面一句实现的功能是：执行到wait语句时直接解锁并堵塞，直到其他线程调用notify_one（），直接锁住互斥量并往下执行
        */
        map_update_.wait(lock);

        /// 后端仅优化激活的Frames和Landmarks
        Map::KeyframesType active_kfs = map_->GetActiveKeyFrames();
        Map::LandmarksType active_landmarks = map_->GetActiveMapPoints();
        //开始做全局优化
        Optimize(active_kfs, active_landmarks);
    }
}

void Backend::Optimize(Map::KeyframesType &keyframes,
                       Map::LandmarksType &landmarks) {
    // setup g2o
    typedef g2o::BlockSolver_6_3 BlockSolverType;
    typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType>
        LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(
            g2o::make_unique<LinearSolverType>()));
    //创建稀疏优化器
    g2o::SparseOptimizer optimizer;
    //打开调试输出
    optimizer.setAlgorithm(solver);

    // pose 顶点，使用Keyframe id
    std::map<unsigned long, VertexPose *> vertices;
    unsigned long max_kf_id = 0;
    //遍历关键帧   确定第一个顶点
    for (auto &keyframe : keyframes) {
        auto kf = keyframe.second;
        VertexPose *vertex_pose = new VertexPose();  // camera vertex_pose
        vertex_pose->setId(kf->keyframe_id_);
        vertex_pose->setEstimate(kf->Pose());
        optimizer.addVertex(vertex_pose);
        //这个判断是不是多此一举呢？为什么要做这样一个判断呢
        //难道keyframe_id_这个参数有可能重复？不可能啊！！！！
        if (kf->keyframe_id_ > max_kf_id) {
            max_kf_id = kf->keyframe_id_;
        }
        //插入自定义map类型的vertices 不要make_pair也可以嘛
        vertices.insert({kf->keyframe_id_, vertex_pose});
    }

    // 路标顶点，使用路标id索引
    std::map<unsigned long, VertexXYZ *> vertices_landmarks;

    // K 和左右外参
    Mat33 K = cam_left_->K();
    SE3 left_ext = cam_left_->pose();
    SE3 right_ext = cam_right_->pose();

    // edges
    int index = 1;
    double chi2_th = 5.991;  // robust kernel 阈值
    std::map<EdgeProjection *, Feature::Ptr> edges_and_features;
    //遍历所有活动路标点
    for (auto &landmark : landmarks) {
        //外点不优化
        if (landmark.second->is_outlier_) continue;
        //路标点id
        unsigned long landmark_id = landmark.second->id_;
        //得到所有观测到这个路标点的feature 
        auto observations = landmark.second->GetObs();
        //遍历所有观测到这个路标点的feature，得到第二个顶点，形成对应的点边关系
        for (auto &obs : observations) {
            if (obs.lock() == nullptr) continue;
            auto feat = obs.lock();
            if (feat->is_outlier_ || feat->frame_.lock() == nullptr) continue;
            //得到该feature所在的frame
            auto frame = feat->frame_.lock();
            EdgeProjection *edge = nullptr;
            //判断这个feature在哪个相机
            if (feat->is_on_left_image_) {
                edge = new EdgeProjection(K, left_ext);
            } else {
                edge = new EdgeProjection(K, right_ext);
            }

            // 如果landmark还没有被加入优化，则新加一个顶点
            //意思是无论mappoint被观测到几次，只与其中一个形成关系
            if (vertices_landmarks.find(landmark_id) ==
                vertices_landmarks.end()) {
                VertexXYZ *v = new VertexXYZ;
                v->setEstimate(landmark.second->Pos());
                v->setId(landmark_id + max_kf_id + 1);
                //边缘化
                v->setMarginalized(true);
                vertices_landmarks.insert({landmark_id, v});
                //增加point顶点
                optimizer.addVertex(v);
            }

            edge->setId(index);
            edge->setVertex(0, vertices.at(frame->keyframe_id_));    // pose
            edge->setVertex(1, vertices_landmarks.at(landmark_id));  // landmark
            edge->setMeasurement(toVec2(feat->position_.pt));
            //e转置*信息矩阵*e,所以由此可以看出误差向量为n×1,则信息矩阵为n×n
            edge->setInformation(Mat22::Identity());
            //定义robust kernel函数
            auto rk = new g2o::RobustKernelHuber();
            //设置阈值
            rk->setDelta(chi2_th);
            //设置核函数
            //设置鲁棒核函数，之所以要设置鲁棒核函数是为了平衡误差，不让二范数的误差增加的过快。
            // 鲁棒核函数里要自己设置delta值，
            // 这个delta值是，当误差的绝对值小于等于它的时候，误差函数不变。否则误差函数根据相应的鲁棒核函数发生变化。
            edge->setRobustKernel(rk);
            edges_and_features.insert({edge, feat});
            //增加边
            optimizer.addEdge(edge);

            index++;
        }
    }

    // do optimization and eliminate the outliers
    optimizer.initializeOptimization();
    optimizer.optimize(10);

    int cnt_outlier = 0, cnt_inlier = 0;
    int iteration = 0;
    //确保内点占1/2以上，否则调整阈值，直到迭代结束
    while (iteration < 5) {
        cnt_outlier = 0;
        cnt_inlier = 0;
        // determine if we want to adjust the outlier threshold
        for (auto &ef : edges_and_features) {
            if (ef.first->chi2() > chi2_th) {
                cnt_outlier++;
            } else {
                cnt_inlier++;
            }
        }
        double inlier_ratio = cnt_inlier / double(cnt_inlier + cnt_outlier);
        if (inlier_ratio > 0.5) {
            break;
        } else {
            chi2_th *= 2;
            iteration++;
        }
    }
    //根据新的阈值，调整哪些是外点 ，并移除
    for (auto &ef : edges_and_features) {
        if (ef.first->chi2() > chi2_th) {
            ef.second->is_outlier_ = true;
            // remove the observation
            ef.second->map_point_.lock()->RemoveObservation(ef.second);
        } else {
            ef.second->is_outlier_ = false;
        }
    }

    LOG(INFO) << "Outlier/Inlier in optimization: " << cnt_outlier << "/"
              << cnt_inlier;

    // Set pose and lanrmark position
    //保存优化后的pose和point
    for (auto &v : vertices) {
        keyframes.at(v.first)->SetPose(v.second->estimate());
    }
    for (auto &v : vertices_landmarks) {
        landmarks.at(v.first)->SetPos(v.second->estimate());
    }
}

}  // namespace myslam

二、VO接口 visual_odometry.cpp

//
// Created by gaoxiang on 19-5-4.
//
#include "myslam/visual_odometry.h"
#include <chrono>
#include "myslam/config.h"

namespace myslam {

//定义只需要指明文件路径
VisualOdometry::VisualOdometry(std::string &config_path)
    : config_file_path_(config_path) {}

bool VisualOdometry::Init() {
    // read from config file
    //判断一下这个config_file_path_是否存在，
    //同时将配置文件赋给Config类中的cv::FileStorage file_,便于对文件操作
    if (Config::SetParameterFile(config_file_path_) == false) {
        return false;
    }
    //1.模板函数自动类型推导调用Config::Get("dataset_dir")；
    //2.模板函数具体类型显示调用Config::Get<std::string>("dataset_dir")；
    dataset_ =
        Dataset::Ptr(new Dataset(Config::Get<std::string>("dataset_dir")));
    CHECK_EQ(dataset_->Init(), true);

    // create components and links
    //接下来按照逻辑关系一层层的确立联系，
    //一个完整的VO包含前端,后端,地图,可视化器等模块，因此有下述创建代码
    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;
}

void VisualOdometry::Run() {
    while (1) {
        LOG(INFO) << "VO is running";
         //这里的主过程执行在这条if语句中,
         //每次做条件判断都需要执行Step()，即步进操作，如果步进出问题，则跳出死循环while (1)
        if (Step() == false) {
            break;
        }
    }

    backend_->Stop();
    viewer_->Close();

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

bool VisualOdometry::Step() {
    //从数据集中读出下一帧
    Frame::Ptr new_frame = dataset_->NextFrame();
    //如果读到的下一帧为空，也就是说没有读到下一帧，则无法继续跟踪，报错
    if (new_frame == nullptr) return false;
    //计时
    auto t1 = std::chrono::steady_clock::now();
    //将新的一帧加入到前端中，进行跟踪处理,帧间位姿估计
    bool success = frontend_->AddFrame(new_frame);
    auto t2 = std::chrono::steady_clock::now();
    auto time_used =
        std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    LOG(INFO) << "VO cost time: " << time_used.count() << " seconds.";
    return success;
}

}  // namespace myslam