diff --git a/src/stella_vslam/module/local_map_updater.cc b/src/stella_vslam/module/local_map_updater.cc index 848d17c2..3e0efc3e 100644 --- a/src/stella_vslam/module/local_map_updater.cc +++ b/src/stella_vslam/module/local_map_updater.cc @@ -27,7 +27,7 @@ bool local_map_updater::acquire_local_map(const std::vector already_found_keyfrm_ids; const auto first_local_keyfrms = find_first_local_keyframes(num_shared_lms_and_keyfrm, already_found_keyfrm_ids); - const auto second_local_keyfrms = find_second_local_keyframes(first_local_keyfrms, already_found_keyfrm_ids); + const auto second_local_keyfrms = find_second_local_keyframes(first_local_keyfrms, already_found_keyfrm_ids, keyframe_id_threshold); local_keyfrms_ = first_local_keyfrms; std::copy(second_local_keyfrms.begin(), second_local_keyfrms.end(), std::back_inserter(local_keyfrms_)); return true; @@ -120,19 +120,23 @@ auto local_map_updater::find_first_local_keyframes( } auto local_map_updater::find_second_local_keyframes(const std::vector>& first_local_keyframes, - std::unordered_set& already_found_keyfrm_ids) const + std::unordered_set& already_found_keyfrm_ids, + unsigned int keyframe_id_threshold) const -> std::vector> { std::vector> second_local_keyfrms; second_local_keyfrms.reserve(4 * first_local_keyframes.size()); // add the second-order keyframes to the local landmarks - auto add_second_local_keyframe = [this, &second_local_keyfrms, &already_found_keyfrm_ids](const std::shared_ptr& keyfrm) { + auto add_second_local_keyframe = [this, &second_local_keyfrms, &already_found_keyfrm_ids, keyframe_id_threshold](const std::shared_ptr& keyfrm) { if (!keyfrm) { return false; } if (keyfrm->will_be_erased()) { return false; } + if (keyframe_id_threshold > 0 && keyfrm->id_ >= keyframe_id_threshold) { + return false; + } // avoid duplication if (already_found_keyfrm_ids.count(keyfrm->id_)) { return false; @@ -151,16 +155,18 @@ auto local_map_updater::find_second_local_keyframes(const std::vectorgraph_node_->get_top_n_covisibilities(10); for (const auto& neighbor : neighbors) { - if (add_second_local_keyframe(neighbor)) { - break; + add_second_local_keyframe(neighbor); + if (max_num_local_keyfrms_ < first_local_keyframes.size() + second_local_keyfrms.size()) { + return second_local_keyfrms; } } // children of the spanning tree const auto spanning_children = keyfrm->graph_node_->get_spanning_children(); for (const auto& child : spanning_children) { - if (add_second_local_keyframe(child)) { - break; + add_second_local_keyframe(child); + if (max_num_local_keyfrms_ < first_local_keyframes.size() + second_local_keyfrms.size()) { + return second_local_keyfrms; } } @@ -173,7 +179,8 @@ auto local_map_updater::find_second_local_keyframes(const std::vector>& frm_lms, - const unsigned int num_keypts) { + const unsigned int num_keypts, + unsigned int keyframe_id_threshold) { local_lms_.clear(); local_lms_.reserve(50 * local_keyfrms_.size()); @@ -205,6 +212,22 @@ bool local_map_updater::find_local_landmarks(const std::vectorid_); + if (keyframe_id_threshold > 0) { + const auto observations = lm->get_observations(); + unsigned int temporal_observations = 0; + for (auto obs : observations) { + auto keyfrm = obs.first.lock(); + if (keyfrm->id_ >= keyframe_id_threshold) { + ++temporal_observations; + } + } + const double temporal_ratio_thr = 0.5; + double temporal_ratio = static_cast(temporal_observations) / observations.size(); + if (temporal_ratio > temporal_ratio_thr) { + continue; + } + } + local_lms_.push_back(lm); } } diff --git a/src/stella_vslam/module/local_map_updater.h b/src/stella_vslam/module/local_map_updater.h index ebe51f35..d730413c 100644 --- a/src/stella_vslam/module/local_map_updater.h +++ b/src/stella_vslam/module/local_map_updater.h @@ -58,12 +58,14 @@ class local_map_updater { //! Find the second-order local keyframes auto find_second_local_keyframes(const std::vector>& first_local_keyframes, - std::unordered_set& already_found_ids) const + std::unordered_set& already_found_ids, + unsigned int keyframe_id_threshold) const -> std::vector>; //! Find the local landmarks bool find_local_landmarks(const std::vector>& frm_lms, - const unsigned int num_keypts); + const unsigned int num_keypts, + unsigned int keyframe_id_threshold); // maximum number of the local keyframes const unsigned int max_num_local_keyfrms_;