Method optimize

src/PLPSLAM/module/loop_bundle_adjuster.cc:68–266

        }

        void loop_bundle_adjuster::optimize(const unsigned int identifier)
        {
            spdlog::info("start loop bundle adjustment");

            unsigned int num_exec_loop_BA = 0;
            {
                std::lock_guard<std::mutex> lock(mtx_thread_);
                loop_BA_is_running_ = true;
                abort_loop_BA_ = false;
                num_exec_loop_BA = num_exec_loop_BA_;
            }

            // FW: global BA with point and line
            const auto global_bundle_adjuster = optimize::global_bundle_adjuster(map_db_, num_iter_, false);
            global_bundle_adjuster.optimize(identifier, &abort_loop_BA_);

            {
                std::lock_guard<std::mutex> lock1(mtx_thread_);

                // if count_loop_BA_execution() was called during the loop BA or the loop BA was aborted,
                // cannot update the map
                if (num_exec_loop_BA != num_exec_loop_BA_ || abort_loop_BA_)
                {
                    spdlog::info("abort loop bundle adjustment");
                    loop_BA_is_running_ = false;
                    abort_loop_BA_ = false;
                    return;
                }

                spdlog::info("finish loop bundle adjustment");
                spdlog::info("updating the map with pose propagation");

                // stop mapping module
                mapper_->request_pause();
                while (!mapper_->is_paused() && !mapper_->is_terminated())
                {
                    std::this_thread::sleep_for(std::chrono::microseconds(1000));
                }

                std::lock_guard<std::mutex> lock2(data::map_database::mtx_database_);

                // update the camera pose along the spanning tree from the origin
                std::list<data::keyframe *> keyfrms_to_check;
                keyfrms_to_check.push_back(map_db_->origin_keyfrm_);
                while (!keyfrms_to_check.empty())
                {
                    auto parent = keyfrms_to_check.front();
                    const Mat44_t cam_pose_wp = parent->get_cam_pose_inv();

                    const auto children = parent->graph_node_->get_spanning_children();
                    for (auto child : children)
                    {
                        if (child->loop_BA_identifier_ != identifier)
                        {
                            // if `child` is NOT optimized by the loop BA
                            // propagate the pose correction from the spanning parent

                            // parent->child