235 lines
9 KiB
Text
235 lines
9 KiB
Text
|
/*
|
||
|
|
* 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;
|
||
|
|
//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);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if(mMethod == BRUTEFORCE){
|
||
|
|
cv::BruteForceMatcher< cv::L2<float> > matcher;
|
||
|
|
matcher.match(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatches);
|
||
|
|
matcher.match(tFeature.getDescriptors(), qFeature.getDescriptors(), tqInitialMatches);
|
||
|
|
}
|
||
|
|
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){
|
||
|
|
cv::BruteForceMatcher< cv::Hamming > matcher;
|
||
|
|
matcher.knnMatch(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatchesVector, knn);
|
||
|
|
matcher.knnMatch(tFeature.getDescriptors(), qFeature.getDescriptors(), tqInitialMatchesVector, knn);
|
||
|
|
}
|
||
|
|
if(mMethod == RADIUS){
|
||
|
|
cv::BruteForceMatcher< cv::Hamming > matcher;
|
||
|
|
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){
|
||
|
|
cv::BruteForceMatcher< cv::Hamming > 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);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if(mMethod == BRUTEFORCE){
|
||
|
|
cv::BruteForceMatcher< cv::Hamming > matcher;
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
}
|