Method correct_loop

src/PLPSLAM/global_optimization_module.cc:191–296

    }

    void global_optimization_module::correct_loop()
    {
        auto final_candidate_keyfrm = loop_detector_->get_selected_candidate_keyframe();

        spdlog::info("detect loop: keyframe {} - keyframe {}", final_candidate_keyfrm->id_, cur_keyfrm_->id_);
        loop_bundle_adjuster_->count_loop_BA_execution();

        // [1] pre-processing

        // stop the mapping module and the previous loop bundle adjuster
        // pause the mapping module
        mapper_->request_pause();

        // abort the previous loop bundle adjuster
        if (thread_for_loop_BA_ || loop_bundle_adjuster_->is_running())
        {
            abort_loop_BA();
        }

        // wait till the mapping module pauses
        while (!mapper_->is_paused())
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(2));
        }

        // update the graph, because we got new matched landmarks by loop detection -> loop_detector::curr_match_lms_observed_in_cand_
        cur_keyfrm_->graph_node_->update_connections();

        // [2] compute the Sim3 of the covisibilities of the current keyframe whose Sim3 is already estimated by the loop detector
        //    then, the covisibilities are moved to the corrected positions
        //    finally, landmarks observed in them are also moved to the correct position using the camera poses before and after camera pose correction

        // acquire the covisibilities of the current keyframe
        std::vector<data::keyframe *> curr_neighbors = cur_keyfrm_->graph_node_->get_covisibilities();
        curr_neighbors.push_back(cur_keyfrm_);

        // Sim3 camera poses BEFORE loop correction
        module::keyframe_Sim3_pairs_t Sim3s_nw_before_correction;
        // Sim3 camera poses AFTER loop correction
        module::keyframe_Sim3_pairs_t Sim3s_nw_after_correction;

        const auto g2o_Sim3_cw_after_correction = loop_detector_->get_Sim3_world_to_current();
        {
            std::lock_guard<std::mutex> lock(data::map_database::mtx_database_);

            // camera pose of the current keyframe BEFORE loop correction
            const Mat44_t cam_pose_wc_before_correction = cur_keyfrm_->get_cam_pose_inv();

            // compute Sim3s BEFORE loop correction
            Sim3s_nw_before_correction = get_Sim3s_before_loop_correction(curr_neighbors);
            // compute Sim3s AFTER loop correction
            Sim3s_nw_after_correction = get_Sim3s_after_loop_correction(cam_pose_wc_before_correction, g2o_Sim3_cw_after_correction, curr_neighbors);

            // correct covisibility landmark positions
            correct_covisibility_landmarks(Sim3s_nw_before_correction, Sim3s_nw_after_correction);
            // correct covisibility keyframe camera poses
            correct_covisibility_keyframes(Sim3s_nw_after_correction);