diff --git a/src/matchers/matcher_base.cpp b/src/matchers/matcher_base.cpp
index 84e8d41b73e72bd7f568b4a9dd85fb53bf4fcb84..40ccd24ae3f6278433a035f687361c5b4c06d053 100644
--- a/src/matchers/matcher_base.cpp
+++ b/src/matchers/matcher_base.cpp
@@ -125,6 +125,58 @@ std::vector<Scalar> MatcherBase::match(const cv::Mat& _desc1,
return normalized_scores;
}
+std::vector<Scalar> MatcherBase::robustMatch(const KeyPointVector& _raw_kps1,
+ const KeyPointVector& _raw_kps2,
+ const cv::Mat& _raw_desc1,
+ const cv::Mat& _raw_desc2,
+ const int& desc_size_bytes,
+ KeyPointVector& _inlier_kps1,
+ KeyPointVector& _inlier_kps2,
+ cv::Mat& _inlier_desc1,
+ cv::Mat& _inlier_desc2,
+ DMatchVector& _inlier_matches,
+ cv::InputArray _mask)
+{
+ assert(!_raw_desc1.empty() && !_raw_desc2.empty() && "[vision_utils]: Robust match: empty descriptors cv::Mat");
+
+ clock_t tStart = clock();
+
+ std::vector<Scalar> normalized_scores;
+
+ // Obj: distance inliers
+ KeyPointVector _inlier_dist_kps1, _inlier_dist_kps2;
+ cv::Mat _inlier_dist_desc1, _inlier_dist_desc2;
+ DMatchVector _inlier_dist_matches;
+
+
+ if (params_base_ptr_->match_type==MATCH)
+ {
+ // Match
+ DMatchVector raw_matches;
+ normalized_scores = match(_raw_desc1, _raw_desc2, desc_size_bytes, raw_matches);
+
+ // Filter by distance
+ normalized_scores = filterByDistance(_raw_kps1, _raw_kps2, _raw_desc1, _raw_desc2, raw_matches, _inlier_dist_kps1, _inlier_dist_kps2, _inlier_dist_desc1, _inlier_dist_desc2, _inlier_dist_matches, normalized_scores);
+ }
+ else if (params_base_ptr_->match_type == KNNMATCH || params_base_ptr_->match_type == RADIUSMATCH)
+ {
+ // Match
+ std::vector< DMatchVector > raw_matches;
+ normalized_scores = match(_raw_desc1, _raw_desc2, desc_size_bytes, raw_matches);
+
+ // Filter by distance
+ normalized_scores = filterByDistance(_raw_kps1, _raw_kps2, _raw_desc1, _raw_desc2, raw_matches, _inlier_dist_kps1, _inlier_dist_kps2, _inlier_dist_desc1, _inlier_dist_desc2, _inlier_dist_matches, normalized_scores);
+ }
+
+ // Ransac test
+ normalized_scores = ransacTest(_inlier_dist_kps1, _inlier_dist_kps2, _inlier_dist_desc1, _inlier_dist_desc2, _inlier_dist_matches, _inlier_kps1, _inlier_kps2, _inlier_desc1, _inlier_desc2, _inlier_matches, normalized_scores);
+
+ comp_time_ = (double)(clock() - tStart) / CLOCKS_PER_SEC;
+
+ return normalized_scores;
+}
+
+
void MatcherBase::filterByDistance(const int& _max_pixel_dist,
const float& _img_size_percentage,
const KeyPointVector& _kps1,
@@ -203,20 +255,30 @@ void MatcherBase::filterByDistance(const int& _max_pixel_dist,
std::cerr << "[" << name_ << "]: Wrong input type in filterByDistance method." << std::endl;
}
-void MatcherBase::filterByDistance(const KeyPointVector& _kps1,
- const KeyPointVector& _kps2,
+std::vector<Scalar> MatcherBase::filterByDistance(const KeyPointVector& _raw_kps1,
+ const KeyPointVector& _raw_kps2,
+ const cv::Mat& _raw_desc1,
+ const cv::Mat& _raw_desc2,
const DMatchVector& _raw_matches,
KeyPointVector& _inlier_kps1,
KeyPointVector& _inlier_kps2,
- DMatchVector& _inlier_matches)
+ cv::Mat& _inlier_desc1,
+ cv::Mat& _inlier_desc2,
+ DMatchVector& _inlier_matches,
+ const std::vector<Scalar>& _normalized_scores)
{
+ std::vector<Scalar> normalized_scores;
+
if (params_base_ptr_->match_type == MATCH)
{
_inlier_matches.reserve(_raw_matches.size());
for (size_t ii = 0; ii < _raw_matches.size(); ++ii)
{
- cv::Point2f p1 = _kps1[_raw_matches[ii].queryIdx].pt;
- cv::Point2f p2 = _kps2[_raw_matches[ii].trainIdx].pt;
+ cv::Point2f p1 = _raw_kps1[_raw_matches[ii].queryIdx].pt;
+ cv::Point2f p2 = _raw_kps2[_raw_matches[ii].trainIdx].pt;
+ cv::Mat d1 = _raw_desc1.row(_raw_matches[ii].queryIdx);
+ cv::Mat d2 = _raw_desc2.row(_raw_matches[ii].trainIdx);
+
//calculate local distance for each possible match
Scalar dist = std::sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
@@ -227,20 +289,33 @@ void MatcherBase::filterByDistance(const KeyPointVector& _kps1,
_inlier_matches.push_back(_raw_matches[ii]);
_inlier_kps1.push_back(cv::KeyPoint(p1,1));
_inlier_kps2.push_back(cv::KeyPoint(p2,1));
+ _inlier_desc1.push_back(d1);
+ _inlier_desc2.push_back(d2);
+ if (!_normalized_scores.empty())
+ normalized_scores.push_back(_normalized_scores[ii]);
}
}
}
else
std::cerr << "[" << name_ << "]: Wrong input type in filterByDistance method." << std::endl;
+
+ return normalized_scores;
}
-void MatcherBase::filterByDistance(const KeyPointVector& _kps1,
- const KeyPointVector& _kps2,
+std::vector<Scalar> MatcherBase::filterByDistance(const KeyPointVector& _raw_kps1,
+ const KeyPointVector& _raw_kps2,
+ const cv::Mat& _raw_desc1,
+ const cv::Mat& _raw_desc2,
const std::vector< DMatchVector >& _raw_matches,
KeyPointVector& _inlier_kps1,
KeyPointVector& _inlier_kps2,
- DMatchVector& _inlier_matches)
+ cv::Mat& _inlier_desc1,
+ cv::Mat& _inlier_desc2,
+ DMatchVector& _inlier_matches,
+ const std::vector<Scalar>& _normalized_scores)
{
+ std::vector<Scalar> normalized_scores;
+
if (params_base_ptr_->match_type == KNNMATCH || params_base_ptr_->match_type == RADIUSMATCH)
{
_inlier_matches.reserve(_raw_matches.size());
@@ -248,8 +323,10 @@ void MatcherBase::filterByDistance(const KeyPointVector& _kps1,
{
for (unsigned int jj = 0; jj < _raw_matches[ii].size(); jj++)
{
- cv::Point2f p1 = _kps1[_raw_matches[ii][jj].queryIdx].pt;
- cv::Point2f p2 = _kps2[_raw_matches[ii][jj].trainIdx].pt;
+ cv::Point2f p1 = _raw_kps1[_raw_matches[ii][jj].queryIdx].pt;
+ cv::Point2f p2 = _raw_kps2[_raw_matches[ii][jj].trainIdx].pt;
+ cv::Mat d1 = _raw_desc1.row(_raw_matches[ii][jj].queryIdx);
+ cv::Mat d2 = _raw_desc2.row(_raw_matches[ii][jj].trainIdx);
//calculate local distance for each possible match
double dist = std::sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
@@ -260,6 +337,10 @@ void MatcherBase::filterByDistance(const KeyPointVector& _kps1,
_inlier_matches.push_back(_raw_matches[ii][jj]);
_inlier_kps1.push_back(cv::KeyPoint(p2,1));
_inlier_kps2.push_back(cv::KeyPoint(p1,1));
+ _inlier_desc1.push_back(d1);
+ _inlier_desc2.push_back(d2);
+ if (!_normalized_scores.empty())
+ normalized_scores.push_back(_normalized_scores[ii]);
break;
}
}
@@ -267,15 +348,24 @@ void MatcherBase::filterByDistance(const KeyPointVector& _kps1,
}
else
std::cerr << "[" << name_ << "]: Wrong input type in filterByDistance method." << std::endl;
+
+ return normalized_scores;
}
-void MatcherBase::ransacTest(const KeyPointVector& _raw_kps1,
+std::vector<Scalar> MatcherBase::ransacTest(const KeyPointVector& _raw_kps1,
const KeyPointVector& _raw_kps2,
+ cv::Mat& _raw_desc1,
+ cv::Mat& _raw_desc2,
const DMatchVector& _raw_matches,
KeyPointVector& _inlier_kps1,
KeyPointVector& _inlier_kps2,
- DMatchVector& _inlier_matches)
+ cv::Mat& _inlier_desc1,
+ cv::Mat& _inlier_desc2,
+ DMatchVector& _inlier_matches,
+ const std::vector<Scalar>& _normalized_scores)
{
+ std::vector<Scalar> normalized_scores;
+
// Convert keypoints into Point2f
PointVector raw_pts1, raw_pts2;
for (std::vector<cv::DMatch>::const_iterator it= _raw_matches.begin(); it!= _raw_matches.end(); ++it)
@@ -294,18 +384,20 @@ void MatcherBase::ransacTest(const KeyPointVector& _raw_kps1,
params_base_ptr_->ransac_confidence_prob); // confidence probability
// extract the surviving (inliers) matches
- std::vector<uchar>::const_iterator itIn = inliers.begin();
- std::vector<cv::DMatch>::const_iterator itM= _raw_matches.begin();
- // for all raw matches
- for ( ;itIn!= inliers.end(); ++itIn, ++itM)
+ for (unsigned int ii=0; ii<inliers.size(); ii++)
{
- if (*itIn)// it is a valid match
+ if (inliers[ii])// it is a valid match
{
- _inlier_matches.push_back(*itM);
- _inlier_kps1.push_back(_raw_kps1[itM->queryIdx]);
- _inlier_kps2.push_back(_raw_kps2[itM->trainIdx]);
+ _inlier_matches.push_back(_raw_matches[ii]);
+ _inlier_kps1.push_back(_raw_kps1[_raw_matches[ii].queryIdx]);
+ _inlier_kps2.push_back(_raw_kps2[_raw_matches[ii].trainIdx]);
+ _inlier_desc1.push_back(_raw_desc1.row(_raw_matches[ii].queryIdx));
+ _inlier_desc2.push_back(_raw_desc2.row(_raw_matches[ii].trainIdx));
+ if (!_normalized_scores.empty())
+ normalized_scores.push_back(_normalized_scores[ii]);
}
}
+ return normalized_scores;
}
void MatcherBase::nnSymmetryTest(const std::vector<DMatchVector>& _matches1,
diff --git a/src/matchers/matcher_base.h b/src/matchers/matcher_base.h
index 224d2c5bbc9a5bd70569bcae23736510b6c8d4ef..c89a3cbfadb5ff6219e962ec21445836f647bc33 100644
--- a/src/matchers/matcher_base.h
+++ b/src/matchers/matcher_base.h
@@ -90,6 +90,18 @@ class MatcherBase : public VUBase, public std::enable_shared_from_this<MatcherBa
const int& desc_size_bytes,
cv::DMatch& _match);
+ std::vector<Scalar> robustMatch(const KeyPointVector& _raw_kps1,
+ const KeyPointVector& _raw_kps2,
+ const cv::Mat& _raw_desc1,
+ const cv::Mat& _raw_desc2,
+ const int& desc_size_bytes,
+ KeyPointVector& _inlier_kps1,
+ KeyPointVector& _inlier_kps2,
+ cv::Mat& _inlier_desc1,
+ cv::Mat& _inlier_desc2,
+ DMatchVector& _inlier_matches,
+ cv::InputArray _mask=cv::noArray());
+
void filterByDistance(const int& _max_pixel_dist,
const float& _percentage,
const KeyPointVector& _kps1,
@@ -112,27 +124,42 @@ class MatcherBase : public VUBase, public std::enable_shared_from_this<MatcherBa
KeyPointVector& _inlier_kps2,
DMatchVector& _inlier_matches);
- void filterByDistance(const KeyPointVector& _kps1,
- const KeyPointVector& _kps2,
+ std::vector<Scalar> filterByDistance(const KeyPointVector& _raw_kps1,
+ const KeyPointVector& _raw_kps2,
+ const cv::Mat& _raw_desc1,
+ const cv::Mat& _raw_desc2,
const DMatchVector& _raw_matches,
KeyPointVector& _inlier_kps1,
KeyPointVector& _inlier_kps2,
- DMatchVector& _inlier_matches);
-
- void filterByDistance(const KeyPointVector& _kps1,
- const KeyPointVector& _kps2,
- const std::vector< DMatchVector >& _raw_matches,
- KeyPointVector& _inlier_kps1,
- KeyPointVector& _inlier_kps2,
- DMatchVector& _inlier_matches);
+ cv::Mat& _inlier_desc1,
+ cv::Mat& _inlier_desc2,
+ DMatchVector& _inlier_matches,
+ const std::vector<Scalar>& _normalized_scores = std::vector<Scalar>());
+
+ std::vector<Scalar> filterByDistance(const KeyPointVector& _raw_kps1,
+ const KeyPointVector& _raw_kps2,
+ const cv::Mat& _raw_desc1,
+ const cv::Mat& _raw_desc2,
+ const std::vector< DMatchVector >& _raw_matches,
+ KeyPointVector& _inlier_kps1,
+ KeyPointVector& _inlier_kps2,
+ cv::Mat& _inlier_desc1,
+ cv::Mat& _inlier_desc2,
+ DMatchVector& _inlier_matches,
+ const std::vector<Scalar>& _normalized_scores = std::vector<Scalar>());
// Identify good matches using RANSAC
- void ransacTest(const KeyPointVector& _raw_kps1,
- const KeyPointVector& _raw_kps2,
- const DMatchVector& _raw_matches,
- KeyPointVector& _inlier_kps1,
- KeyPointVector& _inlier_kps2,
- DMatchVector& _inlier_matches);
+ std::vector<Scalar> ransacTest(const KeyPointVector& _raw_kps1,
+ const KeyPointVector& _raw_kps2,
+ cv::Mat& _raw_desc1,
+ cv::Mat& _raw_desc2,
+ const DMatchVector& _raw_matches,
+ KeyPointVector& _inlier_kps1,
+ KeyPointVector& _inlier_kps2,
+ cv::Mat& _inlier_desc1,
+ cv::Mat& _inlier_desc2,
+ DMatchVector& _inlier_matches,
+ const std::vector<Scalar>& _normalized_scores = std::vector<Scalar>());
// Check symmetry between nn matches from <im1->im2> and from <im2->im1> pairs
void nnSymmetryTest(const std::vector<DMatchVector>& _matches1,
diff --git a/src/test/gtest_matcher_base.cpp b/src/test/gtest_matcher_base.cpp
index 142caa33672de63149bb611cb64f6c9303289b83..ca651f01dfbcf70fd5e25abb65c44c45378c5b42 100644
--- a/src/test/gtest_matcher_base.cpp
+++ b/src/test/gtest_matcher_base.cpp
@@ -91,15 +91,17 @@ TEST(MatcherBase, filterByDistance)
ASSERT_EQ(params_ptr->match_type,1);
// match
+ KeyPointVector kpts1_matched, kpts2_matched;
+ cv::Mat desc1_matched, desc2_matched;
DMatchVector matches;
mat_ptr->match(desc1,desc2,des_ptr->getSize(),matches);
ASSERT_TRUE(matches.size()-75>0);
- KeyPointVector inlier_kpts1;
- KeyPointVector inlier_kpts2;
+ KeyPointVector inlier_kpts1, inlier_kpts2;
+ cv::Mat inlier_desc1, inlier_desc2;
DMatchVector inlier_matches;
- mat_ptr->filterByDistance(kpts1, kpts2, matches,
- inlier_kpts1, inlier_kpts2, inlier_matches);
+ mat_ptr->filterByDistance(kpts1, kpts2, desc1, desc2, matches,
+ inlier_kpts1, inlier_kpts2, inlier_desc1, inlier_desc2, inlier_matches);
ASSERT_TRUE(inlier_matches.size()<40);
}
@@ -147,15 +149,17 @@ TEST(MatcherBase, ransacTest)
ASSERT_EQ(params_ptr->match_type,1);
// match
+ KeyPointVector kpts1_matched, kpts2_matched;
+ cv::Mat desc1_matched, desc2_matched;
DMatchVector matches;
mat_ptr->match(desc1,desc2,des_ptr->getSize(),matches);
ASSERT_TRUE(matches.size()-75>0);
- KeyPointVector inlier_kpts1;
- KeyPointVector inlier_kpts2;
+ KeyPointVector inlier_kpts1, inlier_kpts2;
+ cv::Mat inlier_desc1, inlier_desc2;
DMatchVector inlier_matches;
- mat_ptr->ransacTest(kpts1, kpts2, matches,
- inlier_kpts1, inlier_kpts2, inlier_matches);
+ mat_ptr->ransacTest(kpts1, kpts2, desc1, desc2, matches,
+ inlier_kpts1, inlier_kpts2, inlier_desc1, inlier_desc2, inlier_matches);
ASSERT_TRUE(inlier_matches.size()<50);
}