slam系列之openvslam解读5

接上一篇：https://blog.csdn.net/xiechaoyi123/article/details/104640021

以单目实现为例，走一遍openvslam的流程：

1）slam system初始化，

包括传入的控制参数（orb参数，相机类型，图片mask等）：

cfg_(cfg), camera_(cfg->camera_)

传入的BOW词袋数据库：bow_vocab：

    bow_vocab_ = new data::bow_vocabulary();     
    try {        
         bow_vocab_->loadFromBinaryFile(vocab_file_path);     
    }

三个模块对象申明定义，基本数据对象申明定义：

    // database
    cam_db_ = new data::camera_database(camera_);
    map_db_ = new data::map_database();
    bow_db_ = new data::bow_database(bow_vocab_);

    // frame and map publisher
    frame_publisher_ = std::shared_ptr<publish::frame_publisher>(new publish::frame_publisher(cfg_, map_db_));
    map_publisher_ = std::shared_ptr<publish::map_publisher>(new publish::map_publisher(cfg_, map_db_));

    // tracking module
    tracker_ = new tracking_module(cfg_, this, map_db_, bow_vocab_, bow_db_);
    // mapping module
    mapper_ = new mapping_module(map_db_, camera_->setup_type_ == camera::setup_type_t::Monocular);
    // global optimization module
    global_optimizer_ = new global_optimization_module(map_db_, bow_db_, bow_vocab_, camera_->setup_type_ != camera::setup_type_t::Monocular);

    // connect modules each other
    tracker_->set_mapping_module(mapper_);
    tracker_->set_global_optimization_module(global_optimizer_);
    mapper_->set_tracking_module(tracker_);
    mapper_->set_global_optimization_module(global_optimizer_);
    global_optimizer_->set_tracking_module(tracker_);
    global_optimizer_->set_mapping_module(mapper_);

2）传入数据流：传入单张图片（or视频帧）
进入跟踪模块，返回当前帧的位姿，对应的三维点信息以及跟踪的状态信息

更新显示信息

feed_monocular_frame(const cv::Mat& img, const double timestamp, const cv::Mat& mask){
    assert(camera_->setup_type_ == camera::setup_type_t::Monocular);

    check_reset_request();

    const Mat44_t cam_pose_cw = tracker_->track_monocular_image(img, timestamp, mask);

    frame_publisher_->update(tracker_);
    if (tracker_->tracking_state_ == tracker_state_t::Tracking) {
        map_publisher_->set_current_cam_pose(cam_pose_cw);
    }

    return cam_pose_cw;
}

3）跟踪模块分解：

3.1 传入的图片转为灰度图

3.2 构造frame对象

3.3 跟踪

3.4 返回当前帧位姿

    const auto start = std::chrono::system_clock::now();

    // color conversion
    img_gray_ = img;
    util::convert_to_grayscale(img_gray_, camera_->color_order_);

    // create current frame object
    if (tracking_state_ == tracker_state_t::NotInitialized || tracking_state_ == tracker_state_t::Initializing) {
        curr_frm_ = data::frame(img_gray_, timestamp, ini_extractor_left_, bow_vocab_, camera_, cfg_->true_depth_thr_, mask);
    }
    else {
        curr_frm_ = data::frame(img_gray_, timestamp, extractor_left_, bow_vocab_, camera_, cfg_->true_depth_thr_, mask);
    }

    track();

    const auto end = std::chrono::system_clock::now();
    elapsed_ms_ = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();

    return curr_frm_.cam_pose_cw_;

4）跟踪继续分解：

4.1 确认跟踪状态

    若为没有初始化状态: 更新为已经初始化

4.2 初始化状态：初始化，更新局部信息，将所有关键帧及三维点放入全局信息map_database中，跟踪状态置为跟踪状态：

   

       if (!initialize()) {
            return;
        }

        // update the reference keyframe, local keyframes, and local landmarks
        update_local_map();

        // pass all of the keyframes to the mapping module
        const auto keyfrms = map_db_->get_all_keyframes();
        for (const auto keyfrm : keyfrms) {
            mapper_->queue_keyframe(keyfrm);
        }

        // state transition to Tracking mode
        tracking_state_ = tracker_state_t::Tracking;

    4.3 跟踪状态：

（1）更新上一帧的landmark信息和位姿信息（mapping module可能会更改这些信息）

（2）跟踪当前帧（主要是通过三种匹配方式（motion，BOW，brute)去计算得到匹配关系）

（3）更新局部信息（local landmark,keyframe)以及motion model

（4）更新统计信息（frame_statistics)

（5）若是当前帧是需要的关键帧，插入关键帧

    // apply replace of landmarks observed in the last frame
        apply_landmark_replace();
        // update the camera pose of the last frame
        // because the mapping module might optimize the camera pose of the last frame's reference keyframe
        update_last_frame();

        // set the reference keyframe of the current frame
        curr_frm_.ref_keyfrm_ = ref_keyfrm_;

        auto succeeded = track_current_frame();

        // update the local map and optimize the camera pose of the current frame
        if (succeeded) {
            update_local_map();
            succeeded = optimize_current_frame_with_local_map();
        }

        // update the motion model
        if (succeeded) {
            update_motion_model();
        }

        // state transition
        tracking_state_ = succeeded ? tracker_state_t::Tracking : tracker_state_t::Lost;

        // update the frame statistics
        map_db_->update_frame_statistics(curr_frm_, tracking_state_ == tracker_state_t::Lost);

        // if tracking is failed soon after initialization, reset the databases
        if (tracking_state_ == tracker_state_t::Lost && curr_frm_.id_ < camera_->fps_) {
            spdlog::info("tracking lost soon after initialization");
            system_->request_reset();
            return;
        }

        // show message if tracking has been lost
        if (last_tracking_state_ != tracker_state_t::Lost && tracking_state_ == tracker_state_t::Lost) {
            spdlog::info("tracking lost");
        }

        // check to insert the new keyframe derived from the current frame
        if (succeeded && new_keyframe_is_needed()) {
            insert_new_keyframe();
        }

        // tidy up observations
        for (unsigned int idx = 0; idx < curr_frm_.num_keypts_; ++idx) {
            if (curr_frm_.landmarks_.at(idx) && curr_frm_.outlier_flags_.at(idx)) {
                curr_frm_.landmarks_.at(idx) = nullptr;
            }
        }