From e6d2669331ece7254f86f99966affbc41e706c78 Mon Sep 17 00:00:00 2001
From: ymd-stella <world.applepie@gmail.com>
Date: Wed, 1 May 2024 12:19:26 +0900
Subject: [PATCH] Use partial_sort

---
 src/stella_vslam/module/local_map_updater.cc | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/stella_vslam/module/local_map_updater.cc b/src/stella_vslam/module/local_map_updater.cc
index ede01c20..86a641bb 100644
--- 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;
 }