diff --git a/src/stella_vslam/mapping_module.cc b/src/stella_vslam/mapping_module.cc index 276e96734..643b1bb44 100644 --- 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 index 706553cda..bbbf23222 100644 --- 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(false)) {} void local_bundle_adjuster::optimize(data::map_database* map_db, const std::shared_ptr& 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 index 5c5c5fca5..d387e1d5b 100644 --- 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