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 &pts0, const std::vector &pts1, const cv::Mat &desc0, const cv::Mat &desc1, size_t id0, size_t id1, std::vector &matches) { + if (pts0.empty() || pts1.empty()) + return; - // Our 1to2 and 2to1 match vectors - std::vector> 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 matches_good; - robust_symmetry_test(matches0to1, matches1to0, matches_good); + matcher->match(desc0, desc1, matches_good); // Convert points into points for RANSAC std::vector pts0_rsc, pts1_rsc; @@ -534,42 +524,3 @@ void TrackDescriptor::robust_match(const std::vector &pts0, const matches.push_back(matches_good.at(i)); } } - -void TrackDescriptor::robust_ratio_test(std::vector> &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> &matches1, std::vector> &matches2, - std::vector &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 &pts0, const std::vector &pts1, const cv::Mat &desc0, const cv::Mat &desc1, size_t id0, size_t id1, std::vector &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> &matches); - void robust_symmetry_test(std::vector> &matches1, std::vector> &matches2, - std::vector &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 orb1 = cv::ORB::create(); // Our descriptor matcher - cv::Ptr matcher = cv::DescriptorMatcher::create("BruteForce-Hamming"); + cv::Ptr matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true); // Parameters for our FAST grid detector int threshold;