flirtlib: RansacFeatureSetMatcher.cpp Source File

00001 #include "RansacFeatureSetMatcher.h"
00002 
00003 #include <boost/random.hpp>
00004 #include <boost/random/uniform_smallint.hpp>
00005 #include <sys/time.h>
00006 
00007 
00008 RansacFeatureSetMatcher::RansacFeatureSetMatcher(double acceptanceThreshold, double successProbability, double inlierProbability, double distanceThreshold, double rigidityThreshold, bool adaptive):
00009     AbstractFeatureSetMatcher(acceptanceThreshold),
00010     m_successProbability(successProbability),
00011     m_inlierProbability(inlierProbability),
00012     m_distanceThreshold(distanceThreshold),
00013     m_rigidityThreshold(rigidityThreshold),
00014     m_adaptive(adaptive)
00015 {
00016 
00017 }
00018 
00019 double RansacFeatureSetMatcher::matchSets(const std::vector<InterestPoint *> &reference, const std::vector<InterestPoint *> &data, OrientedPoint2D& transformation) const
00020 {
00021     std::vector< std::pair<InterestPoint *, InterestPoint *> > correspondences;
00022     return matchSets(reference, data, transformation, correspondences);
00023 }
00024 
00025 double RansacFeatureSetMatcher::matchSets(const std::vector<InterestPoint *> &reference, const std::vector<InterestPoint *> &data, OrientedPoint2D& transformation,
00026                                           std::vector< std::pair<InterestPoint *, InterestPoint *> > &correspondences) const
00027 {
00028     correspondences.clear();
00029     unsigned int iterations = m_adaptive ? 1e17 : ceil(log(1. - m_successProbability)/log(1. - m_inlierProbability * m_inlierProbability));
00030     
00031     // Compute possible correspondences based on NN thresholding
00032    std::vector< std::pair<InterestPoint *, InterestPoint *> > possibleCorrespondences;
00033    for(unsigned int i = 0; i < data.size(); i++){
00034         double minCorrespondenceDistance = 1e17;
00035         unsigned int minCorrespondenceIndex = 0;
00036         for(unsigned int j = 0; j < reference.size(); j++){
00037             double distance = data[i]->getDescriptor()->distance(reference[j]->getDescriptor());
00038             if(distance < minCorrespondenceDistance){
00039                 minCorrespondenceDistance = distance;
00040                 minCorrespondenceIndex = j;
00041             }
00042         }
00043         if(minCorrespondenceDistance < m_distanceThreshold){
00044             possibleCorrespondences.push_back(std::make_pair(data[i], reference[minCorrespondenceIndex]));
00045         }
00046     }
00047     
00048     // Check if there are enough absolute matches 
00049     if(possibleCorrespondences.size() < 2){  
00050         return 1e17;
00051     }
00052     
00053     // Check if there are enough matches compared to the inlier probability (FIXME maybe is better to adjust the inlier probability)
00054     if(double(possibleCorrespondences.size()) * m_inlierProbability < 2){  
00055         return 1e17;
00056     }
00057     
00058     boost::mt19937 rng;
00059     boost::uniform_smallint<int> generator(0, possibleCorrespondences.size() - 1);
00060     
00061     // Main loop
00062     double minimumScore = 1e17;
00063     for(unsigned int i = 0; i < iterations; i++){
00064         unsigned int first = generator(rng);
00065         unsigned int second = generator(rng);
00066         while(second == first) second = generator(rng); // avoid useless samples
00067         std::pair< std::pair<InterestPoint *, InterestPoint *>, std::pair<InterestPoint *, InterestPoint *> > minimumSampleSet(possibleCorrespondences[first], possibleCorrespondences[second]);
00068         
00069         // Test rigidity
00070         const Point2D& diffFirst = possibleCorrespondences[first].first->getPosition() - possibleCorrespondences[second].first->getPosition();
00071         const Point2D& diffSecond = possibleCorrespondences[first].second->getPosition() - possibleCorrespondences[second].second->getPosition();
00072         double distanceFirst = diffFirst * diffFirst;
00073         double distanceSecond = diffSecond * diffSecond;
00074         if((distanceFirst - distanceSecond)*(distanceFirst - distanceSecond)/(8*(distanceFirst + distanceSecond )) > m_rigidityThreshold){
00075             continue;
00076         }
00077         
00078         // Compute hypothesis
00079         std::vector< std::pair<InterestPoint *, InterestPoint *> > inlierSet;
00080         OrientedPoint2D hypothesis = generateHypothesis(minimumSampleSet);
00081         
00082         // Verify hypothesis
00083         double score = verifyHypothesis(reference, data, hypothesis, inlierSet);
00084         if(score < minimumScore){
00085             minimumScore = score;
00086             transformation = hypothesis;
00087             correspondences = inlierSet;
00088             
00089             // Adapt the number of iterations
00090             if (m_adaptive){
00091                 double inlierProbability = double(correspondences.size())/double(data.size());
00092                 iterations = ceil(log(1. - m_successProbability)/log(1. - inlierProbability * inlierProbability));
00093             }
00094         }
00095     }
00096     std::vector<std::pair<Point2D, Point2D> > pointCorrespondences(correspondences.size());
00097     for(unsigned int i = 0; i < correspondences.size(); i++){
00098         pointCorrespondences[i] = std::make_pair(correspondences[i].first->getPosition(), correspondences[i].second->getPosition());
00099     }
00100     compute2DPose(pointCorrespondences, transformation);
00101 
00102     return verifyHypothesis(reference, data, transformation, correspondences);
00103 }