diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp new file mode 100644 index 0000000000..7ef9f887a6 --- /dev/null +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp @@ -0,0 +1,152 @@ +/* + * RobustMatcher.cpp + * + * Created on: Jun 4, 2014 + * Author: eriba + */ + +#include "RobustMatcher.h" +#include + +#include + +RobustMatcher::~RobustMatcher() +{ + // TODO Auto-generated destructor stub +} + +void RobustMatcher::computeKeyPoints( const cv::Mat& image, std::vector& keypoints) +{ + detector_->detect(image, keypoints); +} + +void RobustMatcher::computeDescriptors( const cv::Mat& image, std::vector& keypoints, cv::Mat& descriptors) +{ + extractor_->compute(image, keypoints, descriptors); +} + +int RobustMatcher::ratioTest(std::vector > &matches) +{ + int removed = 0; + // for all matches + for ( std::vector >::iterator + matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator) + { + // if 2 NN has been identified + if (matchIterator->size() > 1) + { + // check distance ratio + if ((*matchIterator)[0].distance / (*matchIterator)[1].distance > ratio_) + { + matchIterator->clear(); // remove match + removed++; + } + } + else + { // does not have 2 neighbours + matchIterator->clear(); // remove match + removed++; + } + } + return removed; +} + +void RobustMatcher::symmetryTest( const std::vector >& matches1, + const std::vector >& matches2, + std::vector& symMatches ) +{ + + // for all matches image 1 -> image 2 + for (std::vector >::const_iterator + matchIterator1 = matches1.begin(); matchIterator1 != matches1.end(); ++matchIterator1) + { + + // ignore deleted matches + if (matchIterator1->empty() || matchIterator1->size() < 2) + continue; + + // for all matches image 2 -> image 1 + for (std::vector >::const_iterator + matchIterator2 = matches2.begin(); matchIterator2 != matches2.end(); ++matchIterator2) + { + // ignore deleted matches + if (matchIterator2->empty() || matchIterator2->size() < 2) + continue; + + // Match symmetry test + if ((*matchIterator1)[0].queryIdx == + (*matchIterator2)[0].trainIdx && + (*matchIterator2)[0].queryIdx == + (*matchIterator1)[0].trainIdx) + { + // add symmetrical match + symMatches.push_back( + cv::DMatch((*matchIterator1)[0].queryIdx, + (*matchIterator1)[0].trainIdx, + (*matchIterator1)[0].distance)); + break; // next match in image 1 -> image 2 + } + } + } + +} + +void RobustMatcher::robustMatch( const cv::Mat& frame, std::vector& good_matches, + std::vector& keypoints_frame, const cv::Mat& descriptors_model ) +{ + + // 1a. Detection of the ORB features + this->computeKeyPoints(frame, keypoints_frame); + + // 1b. Extraction of the ORB descriptors + cv::Mat descriptors_frame; + this->computeDescriptors(frame, keypoints_frame, descriptors_frame); + + // 2. Match the two image descriptors + std::vector > matches12, matches21; + + // 2a. From image 1 to image 2 + matcher_->knnMatch(descriptors_frame, descriptors_model, matches12, 2); // return 2 nearest neighbours + + // 2b. From image 2 to image 1 + matcher_->knnMatch(descriptors_model, descriptors_frame, matches21, 2); // return 2 nearest neighbours + + // 3. Remove matches for which NN ratio is > than threshold + // clean image 1 -> image 2 matches + int removed1 = ratioTest(matches12); + // clean image 2 -> image 1 matches + int removed2 = ratioTest(matches21); + + // 4. Remove non-symmetrical matches + symmetryTest(matches12, matches21, good_matches); + +} + +void RobustMatcher::fastRobustMatch( const cv::Mat& frame, std::vector& good_matches, + std::vector& keypoints_frame, + const cv::Mat& descriptors_model ) +{ + good_matches.clear(); + + // 1a. Detection of the ORB features + this->computeKeyPoints(frame, keypoints_frame); + + // 1b. Extraction of the ORB descriptors + cv::Mat descriptors_frame; + this->computeDescriptors(frame, keypoints_frame, descriptors_frame); + + // 2. Match the two image descriptors + std::vector > matches; + matcher_->knnMatch(descriptors_frame, descriptors_model, matches, 2); + + // 3. Remove matches for which NN ratio is > than threshold + int removed = ratioTest(matches); + + // 4. Fill good matches container + for ( std::vector >::iterator + matchIterator= matches.begin(); matchIterator!= matches.end(); ++matchIterator) + { + if (!matchIterator->empty()) good_matches.push_back((*matchIterator)[0]); + } + +}