Fix max_num_local_keyfrms (#515)

src/stella_vslam/data/graph_node.cc

@@ -2,14 +2,6 @@
 #include "stella_vslam/data/graph_node.h"
 #include "stella_vslam/data/landmark.h"
 
-namespace {
-struct {
-    bool operator()(const std::pair<unsigned int, std::shared_ptr<stella_vslam::data::keyframe>>& a, const std::pair<unsigned int, std::shared_ptr<stella_vslam::data::keyframe>>& b) {
-        return a.first < b.first || (a.first == b.first && a.second != nullptr && (b.second == nullptr || a.second->id_ < b.second->id_));
-    }
-} cmp_num_shared_lms_and_keyfrm_pairs;
-} // namespace
-
 namespace stella_vslam {
 namespace data {
 
@@ -131,7 +123,8 @@ void graph_node::update_connections(unsigned int min_num_shared_lms) {
 
     // sort with number of shared landmarks and keyframe IDs for consistency; IDs are also in reverse order
     // to match selection of nearest_covisibility.
-    std::sort(num_shared_lms_and_covisibility_pairs.rbegin(), num_shared_lms_and_covisibility_pairs.rend(), cmp_num_shared_lms_and_keyfrm_pairs);
+    std::sort(num_shared_lms_and_covisibility_pairs.rbegin(), num_shared_lms_and_covisibility_pairs.rend(),
+              less_number_and_id_object_pairs<unsigned int, data::keyframe>());
 
     decltype(ordered_covisibilities_) ordered_covisibilities;
     ordered_covisibilities.reserve(num_shared_lms_and_covisibility_pairs.size());
@@ -174,7 +167,8 @@ void graph_node::update_covisibility_orders_impl() {
     }
 
     // sort with number of shared landmarks and keyframe IDs for consistency
-    std::sort(num_shared_lms_and_keyfrm_pairs.rbegin(), num_shared_lms_and_keyfrm_pairs.rend(), cmp_num_shared_lms_and_keyfrm_pairs);
+    std::sort(num_shared_lms_and_keyfrm_pairs.rbegin(), num_shared_lms_and_keyfrm_pairs.rend(),
+              less_number_and_id_object_pairs<unsigned int, data::keyframe>());
 
     ordered_covisibilities_.clear();
     ordered_covisibilities_.reserve(num_shared_lms_and_keyfrm_pairs.size());

src/stella_vslam/module/local_map_updater.cc

@@ -34,22 +34,27 @@ bool local_map_updater::acquire_local_map(const std::vector<std::shared_ptr<data
 bool local_map_updater::find_local_keyframes(const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
                                              const unsigned int num_keypts,
                                              unsigned int keyframe_id_threshold) {
-    const auto keyfrm_to_num_shared_lms = count_num_shared_lms(frm_lms, num_keypts, keyframe_id_threshold);
-    if (keyfrm_to_num_shared_lms.empty()) {
+    const auto num_shared_lms_and_keyfrm = count_num_shared_lms(frm_lms, num_keypts, keyframe_id_threshold);
+    if (num_shared_lms_and_keyfrm.empty()) {
         SPDLOG_TRACE("find_local_keyframes: empty");
         return false;
     }
+
     std::unordered_set<unsigned int> already_found_keyfrm_ids;
-    const auto first_local_keyfrms = find_first_local_keyframes(keyfrm_to_num_shared_lms, 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);
     local_keyfrms_ = first_local_keyfrms;
     std::copy(second_local_keyfrms.begin(), second_local_keyfrms.end(), std::back_inserter(local_keyfrms_));
     return true;
 }
 
-local_map_updater::keyframe_to_num_shared_lms_t local_map_updater::count_num_shared_lms(const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
-                                                                                        const unsigned int num_keypts,
-                                                                                        unsigned int keyframe_id_threshold) const {
+auto local_map_updater::count_num_shared_lms(
+    const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
+    const unsigned int num_keypts,
+    unsigned int keyframe_id_threshold) const
+    -> std::vector<std::pair<unsigned int, std::shared_ptr<data::keyframe>>> {
+    std::vector<std::pair<unsigned int, std::shared_ptr<data::keyframe>>> num_shared_lms_and_keyfrm;
+
     // count the number of sharing landmarks between the current frame and each of the neighbor keyframes
     // key: keyframe, value: number of sharing landmarks
     keyframe_to_num_shared_lms_t keyfrm_to_num_shared_lms;
@@ -70,19 +75,26 @@ local_map_updater::keyframe_to_num_shared_lms_t local_map_updater::count_num_sha
             ++keyfrm_to_num_shared_lms[keyfrm];
         }
     }
-    return keyfrm_to_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>());
+
+    return num_shared_lms_and_keyfrm;
 }
 
-auto local_map_updater::find_first_local_keyframes(const keyframe_to_num_shared_lms_t& keyfrm_to_num_shared_lms,
-                                                   std::unordered_set<unsigned int>& already_found_keyfrm_ids)
+auto local_map_updater::find_first_local_keyframes(
+    const std::vector<std::pair<unsigned int, std::shared_ptr<data::keyframe>>>& num_shared_lms_and_keyfrm,
+    std::unordered_set<unsigned int>& already_found_keyfrm_ids)
     -> std::vector<std::shared_ptr<data::keyframe>> {
     std::vector<std::shared_ptr<data::keyframe>> first_local_keyfrms;
-    first_local_keyfrms.reserve(2 * keyfrm_to_num_shared_lms.size());
+    first_local_keyfrms.reserve(std::min(static_cast<size_t>(max_num_local_keyfrms_), 2 * num_shared_lms_and_keyfrm.size()));
 
     unsigned int max_num_shared_lms = 0;
-    for (auto& keyfrm_and_num_shared_lms : keyfrm_to_num_shared_lms) {
-        const auto& keyfrm = keyfrm_and_num_shared_lms.first;
-        const auto num_shared_lms = keyfrm_and_num_shared_lms.second;
+    for (auto& keyfrm_and_num_shared_lms : num_shared_lms_and_keyfrm) {
+        const auto num_shared_lms = keyfrm_and_num_shared_lms.first;
+        const auto& keyfrm = keyfrm_and_num_shared_lms.second;
 
         if (keyfrm->will_be_erased()) {
             continue;
@@ -98,6 +110,10 @@ auto local_map_updater::find_first_local_keyframes(const keyframe_to_num_shared_
             max_num_shared_lms = num_shared_lms;
             nearest_covisibility_ = keyfrm;
         }
+
+        if (max_num_local_keyfrms_ <= first_local_keyfrms.size()) {
+            break;
+        }
     }
 
     return first_local_keyfrms;

src/stella_vslam/module/local_map_updater.h

@@ -44,13 +44,16 @@ class local_map_updater {
                               unsigned int keyframe_id_threshold);
 
     //! Count the number of shared landmarks between the current frame and each of the neighbor keyframes
-    keyframe_to_num_shared_lms_t count_num_shared_lms(const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
-                                                      const unsigned int num_keypts,
-                                                      unsigned int keyframe_id_threshold) const;
+    auto count_num_shared_lms(
+        const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
+        const unsigned int num_keypts,
+        unsigned int keyframe_id_threshold) const
+        -> std::vector<std::pair<unsigned int, std::shared_ptr<data::keyframe>>>;
 
     //! Find the first-order local keyframes
-    auto find_first_local_keyframes(const keyframe_to_num_shared_lms_t& keyfrm_weights,
-                                    std::unordered_set<unsigned int>& already_found_ids)
+    auto find_first_local_keyframes(
+        const std::vector<std::pair<unsigned int, std::shared_ptr<data::keyframe>>>& keyfrm_weights,
+        std::unordered_set<unsigned int>& already_found_ids)
         -> std::vector<std::shared_ptr<data::keyframe>>;
 
     //! Find the second-order local keyframes

src/stella_vslam/type.h

@@ -133,6 +133,20 @@ struct id_less<std::weak_ptr<T>> {
     }
 };
 
+template<class T, class U>
+struct less_number_and_id_object_pairs {
+    bool operator()(const std::pair<T, std::shared_ptr<U>>& a, const std::pair<T, std::shared_ptr<U>>& b) {
+        return a.first < b.first || (a.first == b.first && a.second != nullptr && (b.second == nullptr || a.second->id_ < b.second->id_));
+    }
+};
+
+template<class T, class U>
+struct greater_number_and_id_object_pairs {
+    bool operator()(const std::pair<T, std::shared_ptr<U>>& a, const std::pair<T, std::shared_ptr<U>>& b) {
+        return a.first > b.first || (a.first == b.first && a.second != nullptr && (b.second == nullptr || a.second->id_ < b.second->id_));
+    }
+};
+
 template<class T>
 using id_ordered_set = std::set<T, id_less<T>>;