diff --git a/example/aist/equirectangular.yaml b/example/aist/equirectangular.yaml index 9ed14015f..d4ab93551 100755 --- a/example/aist/equirectangular.yaml +++ b/example/aist/equirectangular.yaml @@ -42,5 +42,11 @@ LoopDetector: reject_by_graph_distance: true min_distance_on_graph: 50 +GraphOptimizer: + min_num_shared_lms: 200 + +GlobalOptimizer: + thr_neighbor_keyframes: 100 + System: map_format: "msgpack" diff --git a/src/stella_vslam/global_optimization_module.cc b/src/stella_vslam/global_optimization_module.cc index 34d1ef792..4829fd768 100644 --- a/src/stella_vslam/global_optimization_module.cc +++ b/src/stella_vslam/global_optimization_module.cc @@ -18,7 +18,8 @@ global_optimization_module::global_optimization_module(data::map_database* map_d : loop_detector_(new module::loop_detector(bow_db, bow_vocab, util::yaml_optional_ref(yaml_node, "LoopDetector"), fix_scale)), loop_bundle_adjuster_(new module::loop_bundle_adjuster(map_db)), map_db_(map_db), - graph_optimizer_(new optimize::graph_optimizer(fix_scale)) { + graph_optimizer_(new optimize::graph_optimizer(util::yaml_optional_ref(yaml_node, "GraphOptimizer"), fix_scale)), + thr_neighbor_keyframes_(util::yaml_optional_ref(yaml_node, "GlobalOptimizer")["thr_neighbor_keyframes"].as(15)) { spdlog::debug("CONSTRUCT: global_optimization_module"); } @@ -235,7 +236,7 @@ void global_optimization_module::correct_loop() { SPDLOG_TRACE("global_optimization_module: compute the Sim3 of the covisibilities of the current keyframe whose Sim3 is already estimated by the loop detector"); // acquire the covisibilities of the current keyframe - std::vector> curr_neighbors = cur_keyfrm_->graph_node_->get_covisibilities(); + std::vector> curr_neighbors = cur_keyfrm_->graph_node_->get_covisibilities_over_min_num_shared_lms(thr_neighbor_keyframes_); curr_neighbors.push_back(cur_keyfrm_); // Sim3 camera poses BEFORE loop correction diff --git a/src/stella_vslam/global_optimization_module.h b/src/stella_vslam/global_optimization_module.h index 715c5a96c..eb4ede254 100644 --- a/src/stella_vslam/global_optimization_module.h +++ b/src/stella_vslam/global_optimization_module.h @@ -262,6 +262,8 @@ class global_optimization_module { //! thread for running loop BA std::unique_ptr thread_for_loop_BA_ = nullptr; + + unsigned int thr_neighbor_keyframes_ = 15; }; } // namespace stella_vslam diff --git a/src/stella_vslam/optimize/graph_optimizer.cc b/src/stella_vslam/optimize/graph_optimizer.cc index 931bf715e..6341bef4b 100644 --- a/src/stella_vslam/optimize/graph_optimizer.cc +++ b/src/stella_vslam/optimize/graph_optimizer.cc @@ -19,8 +19,9 @@ namespace stella_vslam { namespace optimize { -graph_optimizer::graph_optimizer(const bool fix_scale) - : fix_scale_(fix_scale) {} +graph_optimizer::graph_optimizer(const YAML::Node& yaml_node, const bool fix_scale) + : fix_scale_(fix_scale), + min_num_shared_lms_(yaml_node["min_num_shared_lms"].as(100)) {} void graph_optimizer::optimize(const std::shared_ptr& loop_keyfrm, const std::shared_ptr& curr_keyfrm, const module::keyframe_Sim3_pairs_t& non_corrected_Sim3s, @@ -66,8 +67,6 @@ void graph_optimizer::optimize(const std::shared_ptr& loop_keyfr // Save the added vertices std::unordered_map vertices; - constexpr int min_num_shared_lms = 100; - for (auto keyfrm : all_keyfrms) { if (keyfrm->will_be_erased()) { continue; @@ -140,7 +139,7 @@ void graph_optimizer::optimize(const std::shared_ptr& loop_keyfr // Except the current vs loop edges, // Add the loop edges only over the minimum number of shared landmarks threshold if (!(id1 == curr_keyfrm->id_ && id2 == loop_keyfrm->id_) - && keyfrm->graph_node_->get_num_shared_landmarks(connected_keyfrm) < min_num_shared_lms) { + && keyfrm->graph_node_->get_num_shared_landmarks(connected_keyfrm) < min_num_shared_lms_) { continue; } @@ -207,7 +206,7 @@ void graph_optimizer::optimize(const std::shared_ptr& loop_keyfr } // Add the covisibility information over the minimum number of shared landmarks threshold - const auto connected_keyfrms = keyfrm->graph_node_->get_covisibilities_over_min_num_shared_lms(min_num_shared_lms); + const auto connected_keyfrms = keyfrm->graph_node_->get_covisibilities_over_min_num_shared_lms(min_num_shared_lms_); for (auto connected_keyfrm : connected_keyfrms) { // null check if (!connected_keyfrm || !parent_node) { diff --git a/src/stella_vslam/optimize/graph_optimizer.h b/src/stella_vslam/optimize/graph_optimizer.h index c40162f5c..6dd1e5c25 100644 --- a/src/stella_vslam/optimize/graph_optimizer.h +++ b/src/stella_vslam/optimize/graph_optimizer.h @@ -20,9 +20,10 @@ class graph_optimizer { public: /** * Constructor + * @param yaml_node * @param fix_scale */ - explicit graph_optimizer(const bool fix_scale); + explicit graph_optimizer(const YAML::Node& yaml_node, const bool fix_scale); /** * Destructor @@ -46,6 +47,8 @@ class graph_optimizer { private: //! SE3 optimization or Sim3 optimization const bool fix_scale_; + + unsigned int min_num_shared_lms_ = 100; }; } // namespace optimize