Add parameters about covisibilities

stella-cv · Feb 24, 2024 · 37b8556 · 37b8556
1 parent 07b4e82
commit 37b8556
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 9 deletions.
diff --git 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
@@ -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<unsigned int>(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<std::shared_ptr<data::keyframe>> curr_neighbors = cur_keyfrm_->graph_node_->get_covisibilities();
+    std::vector<std::shared_ptr<data::keyframe>> 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/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<unsigned int>(100)) {}
 
 void graph_optimizer::optimize(const std::shared_ptr<data::keyframe>& loop_keyfrm, const std::shared_ptr<data::keyframe>& curr_keyfrm,
                                const module::keyframe_Sim3_pairs_t& non_corrected_Sim3s,
@@ -66,8 +67,6 @@ void graph_optimizer::optimize(const std::shared_ptr<data::keyframe>& loop_keyfr
     // Save the added vertices
     std::unordered_map<unsigned int, internal::sim3::shot_vertex*> 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<data::keyframe>& 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<data::keyframe>& 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
@@ -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