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

Generated on Mon Sep 1 01:25:41 2014 for Hugintrunk by  doxygen 1.3.9.1