From dbde2d1ee13f7058213d076297821ffdca87128d Mon Sep 17 00:00:00 2001
From: ymd-stella <world.applepie@gmail.com>
Date: Sat, 24 Feb 2024 17:07:26 +0900
Subject: [PATCH] Implement ratio test in match_for_triangulation

---
 src/stella_vslam/mapping_module.cc |  3 +--
 src/stella_vslam/match/robust.cc   | 16 ++++++++++++++--
 2 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/src/stella_vslam/mapping_module.cc b/src/stella_vslam/mapping_module.cc
index edff92c3d..c75c0b215 100644
--- a/src/stella_vslam/mapping_module.cc
+++ b/src/stella_vslam/mapping_module.cc
@@ -276,8 +276,7 @@ void mapping_module::create_new_landmarks(std::atomic<bool>& abort_create_new_la
     // in order to triangulate landmarks between `cur_keyfrm_` and each of the covisibilities
     const auto cur_covisibilities = cur_keyfrm_->graph_node_->get_top_n_covisibilities(num_covisibilities_for_landmark_generation_);
 
-    // lowe's_ratio will not be used
-    match::robust robust_matcher(0.0, false);
+    match::robust robust_matcher(0.95, false);
 
     // camera center of the current keyframe
     const Vec3_t cur_cam_center = cur_keyfrm_->get_trans_wc();
diff --git a/src/stella_vslam/match/robust.cc b/src/stella_vslam/match/robust.cc
index 4f039e05f..a1830b343 100644
--- a/src/stella_vslam/match/robust.cc
+++ b/src/stella_vslam/match/robust.cc
@@ -65,6 +65,7 @@ unsigned int robust::match_for_triangulation(const std::shared_ptr<data::keyfram
                 // Find a keypoint in keyframe 2 that has the minimum hamming distance
                 unsigned int best_hamm_dist = HAMMING_DIST_THR_LOW;
                 int best_idx_2 = -1;
+                unsigned int second_best_hamm_dist = MAX_HAMMING_DIST;
 
                 for (const auto idx_2 : keyfrm_2_indices) {
                     // Ignore if the keypoint is associated any 3D points
@@ -113,8 +114,14 @@ unsigned int robust::match_for_triangulation(const std::shared_ptr<data::keyfram
                     const bool is_inlier = check_epipolar_constraint(bearing_1, bearing_2, E_12,
                                                                      keyfrm_1->orb_params_->scale_factors_.at(keypt_1.octave));
                     if (is_inlier) {
-                        best_idx_2 = idx_2;
-                        best_hamm_dist = hamm_dist;
+                        if (hamm_dist < best_hamm_dist) {
+                            second_best_hamm_dist = best_hamm_dist;
+                            best_hamm_dist = hamm_dist;
+                            best_idx_2 = idx_2;
+                        }
+                        else if (hamm_dist < second_best_hamm_dist) {
+                            second_best_hamm_dist = hamm_dist;
+                        }
                     }
                 }
 
@@ -122,6 +129,11 @@ unsigned int robust::match_for_triangulation(const std::shared_ptr<data::keyfram
                     continue;
                 }
 
+                // Ratio test
+                if (lowe_ratio_ * second_best_hamm_dist < static_cast<float>(best_hamm_dist)) {
+                    continue;
+                }
+
                 is_already_matched_in_keyfrm_2.at(best_idx_2) = true;
                 matched_indices_2_in_keyfrm_1.at(idx_1) = best_idx_2;
                 ++num_matches;