ransac.hpp

#ifndef RANSAC_TEMPLATE_HPP
#define RANSAC_TEMPLATE_HPP

#include <float.h>
#include <vector>
#include <cmath>

#include "kernel/jafarException.hpp"

#include "jmath/randomIntTmplt.hpp"

#define PROBABILITY_GOOD_MODEL  0.90
#define TOO_SMALL_MODEL_CHANGE  0.10

namespace jafar {
  namespace model3d {
    template <class ModelType>
    class RANSAC
    {
    public:
      
      RANSAC();
      
      RANSAC(unsigned int sample_length);
      
      virtual ~RANSAC();
      
      
      void initialize(unsigned int sample_length);
      
      int Algorithm(double inlying_data_fraction,
        ModelType& model,
        std::vector<bool>& inliers,
        unsigned int&    nModelInlierNumber,
        double& dbModelScore);
      
      virtual void fitSampleModel(std::vector<unsigned int>&sample_data_indices,
         ModelType& model)=0;
      
      virtual 
      bool evaluateModel(const ModelType& model,
       std::vector<bool>& inlier, 
       unsigned int& unInlierNumber,
       double& evaluate_score);
      
      virtual 
      double distance(const ModelType& model, unsigned int unIndex)=0;
      //      {return 0.0;}
      
      virtual bool isDegenerate(std::vector<unsigned int>& sample_data_indices) 
      { return false; }
      
      virtual 
      bool generateSample(std::vector<unsigned int>& sample_data_indices);
      
      bool generateRandomSample(std::vector<unsigned int>& sampletable);
      
      
      // Setter and Getter functions
      
      inline void setMaxIterationNumber(unsigned int unMaxIterationNumber)
      { m_unMaxIterationNumber = unMaxIterationNumber; }
      
      void fixIterationNumber(unsigned int uiIterationNumber)
      { m_unFixedIterationNumber = uiIterationNumber; }
      
      void setInlierNumberThreshold(unsigned int unInlierNumberThreshold)
      {m_unInlierNumberThreshold = unInlierNumberThreshold;}
      
      unsigned int getInlierNumberThreshold()
      {return m_unInlierNumberThreshold;}
      
      void setInlierDistanceThreshold (double dbInlierDistanceThreshold)
      {  m_dbInlierDistanceThreshold =  dbInlierDistanceThreshold;}
      
      double getInlierDistanceThreshold ()
      {  return m_dbInlierDistanceThreshold;}
      
      void setModelErrorThreshold(double dbModelErrorThreshold)
      { m_dbModelErrorThreshold = dbModelErrorThreshold;}
      
      double getModelErrorThreshold()
      { return m_dbModelErrorThreshold;}
      
    protected:
      unsigned int m_unDataLength;
      
      unsigned int m_unSampleLength;
      
      jafar::jmath::RandomUIntVect*  m_pRandomUIntVect;
      
      unsigned int m_unMaxIterationNumber;
      
      unsigned int m_unFixedIterationNumber;
      
      double m_dbProbabilityOfGoodModel;
      
      double m_dbInlierDistanceThreshold;
      
      unsigned int m_unInlierNumberThreshold;
      
      double m_dbModelErrorThreshold;
      
      
    private:
      static int ComputeIterationNumber(double inlying_data_fraction, 
          double desired_prob_good, 
          unsigned int sample_length);
    };
    
    
    
    //------------------------------------------------------------
    //   template Implementation                                    
    //------------------------------------------------------------

    template <class ModelType>
    RANSAC<ModelType>::RANSAC()
    {
      m_dbProbabilityOfGoodModel = PROBABILITY_GOOD_MODEL;
      m_unSampleLength=m_unDataLength=m_unMaxIterationNumber=0;
      m_unFixedIterationNumber = 0;
      m_pRandomUIntVect = NULL;
      
      srand(time(NULL));
    }
    
    template <class ModelType>
    RANSAC<ModelType>::RANSAC(unsigned int sample_length)
    {
      m_dbProbabilityOfGoodModel = PROBABILITY_GOOD_MODEL;
      initialize(sample_length);
      m_unDataLength = 0;
      m_pRandomUIntVect = NULL;
      
      srand(time(NULL));
    }
    
    template <class ModelType>
    void RANSAC<ModelType>::initialize(unsigned int sample_length)
    {
      m_unSampleLength = sample_length;
      m_unMaxIterationNumber = 0;
      m_unFixedIterationNumber = 0;
      
      srand(time(NULL));


    }
    
    template <class ModelType>
    RANSAC<ModelType>::~RANSAC()
    {
      if (m_pRandomUIntVect != NULL) {
  delete m_pRandomUIntVect;
  m_pRandomUIntVect = NULL;
      }
    }
    
    template <class ModelType>
    int RANSAC<ModelType>::Algorithm(double inlying_data_fraction,
             ModelType& model,
             std::vector<bool>& inliers,
             unsigned int&    nModelInlierNumber,
             double& dbModelScore)
    {
      JFR_PRECOND(m_unDataLength!=0,
      "m_unDataLength should be set in derived class first.. use initialize");

      if (inlying_data_fraction>1.0 || inlying_data_fraction<0.0){
  JFR_RUN_TIME("invalid value : inlying_data_fraction must be between 0 and 1 ");
      }
      
      if (m_unDataLength < m_unSampleLength) {
  JFR_RUN_TIME("Algorithm: m_unDataLength < m_unSampleLength !");
      }
      
      // current Model
      ModelType curModel;
      // score for current model, set by evaluateModel
      double dbCurScore;
      // number of inliers for current model
      unsigned int nCurInlierNumber; 

      std::vector<bool> curInliers(m_unDataLength, false);

      // indexes of sample for current model
      std::vector<unsigned int> sample_data_indices;

      // current model is accepted by evaluateModel
      bool bGoodModel;
      // we have at least got a good model
      bool bGotModel;
      
      // indicates that the current is the best model so far
      bool theBestFlag;
      
      int iteration_number; // the number of loop
      int nIterationIndex;  // loop index

      // reset values
      nModelInlierNumber   = 0;
      dbModelScore = DBL_MAX;
      bGotModel = false;
      
      // check whether m_unFixedIterationNumber has been set
      // (to a positive value)
      if (m_unFixedIterationNumber>0) {
  // yes the user specified to make exactly 
  // m_unFixedIterationNumber iterations
  iteration_number = m_unFixedIterationNumber; 
      }
      else {
  // compute the number of iterations needed using Torr's thesis
  iteration_number = ComputeIterationNumber(inlying_data_fraction, 
              m_dbProbabilityOfGoodModel,
              m_unSampleLength);
      }
      
      // in all cases, Algorithm will stop at most after
      //  m_unMaxIterationNumber iterations
      if (m_unMaxIterationNumber != 0 && 
    (unsigned)iteration_number > m_unMaxIterationNumber) {
  iteration_number = m_unMaxIterationNumber;
      }
      
      // the main loop      
      for (nIterationIndex = 0; nIterationIndex < iteration_number; 
     nIterationIndex++) {

  // generate a sample  
  if (! generateSample(sample_data_indices)) {
    // generateSample failed, so continue and 
    // try a next iteration
    continue;
  }
    
  // now we have a sample
  if ( isDegenerate(sample_data_indices)){
    // we have a degerated sample, so continue and 
    // try a next iteration
    continue;
  }
    
  // now we have a non-degenerate sample
  fitSampleModel(sample_data_indices, curModel);
  if (bGotModel)
    nCurInlierNumber = nModelInlierNumber;
    
  dbCurScore = DBL_MAX;
  bGoodModel = evaluateModel(curModel, 
           curInliers,
           nCurInlierNumber,
           dbCurScore);
      
  // the criteria for deciding the best model is the following:
  // 1. if the unInlierNumber is higher than the previous best, 
  //    take it,
  // 2. if the unInlierNumber is equal to the previous best, and 
  //    the evaluate score is smaller than the previous best,
  //    take it.
  theBestFlag = false;
  if (bGoodModel) {
    if (!bGotModel) {
      bGotModel = true;
      theBestFlag = true;
    } 
    else {
      if (nCurInlierNumber > nModelInlierNumber ||
    (nCurInlierNumber == nModelInlierNumber &&
     dbCurScore < dbModelScore))
        theBestFlag = true;
    }
    
    if (theBestFlag) {
      // save inliers number, score, inliers and model
      nModelInlierNumber = nCurInlierNumber;
      dbModelScore = dbCurScore;      
      inliers = curInliers;
      model = curModel;     
    }
  } // if (bGoodModel)
      } // for (int nIterationIndex = 0 ...
      
      if (!bGotModel) {
  // "RANSAC: no model was found";
  // so it will return "failed" (-2)
  nIterationIndex = -2;
      }
      
      return nIterationIndex;
    }
    
    
    template <class ModelType>
    bool RANSAC<ModelType>::
    evaluateModel(const ModelType& model,
      std::vector<bool>& inlier,
      unsigned int& unInlierNumber,
      double& dbScore)
    {
      bool bGoodModel = false;
      inlier.clear();
      inlier.resize(m_unDataLength);
      
      unsigned int nCurInlierNumber = 0;
      dbScore = 0.0;
      double dbDistance;
      
      for (unsigned int i=0; i<m_unDataLength; i++){
  dbDistance = distance(model,i);
  inlier[i] = (dbDistance < m_dbInlierDistanceThreshold);
  if (inlier[i]) {
    nCurInlierNumber++;
    dbScore += dbDistance;
  }
      }
      if (nCurInlierNumber!=0) {
  dbScore /= nCurInlierNumber;
      } 
      else {
  dbScore=DBL_MAX;
  bGoodModel=false;
      }
      unInlierNumber = nCurInlierNumber;
      
      // we have a good model if the inlier number is greater than 
      // the inlier number threshold and the model score is smaller than the 
      // model error threshold
      bGoodModel = (unInlierNumber > m_unInlierNumberThreshold)
  && (dbScore < m_dbModelErrorThreshold);

      return bGoodModel;
    }

    
    template <class ModelType>
    bool RANSAC<ModelType>::
    generateSample(std::vector<unsigned int>& sample_data_indices)
    {
      return generateRandomSample(sample_data_indices);
    }
    
    template <class ModelType>
    bool RANSAC<ModelType>::
    generateRandomSample(std::vector<unsigned int>& sample_data_indices)
    {
      if (m_unSampleLength==0){
  //"call initialize or appropriate constructor befor using RANSAC"
  return false;
      }
      if (m_unSampleLength>=m_unDataLength){
  //"m_unDataLength should be set in derived class befor using RANSAC"
  return false;
      }
      JFR_PRECOND(m_pRandomUIntVect!=NULL,
      "NULL pointer:must use new to create it \
 when knowing the data size ");
      
      sample_data_indices = m_pRandomUIntVect->getDifferent();
      
      return true;
    }
    
    
    template <class ModelType>
    int RANSAC<ModelType>::
    ComputeIterationNumber(double inlying_data_fraction, 
         double desired_prob_good, 
         unsigned int sample_length)
    {
      // the analytic expression below cant cope with this so do it explicitly.
      // if all the data are inliers : we don't need to RANSAC
      // we can compute the model directly from the data
      if (inlying_data_fraction == 1.0)      
  return 1;
      
      // from Torr's thesis.
      return int(log (1.0 - desired_prob_good) /
     log (1.0 - pow (inlying_data_fraction, 
         (double) sample_length)));
    }
    
    
  } // namespace model3d
} // namespace jafar

#endif // RANSAC_TEMPLATE_HPP