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 #include <random>
00040 #include <functional>
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     maxIndex = numDataObjects - 1;
00211     std::mt19937 rng(static_cast<unsigned int>(std::time(0)));
00212     std::uniform_int_distribution<> distribIndex(0, maxIndex);
00213     auto randIndex=std::bind(distribIndex, rng);
00214 
00215 //    srand((unsigned)time(NULL)); //seed random number generator
00216     numTries = (int)(numerator/denominator + 0.5);
00217 
00218     //there are cases when the probablistic number of tries is greater than all possible sub-sets
00219     numTries = numTries<allTries ? numTries : allTries;
00220 
00221     for(i=0; i<numTries; i++) {
00222         //randomly select data for exact model fit ('numForEstimate' objects).
00223         memset(notChosen,'1',numDataObjects*sizeof(short));
00224         curSubSetIndexes = new int[numForEstimate];
00225 
00226         exactEstimateData.clear();
00227 
00228         maxIndex = numDataObjects-1; 
00229         for(l=0; l<(int)numForEstimate; l++) {
00230             //selectedIndex is in [0,maxIndex]
00231             unsigned int selectedIndex = randIndex();
00232 //            unsigned int selectedIndex = (unsigned int)(((float)rand()/(float)RAND_MAX)*maxIndex + 0.5);
00233             for(j=-1,k=0; k<(int)numDataObjects && j<(int)selectedIndex; k++) {
00234                 if(notChosen[k])
00235                     j++;
00236             }
00237             k--;
00238             exactEstimateData.push_back(&(data[k]));
00239             notChosen[k] = 0;
00240             maxIndex--;
00241         }
00242         //get the indexes of the chosen objects so we can check that this sub-set hasn't been
00243         //chosen already
00244         for(l=0, j=0; j<(int)numDataObjects; j++) {
00245             if(!notChosen[j]) {
00246                 curSubSetIndexes[l] = j+1;
00247                 l++;
00248             }
00249         }
00250 
00251         //check that the sub set just chosen is unique
00252         std::pair< std::set<int *, SubSetIndexComparator >::iterator, bool > res = chosenSubSets.insert(curSubSetIndexes);
00253 
00254         if(res.second == true) { //first time we chose this sub set
00255                                  //use the selected data for an exact model parameter fit
00256             if (!paramEstimator.estimate(exactEstimateData,exactEstimateParameters))
00257                 //selected data is a singular configuration (e.g. three colinear points for 
00258                 //a circle fit)
00259                 continue;
00260             //see how many agree on this estimate
00261             numVotesForCur = 0;
00262             memset(curVotes,'\0',numDataObjects*sizeof(short));
00263             for(j=0; j<(int)numDataObjects; j++) {
00264                 if(paramEstimator.agree(exactEstimateParameters, data[j])) {
00265                     curVotes[j] = 1;
00266                     numVotesForCur++;
00267                 }
00268             }
00269             // debug output
00270             #ifdef DEBUG_RANSAC
00271             std::cerr << "RANSAC iter " << i << ": inliers: " << numVotesForCur << " parameters:";
00272             for (int jj=0; jj < exactEstimateParameters.size(); jj++)
00273                 std::cerr << " " << exactEstimateParameters[jj];
00274             std::cerr << std::endl;
00275             #endif
00276 
00277             if(numVotesForCur > numVotesForBest) {
00278                 numVotesForBest = numVotesForCur;
00279                 memcpy(bestVotes,curVotes, numDataObjects*sizeof(short));
00280                 parameters = exactEstimateParameters;
00281             }
00282             /*
00283             //update the estimate of outliers and the number of iterations we need
00284             outlierPercentage = 1 - (double)numVotesForCur/(double)numDataObjects;
00285             if(outlierPercentage < maximalOutlierPercentage) {
00286                 maximalOutlierPercentage = outlierPercentage;
00287                 denominator = log(1- pow((double)(1.0-maximalOutlierPercentage), (double)(numForEstimate)));
00288                 numTries = (int)(numerator/denominator + 0.5);
00289                 //there are cases when the probablistic number of tries is greater than all possible sub-sets
00290                 numTries = numTries<allTries ? numTries : allTries;
00291             }
00292             */
00293         }
00294         else {  //this sub set already appeared, don't count this iteration
00295             delete [] curSubSetIndexes;
00296             i--;
00297         }
00298     }
00299 
00300     //release the memory
00301     std::set<int *, SubSetIndexComparator >::iterator it = chosenSubSets.begin();
00302     std::set<int *, SubSetIndexComparator >::iterator chosenSubSetsEnd = chosenSubSets.end();
00303     while(it!=chosenSubSetsEnd) {
00304         delete [] (*it);
00305         it++;
00306     }
00307     chosenSubSets.clear();
00308 
00309     //compute the least squares estimate using the largest sub set
00310     if(numVotesForBest > 0) {
00311         for(j=0; j<(int)numDataObjects; j++) {
00312             if(bestVotes[j]) {
00313                 leastSquaresEstimateData.push_back(&(data[j]));
00314                 inliers.push_back(j);
00315             }
00316         }
00317         paramEstimator.leastSquaresEstimate(leastSquaresEstimateData,parameters);
00318     }
00319     delete [] bestVotes;
00320     delete [] curVotes;
00321     delete [] notChosen;
00322 
00323     return leastSquaresEstimateData;
00324 }
00325 /*****************************************************************************/
00326 template<class Estimator, class S, class T>
00327 std::vector<const T*> Ransac::compute(S &parameters,
00328                     const Estimator & paramEstimator,
00329                     const std::vector<T> &data)
00330 {
00331     unsigned int numForEstimate = paramEstimator.numForEstimate();
00332     std::vector<T *> leastSquaresEstimateData;
00333     int numDataObjects = data.size();
00334     int numVotesForBest = 0;
00335     int *arr = new int[numForEstimate];
00336     short *curVotes = new short[numDataObjects];  //one if data[i] agrees with the current model, otherwise zero
00337     short *bestVotes = new short[numDataObjects];  //one if data[i] agrees with the best model, otherwise zero
00338         
00339     //parameters.clear();
00340 
00341     //there are less data objects than the minimum required for an exact fit
00342     if(numDataObjects < numForEstimate) 
00343         return 0;
00344 
00345     computeAllChoices(paramEstimator,data,numForEstimate,
00346                       bestVotes, curVotes, numVotesForBest, 0, data.size(), numForEstimate, 0, arr);
00347 
00348     //compute the least squares estimate using the largest sub set
00349     if(numVotesForBest > 0) {
00350         for(int j=0; j<numDataObjects; j++) {
00351             if(bestVotes[j])
00352                 leastSquaresEstimateData.push_back(&(data[j]));
00353         }
00354         paramEstimator.leastSquaresEstimate(leastSquaresEstimateData,parameters);
00355     }
00356 
00357     delete [] arr;
00358     delete [] bestVotes;
00359     delete [] curVotes; 
00360 
00361     return leastSquaresEstimateData;
00362 }
00363 /*****************************************************************************/
00364 template<class Estimator, class T>
00365 void Ransac::computeAllChoices(const Estimator &paramEstimator, const std::vector<T> &data,
00366                                 int numForEstimate,short *bestVotes, short *curVotes,
00367                                 int &numVotesForBest, int startIndex, int n, int k,
00368                                 int arrIndex, int *arr)
00369 {
00370                       //we have a new choice of indexes
00371   if(k==0) {
00372                 estimate(paramEstimator, data, numForEstimate, bestVotes, curVotes, numVotesForBest, arr);
00373     return;
00374   }
00375 
00376                //continue to recursivly generate the choice of indexes
00377   int endIndex = n-k;
00378   for(int i=startIndex; i<=endIndex; i++) {
00379     arr[arrIndex] = i;
00380     computeAllChoices(paramEstimator, data, numForEstimate, bestVotes, curVotes, numVotesForBest,
00381                                         i+1, n, k-1, arrIndex+1, arr);
00382   }
00383 
00384 }
00385 /*****************************************************************************/
00386 template<class Estimator, class T, class S>
00387 void Ransac::estimate(const Estimator & paramEstimator, const std::vector<T> &data,
00388                       int numForEstimate, short *bestVotes, short *curVotes,
00389                       int &numVotesForBest, int *arr)
00390 {
00391         std::vector<T *> exactEstimateData;
00392         std::vector<S> exactEstimateParameters;
00393         int numDataObjects;
00394         int numVotesForCur;//initalize with -1 so that the first computation will be set to best
00395         int j;
00396 
00397         numDataObjects = data.size();
00398         memset(curVotes,'\0',numDataObjects*sizeof(short));
00399         numVotesForCur=0;
00400 
00401         for(j=0; j<numForEstimate; j++)
00402                 exactEstimateData.push_back(&(data[arr[j]]));
00403         paramEstimator.estimate(exactEstimateData,exactEstimateParameters);
00404                              //singular data configuration
00405         if(exactEstimateParameters.size()==0)
00406                 return;
00407 
00408         for(j=0; j<numDataObjects; j++) {
00409                 if(paramEstimator.agree(exactEstimateParameters, data[j])) {
00410                         curVotes[j] = 1;
00411                         numVotesForCur++;
00412                 }
00413         }
00414         if(numVotesForCur > numVotesForBest) {
00415                 numVotesForBest = numVotesForCur;
00416                 memcpy(bestVotes,curVotes, numDataObjects*sizeof(short));
00417         }
00418 }
00419 /*****************************************************************************/
00420 inline unsigned int Ransac::choose(unsigned int n, unsigned int m)
00421 {
00422         unsigned int denominatorEnd, numeratorStart, numerator,denominator; 
00423         if((n-m) > m) {
00424                 numeratorStart = n-m+1;
00425                 denominatorEnd = m;
00426         }
00427         else {
00428                 numeratorStart = m+1;
00429                 denominatorEnd = n-m;
00430         }
00431         
00432         unsigned int i;
00433         for(i=numeratorStart, numerator=1; i<=n; i++)
00434                 numerator*=i;
00435         for(i=1, denominator=1; i<=denominatorEnd; i++)
00436                 denominator*=i;
00437         return numerator/denominator;
00438 
00439 }
00440 
00441 
00442 #endif //_RANSAC_H_

Generated on 27 May 2016 for Hugintrunk by  doxygen 1.4.7