Implement ratio test in match_for_triangulation

stella-cv · Feb 24, 2024 · dbde2d1 · dbde2d1
1 parent debdd64
commit dbde2d1
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 4 deletions.
diff --git 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
@@ -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,15 +114,26 @@ 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;
+                        }
                     }
                 }
 
                 if (best_idx_2 < 0) {
                     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;