ransac.h

Go to the documentation of this file.

// -*- c-basic-offset: 4 -*-
#ifndef _RANSAC_H_
#define _RANSAC_H_

#include <set>
#include <vector>
#include <stdlib.h>
#include <cstring>
#include <math.h>
#include <ctime>

#include "hugin_config.h"
#include <random>
#include <functional>

//#include "ParameterEsitmator.h"

//#define DEBUG_RANSAC

class Ransac {

public:
        template<class Estimator, class S, class T>
        static std::vector<const T*> compute(S & parameters,
                                             std::vector<int> & inliers,
                                             const Estimator & paramEstimator ,
                                             const std::vector<T> &data, 
                                             double desiredProbabilityForNoOutliers,
                                             double maximalOutlierPercentage);


        template<class Estimator, class S, class T>
        static std::vector<const T*> compute(S &parameters, 
                                             const Estimator & paramEstimator ,
                                             const std::vector<T> &data);
        
private:

    static unsigned int choose(unsigned int n, unsigned int m);

    template<class Estimator, class T>
    static void computeAllChoices(const Estimator & paramEstimator,
                                  const std::vector<T> &data,
                                  int numForEstimate,
                                  short *bestVotes, short *curVotes,
                                  int &numVotesForBest, int startIndex,
                                  int n, int k, int arrIndex, int *arr);


    template<class Estimator, class T, class S>
    static void estimate(const Estimator & paramEstimator, const std::vector<T> &data,
                         int numForEstimate,
                         short *bestVotes, short *curVotes,
                         int &numVotesForBest, int *arr);

    class SubSetIndexComparator 
    {
    private:
        int m_length;
        public:
            SubSetIndexComparator(int arrayLength) : m_length(arrayLength)
            {}

        bool operator()(const int *arr1, const int *arr2) const 
        {
            for(int i=0; i<m_length; i++)
                if(arr1[i] < arr2[i])
                    return true;
                return false;                   
        }
    };

};


/*******************************RanSac Implementation*************************/

template<class Estimator, class S, class T>
std::vector<const T *> Ransac::compute(S &parameters,
                                       std::vector<int> & inliers,
                                       const Estimator & paramEstimator,
                                       const std::vector<T> &data,
                                       double desiredProbabilityForNoOutliers,
                                       double maximalOutlierPercentage)
{
    unsigned int numDataObjects = (int) data.size();
    unsigned int numForEstimate = paramEstimator.numForEstimate();
    //there are less data objects than the minimum required for an exact fit, or
    //all the data is outliers?
    if(numDataObjects < numForEstimate || maximalOutlierPercentage>=1.0) 
        return std::vector<const T*>();

    std::vector<const T *> exactEstimateData;
    std::vector<const T *> leastSquaresEstimateData;
    S exactEstimateParameters;
    int i, j, k, l, numVotesForBest, numVotesForCur, maxIndex, numTries;
    short *bestVotes = new short[numDataObjects]; //one if data[i] agrees with the best model, otherwise zero
    short *curVotes = new short[numDataObjects];  //one if data[i] agrees with the current model, otherwise zero
    short *notChosen = new short[numDataObjects]; //not zero if data[i] is NOT chosen for computing the exact fit, otherwise zero
    SubSetIndexComparator subSetIndexComparator(numForEstimate);
    std::set<int *, SubSetIndexComparator > chosenSubSets(subSetIndexComparator);
    int *curSubSetIndexes;
    // double outlierPercentage = maximalOutlierPercentage;
    double numerator = log(1.0-desiredProbabilityForNoOutliers);
    double denominator = log(1- pow((double)(1.0-maximalOutlierPercentage), (double)(numForEstimate)));
    int allTries = choose(numDataObjects,numForEstimate);

    //parameters.clear();


    numVotesForBest = 0; //initalize with 0 so that the first computation which gives any type of fit will be set to best
    
    // intialize random generator
    maxIndex = numDataObjects - 1;
    std::mt19937 rng(static_cast<unsigned int>(std::time(0)));
    std::uniform_int_distribution<> distribIndex(0, maxIndex);
    auto randIndex=std::bind(distribIndex, rng);

//    srand((unsigned)time(NULL)); //seed random number generator
    numTries = (int)(numerator/denominator + 0.5);

    //there are cases when the probablistic number of tries is greater than all possible sub-sets
    numTries = numTries<allTries ? numTries : allTries;

    for(i=0; i<numTries; i++) {
        //randomly select data for exact model fit ('numForEstimate' objects).
        memset(notChosen,'1',numDataObjects*sizeof(short));
        curSubSetIndexes = new int[numForEstimate];

        exactEstimateData.clear();

        maxIndex = numDataObjects-1; 
        for(l=0; l<(int)numForEstimate; l++) {
            //selectedIndex is in [0,maxIndex]
            unsigned int selectedIndex = randIndex();
//            unsigned int selectedIndex = (unsigned int)(((float)rand()/(float)RAND_MAX)*maxIndex + 0.5);
            for(j=-1,k=0; k<(int)numDataObjects && j<(int)selectedIndex; k++) {
                if(notChosen[k])
                    j++;
            }
            k--;
            exactEstimateData.push_back(&(data[k]));
            notChosen[k] = 0;
            maxIndex--;
        }
        //get the indexes of the chosen objects so we can check that this sub-set hasn't been
        //chosen already
        for(l=0, j=0; j<(int)numDataObjects; j++) {
            if(!notChosen[j]) {
                curSubSetIndexes[l] = j+1;
                l++;
            }
        }

        //check that the sub set just chosen is unique
        std::pair< std::set<int *, SubSetIndexComparator >::iterator, bool > res = chosenSubSets.insert(curSubSetIndexes);

        if(res.second == true) { //first time we chose this sub set
                                 //use the selected data for an exact model parameter fit
            if (!paramEstimator.estimate(exactEstimateData,exactEstimateParameters))
                //selected data is a singular configuration (e.g. three colinear points for 
                //a circle fit)
                continue;
            //see how many agree on this estimate
            numVotesForCur = 0;
            memset(curVotes,'\0',numDataObjects*sizeof(short));
            for(j=0; j<(int)numDataObjects; j++) {
                if(paramEstimator.agree(exactEstimateParameters, data[j])) {
                    curVotes[j] = 1;
                    numVotesForCur++;
                }
            }
            // debug output
            #ifdef DEBUG_RANSAC
            std::cerr << "RANSAC iter " << i << ": inliers: " << numVotesForCur << " parameters:";
            for (int jj=0; jj < exactEstimateParameters.size(); jj++)
                std::cerr << " " << exactEstimateParameters[jj];
            std::cerr << std::endl;
            #endif

            if(numVotesForCur > numVotesForBest) {
                numVotesForBest = numVotesForCur;
                memcpy(bestVotes,curVotes, numDataObjects*sizeof(short));
                parameters = exactEstimateParameters;
            }
            /*
            //update the estimate of outliers and the number of iterations we need
            outlierPercentage = 1 - (double)numVotesForCur/(double)numDataObjects;
            if(outlierPercentage < maximalOutlierPercentage) {
                maximalOutlierPercentage = outlierPercentage;
                denominator = log(1- pow((double)(1.0-maximalOutlierPercentage), (double)(numForEstimate)));
                numTries = (int)(numerator/denominator + 0.5);
                //there are cases when the probablistic number of tries is greater than all possible sub-sets
                numTries = numTries<allTries ? numTries : allTries;
            }
            */
        }
        else {  //this sub set already appeared, don't count this iteration
            delete [] curSubSetIndexes;
            i--;
        }
    }

    //release the memory
    std::set<int *, SubSetIndexComparator >::iterator it = chosenSubSets.begin();
    std::set<int *, SubSetIndexComparator >::iterator chosenSubSetsEnd = chosenSubSets.end();
    while(it!=chosenSubSetsEnd) {
        delete [] (*it);
        it++;
    }
    chosenSubSets.clear();

    //compute the least squares estimate using the largest sub set
    if(numVotesForBest > 0) {
        for(j=0; j<(int)numDataObjects; j++) {
            if(bestVotes[j]) {
                leastSquaresEstimateData.push_back(&(data[j]));
                inliers.push_back(j);
            }
        }
        paramEstimator.leastSquaresEstimate(leastSquaresEstimateData,parameters);
    }
    delete [] bestVotes;
    delete [] curVotes;
    delete [] notChosen;

    return leastSquaresEstimateData;
}
/*****************************************************************************/
template<class Estimator, class S, class T>
std::vector<const T*> Ransac::compute(S &parameters,
                    const Estimator & paramEstimator,
                    const std::vector<T> &data)
{
    unsigned int numForEstimate = paramEstimator.numForEstimate();
    std::vector<T *> leastSquaresEstimateData;
    int numDataObjects = data.size();
    int numVotesForBest = 0;
    int *arr = new int[numForEstimate];
    short *curVotes = new short[numDataObjects];  //one if data[i] agrees with the current model, otherwise zero
    short *bestVotes = new short[numDataObjects];  //one if data[i] agrees with the best model, otherwise zero
        
    //parameters.clear();

    //there are less data objects than the minimum required for an exact fit
    if(numDataObjects < numForEstimate) 
        return 0;

    computeAllChoices(paramEstimator,data,numForEstimate,
                      bestVotes, curVotes, numVotesForBest, 0, data.size(), numForEstimate, 0, arr);

    //compute the least squares estimate using the largest sub set
    if(numVotesForBest > 0) {
        for(int j=0; j<numDataObjects; j++) {
            if(bestVotes[j])
                leastSquaresEstimateData.push_back(&(data[j]));
        }
        paramEstimator.leastSquaresEstimate(leastSquaresEstimateData,parameters);
    }

    delete [] arr;
    delete [] bestVotes;
    delete [] curVotes; 

    return leastSquaresEstimateData;
}
/*****************************************************************************/
template<class Estimator, class T>
void Ransac::computeAllChoices(const Estimator &paramEstimator, const std::vector<T> &data,
                                int numForEstimate,short *bestVotes, short *curVotes,
                                int &numVotesForBest, int startIndex, int n, int k,
                                int arrIndex, int *arr)
{
                      //we have a new choice of indexes
  if(k==0) {
                estimate(paramEstimator, data, numForEstimate, bestVotes, curVotes, numVotesForBest, arr);
    return;
  }

               //continue to recursivly generate the choice of indexes
  int endIndex = n-k;
  for(int i=startIndex; i<=endIndex; i++) {
    arr[arrIndex] = i;
    computeAllChoices(paramEstimator, data, numForEstimate, bestVotes, curVotes, numVotesForBest,
                                        i+1, n, k-1, arrIndex+1, arr);
  }

}
/*****************************************************************************/
template<class Estimator, class T, class S>
void Ransac::estimate(const Estimator & paramEstimator, const std::vector<T> &data,
                      int numForEstimate, short *bestVotes, short *curVotes,
                      int &numVotesForBest, int *arr)
{
        std::vector<T *> exactEstimateData;
        std::vector<S> exactEstimateParameters;
        int numDataObjects;
        int numVotesForCur;//initalize with -1 so that the first computation will be set to best
        int j;

        numDataObjects = data.size();
        memset(curVotes,'\0',numDataObjects*sizeof(short));
        numVotesForCur=0;

        for(j=0; j<numForEstimate; j++)
                exactEstimateData.push_back(&(data[arr[j]]));
        paramEstimator.estimate(exactEstimateData,exactEstimateParameters);
                             //singular data configuration
        if(exactEstimateParameters.empty())
                return;

        for(j=0; j<numDataObjects; j++) {
                if(paramEstimator.agree(exactEstimateParameters, data[j])) {
                        curVotes[j] = 1;
                        numVotesForCur++;
                }
        }
        if(numVotesForCur > numVotesForBest) {
                numVotesForBest = numVotesForCur;
                memcpy(bestVotes,curVotes, numDataObjects*sizeof(short));
        }
}
/*****************************************************************************/
inline unsigned int Ransac::choose(unsigned int n, unsigned int m)
{
        unsigned int denominatorEnd, numeratorStart, numerator,denominator; 
        if((n-m) > m) {
                numeratorStart = n-m+1;
                denominatorEnd = m;
        }
        else {
                numeratorStart = m+1;
                denominatorEnd = n-m;
        }
        
        unsigned int i;
        for(i=numeratorStart, numerator=1; i<=n; i++)
                numerator*=i;
        for(i=1, denominator=1; i<=denominatorEnd; i++)
                denominator*=i;
        return numerator/denominator;

}


#endif //_RANSAC_H_