FLANNMatcher.cpp
Go to the documentation of this file.
00001 /*******************************************************************************
00002  *  FLANNMatcher.cpp
00003  *
00004  *  (C) 2007 AG Aktives Sehen <agas@uni-koblenz.de>
00005  *           Universitaet Koblenz-Landau
00006  *
00007  *  Additional information:
00008  *  $Id: $
00009  *******************************************************************************/
00010 
00011 #include "FLANNMatcher.h"
00012 
00013 #include "Architecture/Singleton/Clock.h"
00014 
00015 #include "Workers/Math/vec2.h"
00016 
00017 #include <assert.h>
00018 #include <map>
00019 #include <list>
00020 #include <math.h>
00021 
00022 using namespace std;
00023 
00024 #define THIS FLANNMatcher
00025 
00026 THIS::THIS ()
00027 {
00028     m_hasIndex = false;
00029     m_descriptorLength = 64; // May differ, set when building an index
00030 
00031     getFlannParameters().target_precision = 0.95;
00032     getFlannParameters().log_level = FLANN_LOG_INFO;
00033     getFlannParameters().algorithm = KDTREE;
00034     getFlannParameters().checks = 32;
00035     getFlannParameters().trees = 8;
00036     getFlannParameters().branching = 32;
00037     getFlannParameters().iterations = 7;
00038     getFlannParameters().target_precision = -1;
00039 
00040     m_FlannModelData = 0;
00041 
00042     m_Log << "FLANNMatcher created\n";
00043 }
00044 
00045 THIS::THIS ( const FLANNMatcher& other )
00046 {
00047   operator= ( other );
00048 }
00049 
00050 THIS& THIS::operator= ( const FLANNMatcher& other )
00051 {
00052   if ( this == &other )
00053   {
00054     return *this;
00055   }
00056 
00057   m_Matches = other.m_Matches;
00058 
00059   //m_Log = other.m_Log;
00060 
00061   m_flannIndex = other.m_flannIndex;
00062   m_flannParams = other.m_flannParams;
00063   m_hasIndex = other.m_hasIndex;
00064   m_descriptorLength = other.m_descriptorLength;
00065 
00066   return *this;
00067 }
00068 
00069 THIS::~THIS()
00070 {
00071   clearFLANNMembers();
00072   delete[] m_FlannModelData;
00073 }
00074 
00075 void THIS::createIndex( std::vector< KeyPoint >* keyPoints ){
00076   if(keyPoints->size()==0)
00077   {
00078     ROS_ERROR_STREAM("Cannot create index, because there are no keypoints.");
00079     return;
00080   }
00081   clearFLANNMembers();
00082   if(keyPoints->size() != 0){
00083     m_descriptorLength = keyPoints->at(0).featureVector.size();
00084   }
00085 
00086   int numFeatures = keyPoints->size();
00087   if(m_FlannModelData!=0)
00088   {
00089     delete[] m_FlannModelData;
00090 }
00091   m_FlannModelData = new float[numFeatures*m_descriptorLength];
00092   fillFlannDataWithDescriptors(keyPoints, m_FlannModelData);
00093   float speedup = 0.0;
00094   m_flannIndex = flann_build_index(m_FlannModelData, numFeatures, m_descriptorLength, &speedup, &m_flannParams);
00095   m_hasIndex = true;
00096 }
00097 
00098 void THIS::match (  std::vector< KeyPoint >* keyPoints, float maxDistRatio )
00099 {
00100     m_Matches.clear();
00101     if ( ( keyPoints->size() ==0 ) || ( m_Matches.size() !=0 ) || (!m_hasIndex) )
00102     {
00103         ROS_ERROR_STREAM("Cannot match features.");
00104 
00105         if ( keyPoints->size() ==0 )
00106             ROS_ERROR_STREAM("Key Points Size is 0.");
00107         if ( m_Matches.size() !=0 )
00108             ROS_ERROR_STREAM("Matches not 0.");
00109         if (!m_hasIndex)
00110             ROS_ERROR_STREAM("No Index.");
00111 
00112         return;
00113     }
00114 
00115     int startTime = Clock::getInstance()->getTimestamp();
00116 
00117     unsigned int numKeypoints = keyPoints->size();
00118     int numberOfNeighbours = 2; // Number of approximate nearest neigbours to be determined
00119     int* indices = new int[numberOfNeighbours*numKeypoints]; // Holds the indices of the nearest neighbours
00120     float* distances = new float[numberOfNeighbours*numKeypoints]; // Holds the distance to the 1st and 2nd nearest neighbour
00121     float* testset = new float[numKeypoints*m_descriptorLength]; // Contains the data of the descriptors to be matched
00122     fillFlannDataWithDescriptors(keyPoints, testset);
00123     flann_find_nearest_neighbors_index(m_flannIndex, testset, numKeypoints, indices, distances, numberOfNeighbours, &m_flannParams );
00124 
00125 
00126     // check for distance ratios
00127     double distanceRatio = 0.0;
00128     for(unsigned i = 0; i < numKeypoints; i++){
00129       distanceRatio = distances[i*2+0] / distances[i*2+1];    
00130       if(distanceRatio < maxDistRatio ){
00131           KeyPointMatch match={ indices[i*2+0], i,distanceRatio, 0, 0 };
00132         m_Matches.push_back ( match );
00133       }  
00134     }
00135     delete[] indices;
00136     delete[] distances;
00137     delete[] testset;
00138     m_Log << "\n--- " << m_Matches.size() << " keypoints matched in first phase in " << ( Clock::getInstance()->getTimestamp() - startTime ) << "ms\n";
00139 
00140 //    eliminateMultipleMatches();
00141 }
00142 
00143 
00144 void THIS::eliminateMultipleMatches()
00145 {
00146     //It is possible that more than one ipoint in First has been matched to
00147     //the same ipoint in Second, in this case eliminate all but the closest one
00148 
00149   int startTime = Clock::getInstance()->getTimestamp();
00150 
00151     //maps keypoints in Second to their closest match result
00152     //first: index in m_KeyPointsB
00153     //second: iterator in m_Matches
00154     map< unsigned, MatchElem > bestMatch;
00155 
00156     m_Log << "deleting ";
00157 
00158     MatchElem currentMatch=m_Matches.begin();
00159     while ( currentMatch != m_Matches.end() )
00160     {
00161         unsigned index2=currentMatch->index2;
00162 
00163         //check if a match with this keypoint in Second was found before
00164         map< unsigned, MatchElem >::iterator previous=bestMatch.find ( index2 );
00165 
00166         //this is the first match found which maps to current second index
00167         if ( previous == bestMatch.end() )
00168         {
00169             bestMatch[ index2 ] = currentMatch;
00170             currentMatch++;
00171             continue;
00172         }
00173 
00174         MatchElem previousMatch = previous->second;
00175         //a match mapping to this index in second has been found previously, and had a higher distance
00176         //so delete the previously found match
00177         if ( currentMatch->distance < previousMatch->distance )
00178         {
00179             m_Log << previousMatch->index1 << "->" << previousMatch->index2;
00180             m_Log << " (better:" << currentMatch->index1 << "->" << currentMatch->index2 << ")  ";
00181             m_Matches.erase ( previousMatch );
00182             bestMatch[ index2 ] = currentMatch;
00183             currentMatch++;
00184             continue;
00185         }
00186         //otherwise, the previously found best match is better than current,
00187         //so delete current
00188         m_Log << currentMatch->index1 << "->" << currentMatch->index2;
00189         m_Log << " (better:" << previousMatch->index1 << "->" << previousMatch->index2 << ")  ";
00190         currentMatch=m_Matches.erase ( currentMatch );
00191     }
00192     m_Log << "\n--- " << m_Matches.size() << " remaining after multiple match elimination in " << ( Clock::getInstance()->getTimestamp() - startTime ) << "ms\n";
00193 
00194 }
00195 
00196 string THIS::getLog()
00197 {
00198   string log = m_Log.str();
00199   m_Log.str("");
00200   return log;
00201 }
00202 
00203 
00204 
00205 void THIS::fillFlannDataWithDescriptors(const std::vector< KeyPoint >* features, float* flannDataPtr){
00206   for(unsigned int i = 0; i < features->size(); i++){
00207     for(unsigned int j = 0; j < m_descriptorLength; j++){
00208       flannDataPtr[m_descriptorLength*i + j] = features->at(i).featureVector[j];
00209     }
00210   }
00211 }
00212 
00213 void THIS::clearFLANNMembers(){
00214   if(m_hasIndex){
00215     flann_free_index(m_flannIndex, &m_flannParams);
00216     m_hasIndex = false;
00217   }
00218 
00219 }
00220 
00221 
00222 #undef THIS


or_libs
Author(s): raphael
autogenerated on Mon Oct 6 2014 02:53:18