Use partial_sort

stella-cv · May 1, 2024 · e6d2669 · e6d2669
1 parent f34e502
commit e6d2669
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/src/stella_vslam/module/local_map_updater.cc b/src/stella_vslam/module/local_map_updater.cc
@@ -75,8 +75,11 @@ 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.
+    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>());
 
     return num_shared_lms_and_keyfrm;
 }