Add parameter use_additional_keyfrms_for_monocular

stella-cv · Apr 24, 2022 · 87def6e · 87def6e
1 parent 59538c0
commit 87def6e
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 12 deletions.
diff --git a/src/stella_vslam/mapping_module.cc b/src/stella_vslam/mapping_module.cc
@@ -19,7 +19,7 @@ namespace stella_vslam {
 mapping_module::mapping_module(const YAML::Node& yaml_node, data::map_database* map_db, data::bow_database* bow_db, data::bow_vocabulary* bow_vocab)
     : local_map_cleaner_(new module::local_map_cleaner(yaml_node, map_db, bow_db)),
       map_db_(map_db), bow_db_(bow_db), bow_vocab_(bow_vocab),
-      local_bundle_adjuster_(new optimize::local_bundle_adjuster()) {
+      local_bundle_adjuster_(new optimize::local_bundle_adjuster(yaml_node)) {
     spdlog::debug("CONSTRUCT: mapping_module");
     spdlog::debug("load mapping parameters");
 

diff --git a/src/stella_vslam/optimize/local_bundle_adjuster.cc b/src/stella_vslam/optimize/local_bundle_adjuster.cc
@@ -26,9 +26,11 @@
 namespace stella_vslam {
 namespace optimize {
 
-local_bundle_adjuster::local_bundle_adjuster(const unsigned int num_first_iter,
+local_bundle_adjuster::local_bundle_adjuster(const YAML::Node& yaml_node,
+                                             const unsigned int num_first_iter,
                                              const unsigned int num_second_iter)
-    : num_first_iter_(num_first_iter), num_second_iter_(num_second_iter) {}
+    : num_first_iter_(num_first_iter), num_second_iter_(num_second_iter),
+      use_additional_keyframes_for_monocular_(yaml_node["use_additional_keyframes_for_monocular"].as<bool>(false)) {}
 
 void local_bundle_adjuster::optimize(data::map_database* map_db,
                                      const std::shared_ptr<stella_vslam::data::keyframe>& curr_keyfrm, bool* const force_stop_flag) const {
@@ -126,14 +128,17 @@ void local_bundle_adjuster::optimize(data::map_database* map_db,
         }
     }
 
-    auto additional_keyfrms_size = 2 - fixed_keyfrms.size();
-    if (!has_scale && fixed_keyfrms.size() < 2 && local_keyfrms.size() > additional_keyfrms_size) {
-        for (unsigned int i = 0; i < additional_keyfrms_size; ++i) {
-            auto itr = local_keyfrms.begin();
-            auto keyfrm_id = itr->first;
-            auto keyfrm = itr->second;
-            local_keyfrms.erase(keyfrm_id);
-            fixed_keyfrms[keyfrm_id] = keyfrm;
+    if (use_additional_keyframes_for_monocular_) {
+        // Ensure that there are always at least two fixed keyframes
+        auto additional_keyfrms_size = 2 - fixed_keyfrms.size();
+        if (!has_scale && fixed_keyfrms.size() < 2 && local_keyfrms.size() > additional_keyfrms_size) {
+            for (unsigned int i = 0; i < additional_keyfrms_size; ++i) {
+                auto itr = local_keyfrms.begin();
+                auto keyfrm_id = itr->first;
+                auto keyfrm = itr->second;
+                local_keyfrms.erase(keyfrm_id);
+                fixed_keyfrms[keyfrm_id] = keyfrm;
+            }
         }
     }
 

diff --git a/src/stella_vslam/optimize/local_bundle_adjuster.h b/src/stella_vslam/optimize/local_bundle_adjuster.h
@@ -16,10 +16,12 @@ class local_bundle_adjuster {
 public:
     /**
      * Constructor
+     * @param yaml_node
      * @param num_first_iter
      * @param num_second_iter
      */
-    explicit local_bundle_adjuster(const unsigned int num_first_iter = 5,
+    explicit local_bundle_adjuster(const YAML::Node& yaml_node,
+                                   const unsigned int num_first_iter = 5,
                                    const unsigned int num_second_iter = 10);
 
     /**
@@ -40,6 +42,8 @@ class local_bundle_adjuster {
     const unsigned int num_first_iter_;
     //! number of iterations of second optimization
     const unsigned int num_second_iter_;
+    //!
+    const unsigned int use_additional_keyframes_for_monocular_ = false;
 };
 
 } // namespace optimize