Fix local map size and improve performance (#587)

* Fix local map size * Use partial_sort
stella-cv · May 1, 2024 · 5dd8c16 · 5dd8c16
1 parent 25120c3
commit 5dd8c16
Showing 1 changed file with 15 additions and 5 deletions.
diff --git a/src/stella_vslam/module/local_map_updater.cc b/src/stella_vslam/module/local_map_updater.cc
@@ -75,8 +75,18 @@ auto local_map_updater::count_num_shared_lms(
     for (auto& it : keyfrm_to_num_shared_lms) {
         num_shared_lms_and_keyfrm.emplace_back(it.second, it.first);
     }
-    std::sort(num_shared_lms_and_keyfrm.begin(), num_shared_lms_and_keyfrm.end(),
-              greater_number_and_id_object_pairs<unsigned int, data::keyframe>());
+    constexpr int margin = 5; // Keep a little more than max_num_local_keyfrms_, as keyframes may be deleted.
+    if (num_shared_lms_and_keyfrm.size() > max_num_local_keyfrms_ + margin) {
+        std::partial_sort(num_shared_lms_and_keyfrm.begin(),
+                          num_shared_lms_and_keyfrm.begin() + max_num_local_keyfrms_ + margin,
+                          num_shared_lms_and_keyfrm.end(),
+                          greater_number_and_id_object_pairs<unsigned int, data::keyframe>());
+    }
+    else {
+        std::sort(num_shared_lms_and_keyfrm.begin(),
+                  num_shared_lms_and_keyfrm.end(),
+                  greater_number_and_id_object_pairs<unsigned int, data::keyframe>());
+    }
 
     return num_shared_lms_and_keyfrm;
 }
@@ -143,7 +153,7 @@ auto local_map_updater::find_second_local_keyframes(const std::vector<std::share
         return true;
     };
     for (auto iter = first_local_keyframes.cbegin(); iter != first_local_keyframes.cend(); ++iter) {
-        if (max_num_local_keyfrms_ < first_local_keyframes.size() + second_local_keyfrms.size()) {
+        if (max_num_local_keyfrms_ <= first_local_keyframes.size() + second_local_keyfrms.size()) {
             break;
         }
 
@@ -153,7 +163,7 @@ auto local_map_updater::find_second_local_keyframes(const std::vector<std::share
         const auto neighbors = keyfrm->graph_node_->get_top_n_covisibilities(10);
         for (const auto& neighbor : neighbors) {
             add_second_local_keyframe(neighbor);
-            if (max_num_local_keyfrms_ < first_local_keyframes.size() + second_local_keyfrms.size()) {
+            if (max_num_local_keyfrms_ <= first_local_keyframes.size() + second_local_keyfrms.size()) {
                 return second_local_keyfrms;
             }
         }
@@ -162,7 +172,7 @@ auto local_map_updater::find_second_local_keyframes(const std::vector<std::share
         const auto spanning_children = keyfrm->graph_node_->get_spanning_children();
         for (const auto& child : spanning_children) {
             add_second_local_keyframe(child);
-            if (max_num_local_keyfrms_ < first_local_keyframes.size() + second_local_keyfrms.size()) {
+            if (max_num_local_keyfrms_ <= first_local_keyframes.size() + second_local_keyfrms.size()) {
                 return second_local_keyfrms;
             }
         }