ransac.h

Go to the documentation of this file.
00001 // -*- c-basic-offset: 4 -*-
00028 #ifndef _RANSAC_H_
00029 #define _RANSAC_H_
00030 
00031 #include <set>
00032 #include <vector>
00033 #include <stdlib.h>
00034 #include <cstring>
00035 #include <math.h>
00036 #include <ctime>
00037 
00038 #include "hugin_config.h"
00039 #ifdef HAVE_CXX11
00040 #include <random>
00041 #include <functional>
00042 #else
00043 #include <boost/random/mersenne_twister.hpp>
00044 #include <boost/random/uniform_int.hpp>
00045 #include <boost/random/variate_generator.hpp>
00046 #endif
00047 
00048 //#include "ParameterEsitmator.h"
00049 
00050 //#define DEBUG_RANSAC
00051 
00081 class Ransac {
00082 
00083 public:
00100         template<class Estimator, class S, class T>
00101         static std::vector<const T*> compute(S & parameters,
00102                                              std::vector<int> & inliers,
00103                                              const Estimator & paramEstimator ,
00104                                              const std::vector<T> &data, 
00105                                              double desiredProbabilityForNoOutliers,
00106                                              double maximalOutlierPercentage);
00107 
00108 
00131         template<class Estimator, class S, class T>
00132         static std::vector<const T*> compute(S &parameters, 
00133                                              const Estimator & paramEstimator ,
00134                                              const std::vector<T> &data);
00135         
00136 private:
00137 
00141     static unsigned int choose(unsigned int n, unsigned int m);
00142 
00143     template<class Estimator, class T>
00144     static void computeAllChoices(const Estimator & paramEstimator,
00145                                   const std::vector<T> &data,
00146                                   int numForEstimate,
00147                                   short *bestVotes, short *curVotes,
00148                                   int &numVotesForBest, int startIndex,
00149                                   int n, int k, int arrIndex, int *arr);
00150 
00151 
00152     template<class Estimator, class T, class S>
00153     static void estimate(const Estimator & paramEstimator, const std::vector<T> &data,
00154                          int numForEstimate,
00155                          short *bestVotes, short *curVotes,
00156                          int &numVotesForBest, int *arr);
00157 
00158     class SubSetIndexComparator 
00159     {
00160     private:
00161         int m_length;
00162         public:
00163             SubSetIndexComparator(int arrayLength) : m_length(arrayLength)
00164             {}
00165 
00166         bool operator()(const int *arr1, const int *arr2) const 
00167         {
00168             for(int i=0; i<m_length; i++)
00169                 if(arr1[i] < arr2[i])
00170                     return true;
00171                 return false;                   
00172         }
00173     };
00174 
00175 };
00176 
00177 
00178 /*******************************RanSac Implementation*************************/
00179 
00180 template<class Estimator, class S, class T>
00181 std::vector<const T *> Ransac::compute(S &parameters,
00182                                        std::vector<int> & inliers,
00183                                        const Estimator & paramEstimator,
00184                                        const std::vector<T> &data,
00185                                        double desiredProbabilityForNoOutliers,
00186                                        double maximalOutlierPercentage)
00187 {
00188     unsigned int numDataObjects = (int) data.size();
00189     unsigned int numForEstimate = paramEstimator.numForEstimate();
00190     //there are less data objects than the minimum required for an exact fit, or
00191     //all the data is outliers?
00192     if(numDataObjects < numForEstimate || maximalOutlierPercentage>=1.0) 
00193         return std::vector<const T*>();
00194 
00195     std::vector<const T *> exactEstimateData;
00196     std::vector<const T *> leastSquaresEstimateData;
00197     S exactEstimateParameters;
00198     int i, j, k, l, numVotesForBest, numVotesForCur, maxIndex, numTries;
00199     short *bestVotes = new short[numDataObjects]; //one if data[i] agrees with the best model, otherwise zero
00200     short *curVotes = new short[numDataObjects];  //one if data[i] agrees with the current model, otherwise zero
00201     short *notChosen = new short[numDataObjects]; //not zero if data[i] is NOT chosen for computing the exact fit, otherwise zero
00202     SubSetIndexComparator subSetIndexComparator(numForEstimate);
00203     std::set<int *, SubSetIndexComparator > chosenSubSets(subSetIndexComparator);
00204     int *curSubSetIndexes;
00205     double outlierPercentage = maximalOutlierPercentage;
00206     double numerator = log(1.0-desiredProbabilityForNoOutliers);
00207     double denominator = log(1- pow((double)(1.0-maximalOutlierPercentage), (double)(numForEstimate)));
00208     int allTries = choose(numDataObjects,numForEstimate);
00209 
00210     //parameters.clear();
00211 
00212 
00213     numVotesForBest = 0; //initalize with 0 so that the first computation which gives any type of fit will be set to best
00214     
00215     // intialize random generator
00216     maxIndex = numDataObjects - 1;
00217 #ifdef HAVE_CXX11
00218     std::mt19937 rng(static_cast<unsigned int>(std::time(0)));
00219     std::uniform_int_distribution<> distribIndex(0, maxIndex);
00220     auto randIndex=std::bind(distribIndex, rng);
00221 #else
00222     boost::mt19937 rng;
00223     // start with a different seed every time.
00224     rng.seed(static_cast<unsigned int>(std::time(0)));
00225     // randomly sample points.
00226     boost::uniform_int<> distribIndex(0, maxIndex);
00227     boost::variate_generator<boost::mt19937&, boost::uniform_int<> >
00228                              randIndex(rng, distribIndex);  // glues randomness with mapping
00229 #endif
00230 
00231 //    srand((unsigned)time(NULL)); //seed random number generator
00232     numTries = (int)(numerator/denominator + 0.5);
00233 
00234     //there are cases when the probablistic number of tries is greater than all possible sub-sets
00235     numTries = numTries<allTries ? numTries : allTries;
00236 
00237     for(i=0; i<numTries; i++) {
00238         //randomly select data for exact model fit ('numForEstimate' objects).
00239         memset(notChosen,'1',numDataObjects*sizeof(short));
00240         curSubSetIndexes = new int[numForEstimate];
00241 
00242         exactEstimateData.clear();
00243 
00244         maxIndex = numDataObjects-1; 
00245         for(l=0; l<(int)numForEstimate; l++) {
00246             //selectedIndex is in [0,maxIndex]
00247             unsigned int selectedIndex = randIndex();
00248 //            unsigned int selectedIndex = (unsigned int)(((float)rand()/(float)RAND_MAX)*maxIndex + 0.5);
00249             for(j=-1,k=0; k<(int)numDataObjects && j<(int)selectedIndex; k++) {
00250                 if(notChosen[k])
00251                     j++;
00252             }
00253             k--;
00254             exactEstimateData.push_back(&(data[k]));
00255             notChosen[k] = 0;
00256             maxIndex--;
00257         }
00258         //get the indexes of the chosen objects so we can check that this sub-set hasn't been
00259         //chosen already
00260         for(l=0, j=0; j<(int)numDataObjects; j++) {
00261             if(!notChosen[j]) {
00262                 curSubSetIndexes[l] = j+1;
00263                 l++;
00264             }
00265         }
00266 
00267         //check that the sub set just chosen is unique
00268         std::pair< std::set<int *, SubSetIndexComparator >::iterator, bool > res = chosenSubSets.insert(curSubSetIndexes);
00269 
00270         if(res.second == true) { //first time we chose this sub set
00271                                  //use the selected data for an exact model parameter fit
00272             if (!paramEstimator.estimate(exactEstimateData,exactEstimateParameters))
00273                 //selected data is a singular configuration (e.g. three colinear points for 
00274                 //a circle fit)
00275                 continue;
00276             //see how many agree on this estimate
00277             numVotesForCur = 0;
00278             memset(curVotes,'\0',numDataObjects*sizeof(short));
00279             for(j=0; j<(int)numDataObjects; j++) {
00280                 if(paramEstimator.agree(exactEstimateParameters, data[j])) {
00281                     curVotes[j] = 1;
00282                     numVotesForCur++;
00283                 }
00284             }
00285             // debug output
00286             #ifdef DEBUG_RANSAC
00287             std::cerr << "RANSAC iter " << i << ": inliers: " << numVotesForCur << " parameters:";
00288             for (int jj=0; jj < exactEstimateParameters.size(); jj++)
00289                 std::cerr << " " << exactEstimateParameters[jj];
00290             std::cerr << std::endl;
00291             #endif
00292 
00293             if(numVotesForCur > numVotesForBest) {
00294                 numVotesForBest = numVotesForCur;
00295                 memcpy(bestVotes,curVotes, numDataObjects*sizeof(short));
00296                 parameters = exactEstimateParameters;
00297             }
00298             /*
00299             //update the estimate of outliers and the number of iterations we need
00300             outlierPercentage = 1 - (double)numVotesForCur/(double)numDataObjects;
00301             if(outlierPercentage < maximalOutlierPercentage) {
00302                 maximalOutlierPercentage = outlierPercentage;
00303                 denominator = log(1- pow((double)(1.0-maximalOutlierPercentage), (double)(numForEstimate)));
00304                 numTries = (int)(numerator/denominator + 0.5);
00305                 //there are cases when the probablistic number of tries is greater than all possible sub-sets
00306                 numTries = numTries<allTries ? numTries : allTries;
00307             }
00308             */
00309         }
00310         else {  //this sub set already appeared, don't count this iteration
00311             delete [] curSubSetIndexes;
00312             i--;
00313         }
00314     }
00315 
00316     //release the memory
00317     std::set<int *, SubSetIndexComparator >::iterator it = chosenSubSets.begin();
00318     std::set<int *, SubSetIndexComparator >::iterator chosenSubSetsEnd = chosenSubSets.end();
00319     while(it!=chosenSubSetsEnd) {
00320         delete [] (*it);
00321         it++;
00322     }
00323     chosenSubSets.clear();
00324 
00325     //compute the least squares estimate using the largest sub set
00326     if(numVotesForBest > 0) {
00327         for(j=0; j<(int)numDataObjects; j++) {
00328             if(bestVotes[j]) {
00329                 leastSquaresEstimateData.push_back(&(data[j]));
00330                 inliers.push_back(j);
00331             }
00332         }
00333         paramEstimator.leastSquaresEstimate(leastSquaresEstimateData,parameters);
00334     }
00335     delete [] bestVotes;
00336     delete [] curVotes;
00337     delete [] notChosen;
00338 
00339     return leastSquaresEstimateData;
00340 }
00341 /*****************************************************************************/
00342 template<class Estimator, class S, class T>
00343 std::vector<const T*> Ransac::compute(S &parameters,
00344                     const Estimator & paramEstimator,
00345                     const std::vector<T> &data)
00346 {
00347     unsigned int numForEstimate = paramEstimator.numForEstimate();
00348     std::vector<T *> leastSquaresEstimateData;
00349     int numDataObjects = data.size();
00350     int numVotesForBest = 0;
00351     int *arr = new int[numForEstimate];
00352     short *curVotes = new short[numDataObjects];  //one if data[i] agrees with the current model, otherwise zero
00353     short *bestVotes = new short[numDataObjects];  //one if data[i] agrees with the best model, otherwise zero
00354         
00355     //parameters.clear();
00356 
00357     //there are less data objects than the minimum required for an exact fit
00358     if(numDataObjects < numForEstimate) 
00359         return 0;
00360 
00361     computeAllChoices(paramEstimator,data,numForEstimate,
00362                       bestVotes, curVotes, numVotesForBest, 0, data.size(), numForEstimate, 0, arr);
00363 
00364     //compute the least squares estimate using the largest sub set
00365     if(numVotesForBest > 0) {
00366         for(int j=0; j<numDataObjects; j++) {
00367             if(bestVotes[j])
00368                 leastSquaresEstimateData.push_back(&(data[j]));
00369         }
00370         paramEstimator.leastSquaresEstimate(leastSquaresEstimateData,parameters);
00371     }
00372 
00373     delete [] arr;
00374     delete [] bestVotes;
00375     delete [] curVotes; 
00376 
00377     return leastSquaresEstimateData;
00378 }
00379 /*****************************************************************************/
00380 template<class Estimator, class T>
00381 void Ransac::computeAllChoices(const Estimator &paramEstimator, const std::vector<T> &data,
00382                                 int numForEstimate,short *bestVotes, short *curVotes,
00383                                 int &numVotesForBest, int startIndex, int n, int k,
00384                                 int arrIndex, int *arr)
00385 {
00386                       //we have a new choice of indexes
00387   if(k==0) {
00388                 estimate(paramEstimator, data, numForEstimate, bestVotes, curVotes, numVotesForBest, arr);
00389     return;
00390   }
00391 
00392                //continue to recursivly generate the choice of indexes
00393   int endIndex = n-k;
00394   for(int i=startIndex; i<=endIndex; i++) {
00395     arr[arrIndex] = i;
00396     computeAllChoices(paramEstimator, data, numForEstimate, bestVotes, curVotes, numVotesForBest,
00397                                         i+1, n, k-1, arrIndex+1, arr);
00398   }
00399 
00400 }
00401 /*****************************************************************************/
00402 template<class Estimator, class T, class S>
00403 void Ransac::estimate(const Estimator & paramEstimator, const std::vector<T> &data,
00404                       int numForEstimate, short *bestVotes, short *curVotes,
00405                       int &numVotesForBest, int *arr)
00406 {
00407         std::vector<T *> exactEstimateData;
00408         std::vector<S> exactEstimateParameters;
00409         int numDataObjects;
00410         int numVotesForCur;//initalize with -1 so that the first computation will be set to best
00411         int j;
00412 
00413         numDataObjects = data.size();
00414         memset(curVotes,'\0',numDataObjects*sizeof(short));
00415         numVotesForCur=0;
00416 
00417         for(j=0; j<numForEstimate; j++)
00418                 exactEstimateData.push_back(&(data[arr[j]]));
00419         paramEstimator.estimate(exactEstimateData,exactEstimateParameters);
00420                              //singular data configuration
00421         if(exactEstimateParameters.size()==0)
00422                 return;
00423 
00424         for(j=0; j<numDataObjects; j++) {
00425                 if(paramEstimator.agree(exactEstimateParameters, data[j])) {
00426                         curVotes[j] = 1;
00427                         numVotesForCur++;
00428                 }
00429         }
00430         if(numVotesForCur > numVotesForBest) {
00431                 numVotesForBest = numVotesForCur;
00432                 memcpy(bestVotes,curVotes, numDataObjects*sizeof(short));
00433         }
00434 }
00435 /*****************************************************************************/
00436 inline unsigned int Ransac::choose(unsigned int n, unsigned int m)
00437 {
00438         unsigned int denominatorEnd, numeratorStart, numerator,denominator; 
00439         if((n-m) > m) {
00440                 numeratorStart = n-m+1;
00441                 denominatorEnd = m;
00442         }
00443         else {
00444                 numeratorStart = m+1;
00445                 denominatorEnd = n-m;
00446         }
00447         
00448         unsigned int i;
00449         for(i=numeratorStart, numerator=1; i<=n; i++)
00450                 numerator*=i;
00451         for(i=1, denominator=1; i<=denominatorEnd; i++)
00452                 denominator*=i;
00453         return numerator/denominator;
00454 
00455 }
00456 
00457 
00458 #endif //_RANSAC_H_

Generated on 31 Jul 2015 for Hugintrunk by  doxygen 1.4.7