From 1fbdb63055aec3a6a6cc5536bfcc53f237c4841a Mon Sep 17 00:00:00 2001
From: Borong Yuan <yuanborong@hotmail.com>
Date: Sun, 20 Oct 2024 16:44:27 +0800
Subject: [PATCH] Use OpenCV's brute-force match with cross-check instead of
 KNN match with ratio check and symmetry check

---
 ov_core/src/track/TrackDescriptor.cpp | 55 ++-------------------------
 ov_core/src/track/TrackDescriptor.h   | 17 +++------
 2 files changed, 8 insertions(+), 64 deletions(-)

diff --git a/ov_core/src/track/TrackDescriptor.cpp b/ov_core/src/track/TrackDescriptor.cpp
index 915873d4e..ee10f548a 100644
--- a/ov_core/src/track/TrackDescriptor.cpp
+++ b/ov_core/src/track/TrackDescriptor.cpp
@@ -479,21 +479,11 @@ void TrackDescriptor::perform_detection_stereo(const cv::Mat &img0, const cv::Ma
 
 void TrackDescriptor::robust_match(const std::vector<cv::KeyPoint> &pts0, const std::vector<cv::KeyPoint> &pts1, const cv::Mat &desc0,
                                    const cv::Mat &desc1, size_t id0, size_t id1, std::vector<cv::DMatch> &matches) {
+  if (pts0.empty() || pts1.empty())
+    return;
 
-  // Our 1to2 and 2to1 match vectors
-  std::vector<std::vector<cv::DMatch>> matches0to1, matches1to0;
-
-  // Match descriptors (return 2 nearest neighbours)
-  matcher->knnMatch(desc0, desc1, matches0to1, 2);
-  matcher->knnMatch(desc1, desc0, matches1to0, 2);
-
-  // Do a ratio test for both matches
-  robust_ratio_test(matches0to1);
-  robust_ratio_test(matches1to0);
-
-  // Finally do a symmetry test
   std::vector<cv::DMatch> matches_good;
-  robust_symmetry_test(matches0to1, matches1to0, matches_good);
+  matcher->match(desc0, desc1, matches_good);
 
   // Convert points into points for RANSAC
   std::vector<cv::Point2f> pts0_rsc, pts1_rsc;
@@ -534,42 +524,3 @@ void TrackDescriptor::robust_match(const std::vector<cv::KeyPoint> &pts0, const
     matches.push_back(matches_good.at(i));
   }
 }
-
-void TrackDescriptor::robust_ratio_test(std::vector<std::vector<cv::DMatch>> &matches) {
-  // Loop through all matches
-  for (auto &match : matches) {
-    // If 2 NN has been identified, else remove this feature
-    if (match.size() > 1) {
-      // check distance ratio, remove it if the ratio is larger
-      if (match[0].distance / match[1].distance > knn_ratio) {
-        match.clear();
-      }
-    } else {
-      // does not have 2 neighbours, so remove it
-      match.clear();
-    }
-  }
-}
-
-void TrackDescriptor::robust_symmetry_test(std::vector<std::vector<cv::DMatch>> &matches1, std::vector<std::vector<cv::DMatch>> &matches2,
-                                           std::vector<cv::DMatch> &good_matches) {
-  // for all matches image 1 -> image 2
-  for (auto &match1 : matches1) {
-    // ignore deleted matches
-    if (match1.empty() || match1.size() < 2)
-      continue;
-    // for all matches image 2 -> image 1
-    for (auto &match2 : matches2) {
-      // ignore deleted matches
-      if (match2.empty() || match2.size() < 2)
-        continue;
-      // Match symmetry test
-      if (match1[0].queryIdx == match2[0].trainIdx && match2[0].queryIdx == match1[0].trainIdx) {
-        // add symmetrical match
-        good_matches.emplace_back(cv::DMatch(match1[0].queryIdx, match1[0].trainIdx, match1[0].distance));
-        // next match in image 1 -> image 2
-        break;
-      }
-    }
-  }
-}
diff --git a/ov_core/src/track/TrackDescriptor.h b/ov_core/src/track/TrackDescriptor.h
index 30ce9097f..bf6e9a817 100644
--- a/ov_core/src/track/TrackDescriptor.h
+++ b/ov_core/src/track/TrackDescriptor.h
@@ -127,21 +127,14 @@ class TrackDescriptor : public TrackBase {
    * @param id1 id of the second camera
    * @param matches vector of matches that we have found
    *
-   * This will perform a "robust match" between the two sets of points (slow but has great results).
-   * First we do a simple KNN match from 1to2 and 2to1, which is followed by a ratio check and symmetry check.
-   * Original code is from the "RobustMatcher" in the opencv examples, and seems to give very good results in the matches.
-   * https://github.com/opencv/opencv/blob/master/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp
+   * This will perform a "brute-force match with cross-check" between the two sets of features.
+   * When there are only a few hundred features, this is faster than KNN match followed by a ratio check and symmetry check.
+   * See BFMatcher::create from opencv for more details.
+   * https://docs.opencv.org/4.x/d3/da1/classcv_1_1BFMatcher.html#a02ef4d594b33d091767cbfe442aefb8a
    */
   void robust_match(const std::vector<cv::KeyPoint> &pts0, const std::vector<cv::KeyPoint> &pts1, const cv::Mat &desc0,
                     const cv::Mat &desc1, size_t id0, size_t id1, std::vector<cv::DMatch> &matches);
 
-  // Helper functions for the robust_match function
-  // Original code is from the "RobustMatcher" in the opencv examples
-  // https://github.com/opencv/opencv/blob/master/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp
-  void robust_ratio_test(std::vector<std::vector<cv::DMatch>> &matches);
-  void robust_symmetry_test(std::vector<std::vector<cv::DMatch>> &matches1, std::vector<std::vector<cv::DMatch>> &matches2,
-                            std::vector<cv::DMatch> &good_matches);
-
   // Timing variables
   boost::posix_time::ptime rT1, rT2, rT3, rT4, rT5, rT6, rT7;
 
@@ -150,7 +143,7 @@ class TrackDescriptor : public TrackBase {
   cv::Ptr<cv::ORB> orb1 = cv::ORB::create();
 
   // Our descriptor matcher
-  cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
+  cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true);
 
   // Parameters for our FAST grid detector
   int threshold;