Method optimize

src/PLPSLAM/optimize/graph_optimizer.cc:53–421

        }

        void graph_optimizer::optimize(data::keyframe *loop_keyfrm, data::keyframe *curr_keyfrm,
                                       const module::keyframe_Sim3_pairs_t &non_corrected_Sim3s,
                                       const module::keyframe_Sim3_pairs_t &pre_corrected_Sim3s,
                                       const std::map<data::keyframe *, std::set<data::keyframe *>> &loop_connections) const
        {
            // [1] build optimizer

            auto linear_solver = ::g2o::make_unique<::g2o::LinearSolverCSparse<::g2o::BlockSolver_7_3::PoseMatrixType>>();
            auto block_solver = ::g2o::make_unique<::g2o::BlockSolver_7_3>(std::move(linear_solver));
            auto algorithm = new ::g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));

            ::g2o::SparseOptimizer optimizer;
            optimizer.setAlgorithm(algorithm);

            // [2] add vertex

            const auto all_keyfrms = map_db_->get_all_keyframes();
            const auto all_lms = map_db_->get_all_landmarks();

            // FW:
            const auto all_lms_line = map_db_->get_all_landmarks_line();
            std::unordered_map<unsigned int, data::keyframe *> keyframes;

            const unsigned int max_keyfrm_id = map_db_->get_max_keyframe_id();

            // Convert all keyframe poses (before modification) to Sim3 and save
            std::vector<::g2o::Sim3, Eigen::aligned_allocator<::g2o::Sim3>> Sim3s_cw(max_keyfrm_id + 1);
            // Save the added vertex
            std::vector<g2o::sim3::shot_vertex *> vertices(max_keyfrm_id + 1);

            constexpr int min_weight = 100;

            for (auto keyfrm : all_keyfrms)
            {
                if (keyfrm->will_be_erased())
                {
                    continue;
                }
                auto keyfrm_vtx = new g2o::sim3::shot_vertex();

                const auto id = keyfrm->id_;

                // Check if pose is corrected before optimization
                const auto iter = pre_corrected_Sim3s.find(keyfrm);
                if (iter != pre_corrected_Sim3s.end())
                {
                    // If the pose has been corrected before optimization, take out that pose and set it in vertex
                    Sim3s_cw.at(id) = iter->second;
                    keyfrm_vtx->setEstimate(iter->second);
                }
                else
                {
                    // If the pose is not corrected, convert the pose of keyframe to Sim3 and set it
                    const Mat33_t rot_cw = keyfrm->get_rotation();
                    const Vec3_t trans_cw = keyfrm->get_translation();
                    const ::g2o::Sim3 Sim3_cw(rot_cw, trans_cw, 1.0);

                    Sim3s_cw.at(id) = Sim3_cw;