From f3b741f36bd271b660149db0dbc7340979ee78d4 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 | 14 ++++++++++++--
 1 file changed, 12 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..eede847e 100644
--- 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;
 }