Skip to content
Snippets Groups Projects
Commit 8c55a392 authored by Angel Santamaria-Navarro's avatar Angel Santamaria-Navarro
Browse files

Add robustMatch initial version. no gtest yet

parent 6c07df30
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/matchers/matcher_base.cpp
+ 112
− 20
View file @ 8c55a392
...@@ -125,6 +125,58 @@ std::vector<Scalar> MatcherBase::match(const cv::Mat& _desc1, ...@@ -125,6 +125,58 @@ std::vector<Scalar> MatcherBase::match(const cv::Mat& _desc1,
return normalized_scores; 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, void MatcherBase::filterByDistance(const int& _max_pixel_dist,
const float& _img_size_percentage, const float& _img_size_percentage,
const KeyPointVector& _kps1, const KeyPointVector& _kps1,
...@@ -203,20 +255,30 @@ void MatcherBase::filterByDistance(const int& _max_pixel_dist, ...@@ -203,20 +255,30 @@ void MatcherBase::filterByDistance(const int& _max_pixel_dist,
std::cerr << "[" << name_ << "]: Wrong input type in filterByDistance method." << std::endl; std::cerr << "[" << name_ << "]: Wrong input type in filterByDistance method." << std::endl;
} }
void MatcherBase::filterByDistance(const KeyPointVector& _kps1, std::vector<Scalar> MatcherBase::filterByDistance(const KeyPointVector& _raw_kps1,
const KeyPointVector& _kps2, const KeyPointVector& _raw_kps2,
const cv::Mat& _raw_desc1,
const cv::Mat& _raw_desc2,
const DMatchVector& _raw_matches, const DMatchVector& _raw_matches,
KeyPointVector& _inlier_kps1, KeyPointVector& _inlier_kps1,
KeyPointVector& _inlier_kps2, 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) if (params_base_ptr_->match_type == MATCH)
{ {
_inlier_matches.reserve(_raw_matches.size()); _inlier_matches.reserve(_raw_matches.size());
for (size_t ii = 0; ii < _raw_matches.size(); ++ii) for (size_t ii = 0; ii < _raw_matches.size(); ++ii)
{ {
cv::Point2f p1 = _kps1[_raw_matches[ii].queryIdx].pt; cv::Point2f p1 = _raw_kps1[_raw_matches[ii].queryIdx].pt;
cv::Point2f p2 = _kps2[_raw_matches[ii].trainIdx].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 //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)); 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, ...@@ -227,20 +289,33 @@ void MatcherBase::filterByDistance(const KeyPointVector& _kps1,
_inlier_matches.push_back(_raw_matches[ii]); _inlier_matches.push_back(_raw_matches[ii]);
_inlier_kps1.push_back(cv::KeyPoint(p1,1)); _inlier_kps1.push_back(cv::KeyPoint(p1,1));
_inlier_kps2.push_back(cv::KeyPoint(p2,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 else
std::cerr << "[" << name_ << "]: Wrong input type in filterByDistance method." << std::endl; std::cerr << "[" << name_ << "]: Wrong input type in filterByDistance method." << std::endl;
return normalized_scores;
} }
void MatcherBase::filterByDistance(const KeyPointVector& _kps1, std::vector<Scalar> MatcherBase::filterByDistance(const KeyPointVector& _raw_kps1,
const KeyPointVector& _kps2, const KeyPointVector& _raw_kps2,
const cv::Mat& _raw_desc1,
const cv::Mat& _raw_desc2,
const std::vector< DMatchVector >& _raw_matches, const std::vector< DMatchVector >& _raw_matches,
KeyPointVector& _inlier_kps1, KeyPointVector& _inlier_kps1,
KeyPointVector& _inlier_kps2, 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) if (params_base_ptr_->match_type == KNNMATCH || params_base_ptr_->match_type == RADIUSMATCH)
{ {
_inlier_matches.reserve(_raw_matches.size()); _inlier_matches.reserve(_raw_matches.size());
...@@ -248,8 +323,10 @@ void MatcherBase::filterByDistance(const KeyPointVector& _kps1, ...@@ -248,8 +323,10 @@ void MatcherBase::filterByDistance(const KeyPointVector& _kps1,
{ {
for (unsigned int jj = 0; jj < _raw_matches[ii].size(); jj++) for (unsigned int jj = 0; jj < _raw_matches[ii].size(); jj++)
{ {
cv::Point2f p1 = _kps1[_raw_matches[ii][jj].queryIdx].pt; cv::Point2f p1 = _raw_kps1[_raw_matches[ii][jj].queryIdx].pt;
cv::Point2f p2 = _kps2[_raw_matches[ii][jj].trainIdx].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 //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)); 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, ...@@ -260,6 +337,10 @@ void MatcherBase::filterByDistance(const KeyPointVector& _kps1,
_inlier_matches.push_back(_raw_matches[ii][jj]); _inlier_matches.push_back(_raw_matches[ii][jj]);
_inlier_kps1.push_back(cv::KeyPoint(p2,1)); _inlier_kps1.push_back(cv::KeyPoint(p2,1));
_inlier_kps2.push_back(cv::KeyPoint(p1,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; break;
} }
} }
...@@ -267,15 +348,24 @@ void MatcherBase::filterByDistance(const KeyPointVector& _kps1, ...@@ -267,15 +348,24 @@ void MatcherBase::filterByDistance(const KeyPointVector& _kps1,
} }
else else
std::cerr << "[" << name_ << "]: Wrong input type in filterByDistance method." << std::endl; 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, const KeyPointVector& _raw_kps2,
cv::Mat& _raw_desc1,
cv::Mat& _raw_desc2,
const DMatchVector& _raw_matches, const DMatchVector& _raw_matches,
KeyPointVector& _inlier_kps1, KeyPointVector& _inlier_kps1,
KeyPointVector& _inlier_kps2, 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 // Convert keypoints into Point2f
PointVector raw_pts1, raw_pts2; PointVector raw_pts1, raw_pts2;
for (std::vector<cv::DMatch>::const_iterator it= _raw_matches.begin(); it!= _raw_matches.end(); ++it) 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, ...@@ -294,18 +384,20 @@ void MatcherBase::ransacTest(const KeyPointVector& _raw_kps1,
params_base_ptr_->ransac_confidence_prob); // confidence probability params_base_ptr_->ransac_confidence_prob); // confidence probability
// extract the surviving (inliers) matches // extract the surviving (inliers) matches
std::vector<uchar>::const_iterator itIn = inliers.begin(); for (unsigned int ii=0; ii<inliers.size(); ii++)
std::vector<cv::DMatch>::const_iterator itM= _raw_matches.begin();
// for all raw matches
for ( ;itIn!= inliers.end(); ++itIn, ++itM)
{ {
if (*itIn)// it is a valid match if (inliers[ii])// it is a valid match
{ {
_inlier_matches.push_back(*itM); _inlier_matches.push_back(_raw_matches[ii]);
_inlier_kps1.push_back(_raw_kps1[itM->queryIdx]); _inlier_kps1.push_back(_raw_kps1[_raw_matches[ii].queryIdx]);
_inlier_kps2.push_back(_raw_kps2[itM->trainIdx]); _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, void MatcherBase::nnSymmetryTest(const std::vector<DMatchVector>& _matches1,
......
src/matchers/matcher_base.h
+ 43
− 16
View file @ 8c55a392
...@@ -90,6 +90,18 @@ class MatcherBase : public VUBase, public std::enable_shared_from_this<MatcherBa ...@@ -90,6 +90,18 @@ class MatcherBase : public VUBase, public std::enable_shared_from_this<MatcherBa
const int& desc_size_bytes, const int& desc_size_bytes,
cv::DMatch& _match); 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, void filterByDistance(const int& _max_pixel_dist,
const float& _percentage, const float& _percentage,
const KeyPointVector& _kps1, const KeyPointVector& _kps1,
...@@ -112,27 +124,42 @@ class MatcherBase : public VUBase, public std::enable_shared_from_this<MatcherBa ...@@ -112,27 +124,42 @@ class MatcherBase : public VUBase, public std::enable_shared_from_this<MatcherBa
KeyPointVector& _inlier_kps2, KeyPointVector& _inlier_kps2,
DMatchVector& _inlier_matches); DMatchVector& _inlier_matches);
void filterByDistance(const KeyPointVector& _kps1, std::vector<Scalar> filterByDistance(const KeyPointVector& _raw_kps1,
const KeyPointVector& _kps2, const KeyPointVector& _raw_kps2,
const cv::Mat& _raw_desc1,
const cv::Mat& _raw_desc2,
const DMatchVector& _raw_matches, const DMatchVector& _raw_matches,
KeyPointVector& _inlier_kps1, KeyPointVector& _inlier_kps1,
KeyPointVector& _inlier_kps2, KeyPointVector& _inlier_kps2,
DMatchVector& _inlier_matches); cv::Mat& _inlier_desc1,
cv::Mat& _inlier_desc2,
void filterByDistance(const KeyPointVector& _kps1, DMatchVector& _inlier_matches,
const KeyPointVector& _kps2, const std::vector<Scalar>& _normalized_scores = std::vector<Scalar>());
const std::vector< DMatchVector >& _raw_matches,
KeyPointVector& _inlier_kps1, std::vector<Scalar> filterByDistance(const KeyPointVector& _raw_kps1,
KeyPointVector& _inlier_kps2, const KeyPointVector& _raw_kps2,
DMatchVector& _inlier_matches); 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 // Identify good matches using RANSAC
void ransacTest(const KeyPointVector& _raw_kps1, std::vector<Scalar> ransacTest(const KeyPointVector& _raw_kps1,
const KeyPointVector& _raw_kps2, const KeyPointVector& _raw_kps2,
const DMatchVector& _raw_matches, cv::Mat& _raw_desc1,
KeyPointVector& _inlier_kps1, cv::Mat& _raw_desc2,
KeyPointVector& _inlier_kps2, const DMatchVector& _raw_matches,
DMatchVector& _inlier_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 // Check symmetry between nn matches from <im1->im2> and from <im2->im1> pairs
void nnSymmetryTest(const std::vector<DMatchVector>& _matches1, void nnSymmetryTest(const std::vector<DMatchVector>& _matches1,
......
src/test/gtest_matcher_base.cpp
+ 12
− 8
View file @ 8c55a392
...@@ -91,15 +91,17 @@ TEST(MatcherBase, filterByDistance) ...@@ -91,15 +91,17 @@ TEST(MatcherBase, filterByDistance)
ASSERT_EQ(params_ptr->match_type,1); ASSERT_EQ(params_ptr->match_type,1);
// match // match
KeyPointVector kpts1_matched, kpts2_matched;
cv::Mat desc1_matched, desc2_matched;
DMatchVector matches; DMatchVector matches;
mat_ptr->match(desc1,desc2,des_ptr->getSize(),matches); mat_ptr->match(desc1,desc2,des_ptr->getSize(),matches);
ASSERT_TRUE(matches.size()-75>0); ASSERT_TRUE(matches.size()-75>0);
KeyPointVector inlier_kpts1; KeyPointVector inlier_kpts1, inlier_kpts2;
KeyPointVector inlier_kpts2; cv::Mat inlier_desc1, inlier_desc2;
DMatchVector inlier_matches; DMatchVector inlier_matches;
mat_ptr->filterByDistance(kpts1, kpts2, matches, mat_ptr->filterByDistance(kpts1, kpts2, desc1, desc2, matches,
inlier_kpts1, inlier_kpts2, inlier_matches); inlier_kpts1, inlier_kpts2, inlier_desc1, inlier_desc2, inlier_matches);
ASSERT_TRUE(inlier_matches.size()<40); ASSERT_TRUE(inlier_matches.size()<40);
} }
...@@ -147,15 +149,17 @@ TEST(MatcherBase, ransacTest) ...@@ -147,15 +149,17 @@ TEST(MatcherBase, ransacTest)
ASSERT_EQ(params_ptr->match_type,1); ASSERT_EQ(params_ptr->match_type,1);
// match // match
KeyPointVector kpts1_matched, kpts2_matched;
cv::Mat desc1_matched, desc2_matched;
DMatchVector matches; DMatchVector matches;
mat_ptr->match(desc1,desc2,des_ptr->getSize(),matches); mat_ptr->match(desc1,desc2,des_ptr->getSize(),matches);
ASSERT_TRUE(matches.size()-75>0); ASSERT_TRUE(matches.size()-75>0);
KeyPointVector inlier_kpts1; KeyPointVector inlier_kpts1, inlier_kpts2;
KeyPointVector inlier_kpts2; cv::Mat inlier_desc1, inlier_desc2;
DMatchVector inlier_matches; DMatchVector inlier_matches;
mat_ptr->ransacTest(kpts1, kpts2, matches, mat_ptr->ransacTest(kpts1, kpts2, desc1, desc2, matches,
inlier_kpts1, inlier_kpts2, inlier_matches); inlier_kpts1, inlier_kpts2, inlier_desc1, inlier_desc2, inlier_matches);
ASSERT_TRUE(inlier_matches.size()<50); ASSERT_TRUE(inlier_matches.size()<50);
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment