2012-09-16 12:33:11 +00:00
/*
* feature_mathcer implementation
*
* Copyright ( C ) HamidReza Houshiar
*
* Released under the GPL version 3.
*
*/
# include "slam6d/fbr/feature_matcher.h"
using namespace std ;
namespace fbr {
void feature_matcher : : init ( matcher_method method , int k , double r ) {
mMethod = method ;
knn = k ;
radius = r ;
nOfMatches = 0 ;
nOfFilteredMatches = 0 ;
}
feature_matcher : : feature_matcher ( ) {
init ( RATIO , 0 , 0 ) ;
}
feature_matcher : : feature_matcher ( matcher_method method ) {
if ( method = = KNN )
init ( method , 3 , 0 ) ;
else if ( method = = RADIUS )
init ( method , 0 , 1 ) ;
else
init ( method , 0 , 0 ) ;
}
feature_matcher : : feature_matcher ( matcher_method method , double p ) {
if ( method = = KNN )
init ( method , p , 0 ) ;
else if ( method = = RADIUS )
init ( method , 0 , p ) ;
else
init ( method , 0 , 0 ) ;
}
void feature_matcher : : match ( feature qFeature , feature tFeature ) {
vector < cv : : DMatch > qtInitialMatches , tqInitialMatches , gMatches ;
vector < vector < cv : : DMatch > > qtInitialMatchesVector , tqInitialMatchesVector ;
2012-10-15 10:08:21 +00:00
if ( qFeature . getFeatures ( ) . size ( ) = = 0 | | tFeature . getFeatures ( ) . size ( ) = = 0 ) {
cout < < " No features has found in one or both scans!! " < < endl ;
exit ( - 1 ) ;
}
2012-09-16 12:33:11 +00:00
//Matching descriptors using one of the mMethods for SURF and SIFT feature descriptors
if ( qFeature . getDescriptorMethod ( ) ! = tFeature . getDescriptorMethod ( ) ) {
cout < < " inputs features don't have the same descriptors! " < < endl ;
cout < < " qFeature DescriptorMethod= " < < qFeature . getDescriptorMethod ( ) < < endl ;
cout < < " tFeature DescriptorMethod= " < < tFeature . getDescriptorMethod ( ) < < endl ;
} else if ( qFeature . getDescriptorMethod ( ) = = SURF_DES | | qFeature . getDescriptorMethod ( ) = = SIFT_DES ) {
if ( mMethod = = KNN ) {
cv : : FlannBasedMatcher matcher ;
matcher . knnMatch ( qFeature . getDescriptors ( ) , tFeature . getDescriptors ( ) , qtInitialMatchesVector , knn ) ;
matcher . knnMatch ( tFeature . getDescriptors ( ) , qFeature . getDescriptors ( ) , tqInitialMatchesVector , knn ) ;
}
if ( mMethod = = RADIUS ) {
cv : : FlannBasedMatcher matcher ;
matcher . radiusMatch ( qFeature . getDescriptors ( ) , tFeature . getDescriptors ( ) , qtInitialMatchesVector , radius ) ;
matcher . radiusMatch ( tFeature . getDescriptors ( ) , qFeature . getDescriptors ( ) , tqInitialMatchesVector , radius ) ;
}
if ( mMethod = = KNN | | mMethod = = RADIUS ) {
//find the matches that has been found in both way
for ( unsigned int i = 0 ; i < qtInitialMatchesVector . size ( ) ; i + + ) {
for ( unsigned int j = 0 ; j < qtInitialMatchesVector [ i ] . size ( ) ; j + + ) {
cv : : DMatch forward = qtInitialMatchesVector [ i ] [ j ] ;
for ( unsigned int k = 0 ; k < tqInitialMatchesVector [ forward . trainIdx ] . size ( ) ; k + + ) {
cv : : DMatch backward = tqInitialMatchesVector [ forward . trainIdx ] [ k ] ;
if ( backward . trainIdx = = forward . queryIdx )
matches . push_back ( forward ) ;
}
//matches.push_back(qtInitialMatchesVector[i][j]);
}
}
}
if ( mMethod = = RATIO ) {
cv : : FlannBasedMatcher matcher ;
matcher . knnMatch ( qFeature . getDescriptors ( ) , tFeature . getDescriptors ( ) , qtInitialMatchesVector , 2 ) ;
for ( unsigned int i = 0 ; i < qtInitialMatchesVector . size ( ) ; i + + ) {
float ratio = qtInitialMatchesVector [ i ] [ 0 ] . distance / qtInitialMatchesVector [ i ] [ 1 ] . distance ;
if ( ratio < 0.8 )
matches . push_back ( qtInitialMatchesVector [ i ] [ 0 ] ) ;
}
matcher . knnMatch ( tFeature . getDescriptors ( ) , qFeature . getDescriptors ( ) , tqInitialMatchesVector , 2 ) ;
for ( unsigned int i = 0 ; i < tqInitialMatchesVector . size ( ) ; i + + ) {
float ratio = tqInitialMatchesVector [ i ] [ 0 ] . distance / tqInitialMatchesVector [ i ] [ 1 ] . distance ;
if ( ratio < 0.8 ) {
cv : : DMatch tq_qt ;
tq_qt . queryIdx = tqInitialMatchesVector [ i ] [ 0 ] . trainIdx ;
tq_qt . trainIdx = tqInitialMatchesVector [ i ] [ 0 ] . queryIdx ;
tq_qt . imgIdx = tqInitialMatchesVector [ i ] [ 0 ] . imgIdx ;
tq_qt . distance = tqInitialMatchesVector [ i ] [ 0 ] . distance ;
matches . push_back ( tq_qt ) ;
}
}
}
2012-10-15 10:08:21 +00:00
if ( mMethod = = BRUTEFORCE ) {
//opencv 2.4
# if (CV_MAJOR_VERSION >= 2) && (CV_MINOR_VERSION >= 4)
cv : : BFMatcher matcher ( cv : : NORM_L2 ) ;
# else //older version of opencv than 2.4
2012-09-16 12:33:11 +00:00
cv : : BruteForceMatcher < cv : : L2 < float > > matcher ;
2012-10-15 10:08:21 +00:00
# endif
2012-09-16 12:33:11 +00:00
matcher . match ( qFeature . getDescriptors ( ) , tFeature . getDescriptors ( ) , qtInitialMatches ) ;
matcher . match ( tFeature . getDescriptors ( ) , qFeature . getDescriptors ( ) , tqInitialMatches ) ;
2012-10-15 10:08:21 +00:00
}
2012-09-16 12:33:11 +00:00
if ( mMethod = = FLANN ) {
cv : : FlannBasedMatcher matcher ;
matcher . match ( qFeature . getDescriptors ( ) , tFeature . getDescriptors ( ) , qtInitialMatches ) ;
matcher . match ( tFeature . getDescriptors ( ) , qFeature . getDescriptors ( ) , tqInitialMatches ) ;
}
if ( mMethod = = FLANN | | mMethod = = BRUTEFORCE )
{
//add the intersection of both way matches
for ( unsigned int i = 0 ; i < qtInitialMatches . size ( ) ; i + + ) {
for ( unsigned int j = 0 ; j < tqInitialMatches . size ( ) ; j + + ) {
if ( qtInitialMatches [ i ] . queryIdx = = tqInitialMatches [ j ] . trainIdx & & qtInitialMatches [ i ] . trainIdx = = tqInitialMatches [ j ] . queryIdx ) {
matches . push_back ( qtInitialMatches [ i ] ) ;
}
}
}
}
}
//Matching descriptors using BruteFore with Hamming distance for ORB descriptor
else if ( qFeature . getDescriptorMethod ( ) = = ORB_DES ) {
if ( mMethod = = KNN ) {
2012-10-15 10:08:21 +00:00
//opencv 2.4
# if (CV_MAJOR_VERSION >= 2) && (CV_MINOR_VERSION >= 4)
cv : : BFMatcher matcher ( cv : : NORM_HAMMING ) ;
# else //older version of opencv than 2.4
2012-09-16 12:33:11 +00:00
cv : : BruteForceMatcher < cv : : Hamming > matcher ;
2012-10-15 10:08:21 +00:00
# endif
2012-09-16 12:33:11 +00:00
matcher . knnMatch ( qFeature . getDescriptors ( ) , tFeature . getDescriptors ( ) , qtInitialMatchesVector , knn ) ;
matcher . knnMatch ( tFeature . getDescriptors ( ) , qFeature . getDescriptors ( ) , tqInitialMatchesVector , knn ) ;
}
if ( mMethod = = RADIUS ) {
2012-10-15 10:08:21 +00:00
//opencv 2.4
# if (CV_MAJOR_VERSION >= 2) && (CV_MINOR_VERSION >= 4)
cv : : BFMatcher matcher ( cv : : NORM_HAMMING ) ;
# else //older version of opencv than 2.4
2012-09-16 12:33:11 +00:00
cv : : BruteForceMatcher < cv : : Hamming > matcher ;
2012-10-15 10:08:21 +00:00
# endif
2012-09-16 12:33:11 +00:00
matcher . radiusMatch ( qFeature . getDescriptors ( ) , tFeature . getDescriptors ( ) , qtInitialMatchesVector , radius ) ;
matcher . radiusMatch ( tFeature . getDescriptors ( ) , qFeature . getDescriptors ( ) , tqInitialMatchesVector , radius ) ;
}
if ( mMethod = = KNN | | mMethod = = RADIUS ) {
for ( unsigned int i = 0 ; i < qtInitialMatchesVector . size ( ) ; i + + ) {
for ( unsigned int j = 0 ; j < qtInitialMatchesVector [ i ] . size ( ) ; j + + ) {
cv : : DMatch forward = qtInitialMatchesVector [ i ] [ j ] ;
for ( unsigned int k = 0 ; k < tqInitialMatchesVector [ forward . trainIdx ] . size ( ) ; k + + ) {
cv : : DMatch backward = tqInitialMatchesVector [ forward . trainIdx ] [ k ] ;
if ( backward . trainIdx = = forward . queryIdx )
matches . push_back ( forward ) ;
}
//matches.push_back(qtInitialMatchesVector[i][j]);
}
}
}
if ( mMethod = = RATIO ) {
2012-10-15 10:08:21 +00:00
//opencv 2.4
# if (CV_MAJOR_VERSION >= 2) && (CV_MINOR_VERSION >= 4)
cv : : BFMatcher matcher ( cv : : NORM_HAMMING ) ;
# else //older version of opencv than 2.4
2012-09-16 12:33:11 +00:00
cv : : BruteForceMatcher < cv : : Hamming > matcher ;
2012-10-15 10:08:21 +00:00
# endif
2012-09-16 12:33:11 +00:00
matcher . knnMatch ( qFeature . getDescriptors ( ) , tFeature . getDescriptors ( ) , qtInitialMatchesVector , 2 ) ;
for ( unsigned int i = 0 ; i < qtInitialMatchesVector . size ( ) ; i + + ) {
float ratio = qtInitialMatchesVector [ i ] [ 0 ] . distance / qtInitialMatchesVector [ i ] [ 1 ] . distance ;
if ( ratio < 0.8 )
matches . push_back ( qtInitialMatchesVector [ i ] [ 0 ] ) ;
}
matcher . knnMatch ( tFeature . getDescriptors ( ) , qFeature . getDescriptors ( ) , tqInitialMatchesVector , 2 ) ;
for ( unsigned int i = 0 ; i < tqInitialMatchesVector . size ( ) ; i + + ) {
float ratio = tqInitialMatchesVector [ i ] [ 0 ] . distance / tqInitialMatchesVector [ i ] [ 1 ] . distance ;
if ( ratio < 0.8 ) {
cv : : DMatch tq_qt ;
tq_qt . queryIdx = tqInitialMatchesVector [ i ] [ 0 ] . trainIdx ;
tq_qt . trainIdx = tqInitialMatchesVector [ i ] [ 0 ] . queryIdx ;
tq_qt . imgIdx = tqInitialMatchesVector [ i ] [ 0 ] . imgIdx ;
tq_qt . distance = tqInitialMatchesVector [ i ] [ 0 ] . distance ;
matches . push_back ( tq_qt ) ;
}
}
}
if ( mMethod = = BRUTEFORCE ) {
2012-10-15 10:08:21 +00:00
//opencv 2.4
# if (CV_MAJOR_VERSION >= 2) && (CV_MINOR_VERSION >= 4)
cv : : BFMatcher matcher ( cv : : NORM_HAMMING ) ;
# else //older version of opencv than 2.4
2012-09-16 12:33:11 +00:00
cv : : BruteForceMatcher < cv : : Hamming > matcher ;
2012-10-15 10:08:21 +00:00
# endif
2012-09-16 12:33:11 +00:00
matcher . match ( qFeature . getDescriptors ( ) , tFeature . getDescriptors ( ) , qtInitialMatches ) ;
matcher . match ( tFeature . getDescriptors ( ) , qFeature . getDescriptors ( ) , tqInitialMatches ) ;
for ( unsigned int i = 0 ; i < qtInitialMatches . size ( ) ; i + + ) {
for ( unsigned int j = 0 ; j < tqInitialMatches . size ( ) ; j + + ) {
if ( qtInitialMatches [ i ] . queryIdx = = tqInitialMatches [ j ] . trainIdx & & qtInitialMatches [ i ] . trainIdx = = tqInitialMatches [ j ] . queryIdx ) {
matches . push_back ( qtInitialMatches [ i ] ) ;
}
}
}
}
}
//filter the matches with RANSAC and FundementalMatrix
vector < cv : : Point2f > points_1 , points_2 ;
for ( unsigned int i = 0 ; i < matches . size ( ) ; i + + ) {
//Get the keypoints from the intersection of both matches
points_1 . push_back ( qFeature . getFeatures ( ) [ matches [ i ] . queryIdx ] . pt ) ;
points_2 . push_back ( tFeature . getFeatures ( ) [ matches [ i ] . trainIdx ] . pt ) ;
}
//calculating the fundemental matrix
cv : : Mat fStatus ;
cv : : Mat fundementalMatrix = findFundamentalMat ( points_1 , points_2 , cv : : FM_RANSAC , 3 , 0.99 , fStatus ) ;
cv : : MatIterator_ < uchar > it , end ;
int counter = 0 ;
//get the inliers from fundemental matrix
for ( it = fStatus . begin < uchar > ( ) , end = fStatus . end < uchar > ( ) ; it ! = end ; + + it ) {
if ( * it = = 1 )
gMatches . push_back ( matches [ counter ] ) ;
counter + + ;
}
nOfMatches = matches . size ( ) ;
matches = gMatches ;
nOfFilteredMatches = matches . size ( ) ;
}
vector < cv : : DMatch > feature_matcher : : getMatches ( ) {
return matches ;
}
2012-10-15 10:08:21 +00:00
2012-09-16 12:33:11 +00:00
matcher_method feature_matcher : : getMatcherMethod ( ) {
return mMethod ;
}
unsigned int feature_matcher : : getKnn ( ) {
return knn ;
}
double feature_matcher : : getRadius ( ) {
return radius ;
}
unsigned int feature_matcher : : getNumberOfMatches ( ) {
return nOfMatches ;
}
unsigned int feature_matcher : : getNumberOfFilteredMatches ( ) {
return nOfFilteredMatches ;
}
void feature_matcher : : getDescription ( ) {
cout < < " number of Matches: " < < nOfMatches < < " , number of Matches after filteration: " < < nOfFilteredMatches < < " , matching method: " < < matcherMethodToString ( mMethod ) < < " , knn: " < < knn < < " , radius: " < < radius < < " . " < < endl ;
cout < < endl ;
}
}