From 4f3dbf522a38006c40e5246bdc5565b71b034522 Mon Sep 17 00:00:00 2001
From: ymd-stella <world.applepie@gmail.com>
Date: Sat, 24 Feb 2024 18:42:18 +0900
Subject: [PATCH] Add neighbor keyframes in add_loop_candidate

---
 src/stella_vslam/module/loop_detector.cc | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/stella_vslam/module/loop_detector.cc b/src/stella_vslam/module/loop_detector.cc
index 39f4a231..e5f919ce 100644
--- a/src/stella_vslam/module/loop_detector.cc
+++ b/src/stella_vslam/module/loop_detector.cc
@@ -57,7 +57,16 @@ bool loop_detector::detect_loop_candidates() {
 }
 
 void loop_detector::add_loop_candidate(const std::shared_ptr<data::keyframe>& keyfrm) {
-    loop_candidates_to_validate_.insert(keyfrm);
+    if (top_n_covisibilities_to_search_ > 0) {
+        loop_candidates_to_validate_.insert(keyfrm);
+        auto covisibilities = keyfrm->graph_node_->get_top_n_covisibilities(top_n_covisibilities_to_search_);
+        for (const auto& covisibility : covisibilities) {
+            loop_candidates_to_validate_.insert(covisibility);
+        }
+    }
+    else {
+        loop_candidates_to_validate_.insert(keyfrm);
+    }
 }
 
 bool loop_detector::detect_loop_candidates_impl() {