ransac.h

Go to the documentation of this file.

#ifndef DLR_COMPUTERVISION_RANSAC_H
#define DLR_COMPUTERVISION_RANSAC_H

#include <vector>
#include <dlrComputerVision/randomSampleSelector.h>

namespace dlr {

  namespace computerVision {

    enum RansacInlierStrategy {
      DLR_CV_NAIVE_ERROR_THRESHOLD
    };
      
    
    template <class Problem>
    class Ransac {
    public:

      typedef Problem ProblemType;

      
      typedef typename Problem::ModelType ResultType;

      
      Ransac(ProblemType const& problem,
             size_t minimumConsensusSize = 0,
             double requiredConfidence = 0.99,
             double inlierProbability = 0.5);


      virtual
      ~Ransac() {}


      virtual ResultType
      getResult() {
        // xxx
        ResultType result;
        this->estimate(result);
        return result;
      }
      

    protected:

      void
      computeConsensusSet(ResultType& model, std::vector<bool>& consensusFlags);

      bool
      estimate(ResultType& model);

      bool
      isConverged(std::vector<bool> const& consensusFlags,
                  std::vector<bool>& previousConsensusFlags,
                  size_t& consensusSetSize,
                  size_t& previousConsensusSetSize,
                  size_t& strikes);


      size_t m_minimumConsensusSize;
      size_t m_numberOfRandomSampleSets;
      ProblemType m_problem;
    };


    template <class Sample, class Model>
    class RansacProblem
      : public RandomSampleSelector<Sample>
    {
    public:
      
      // ========= Typedefs that must be present in order   =========
      // ========= to work with the Ransac class template.  =========
      // ========= You probably don't need to change these. =========

      typedef Model ModelType;


      typedef Sample SampleType;


      typedef typename RandomSampleSelector<SampleType>::SampleSequenceType
        SampleSequenceType;
      

      // ========= Member functions that must be provided by user =========
      // ========= You definitely do need to change these.        =========

      virtual ModelType
      estimateModel(SampleSequenceType const& sampleSequence) = 0;


      template <class IterType>
      void
      computeError(ModelType const& model,
                   SampleSequenceType const& sampleSequence,
                   IterType ouputIter) {};


      virtual double 
      getNaiveErrorThreshold() = 0;
      

      // ========= Public constructors, destructors, and    =========
      // ========= predefined member functions.             =========
      // ========= You probably don't need to change these. =========

      template <class IterType>
      RansacProblem(size_t sampleSize, IterType beginIter, IterType endIter)
        : RandomSampleSelector<SampleType>(beginIter, endIter),
          m_sampleSize(sampleSize) {}

      
      virtual
      ~RansacProblem() {}


      virtual size_t
      getSampleSize() {return m_sampleSize;}


      RansacInlierStrategy
      getInlierStrategy() {
        return DLR_CV_NAIVE_ERROR_THRESHOLD;
      }

    protected:

      size_t m_sampleSize;
      
    }; // class RansacProblem
    
  } // namespace computerVision
  
} // namespace dlr



/* ============ Definitions of inline & template functions ============ */


#include <cmath>
#include <algorithm>
#include <functional>
#include <dlrCommon/exception.h>
#include <dlrNumeric/maxRecorder.h>

namespace dlr {

  namespace computerVision {

    // The default constructor currently does nothing.
    template <class Problem>
    Ransac<Problem>::
    Ransac(Problem const& problem,
           size_t minimumConsensusSize,
           double requiredConfidence,
           double inlierProbability)
      : m_minimumConsensusSize(minimumConsensusSize),
        m_numberOfRandomSampleSets(),
        m_problem(problem)
    {
      size_t sampleSize = m_problem.getSampleSize();
      
      // The probability that any particular random sample is all inliers.
      double singlePickConfidence =
        std::pow(inlierProbability, static_cast<double>(sampleSize));

      // The probability that any particular random sample contains at
      // least one outlier.
      double singlePickDisconfidence = 1.0 - singlePickConfidence;

      // Number of times we have to sample in order to get a set of
      // all inliers with probability requiredConfidence.  This
      // follows Fischler's paper exactly.
      m_numberOfRandomSampleSets =
        std::log(1.0 - requiredConfidence) / std::log(singlePickDisconfidence);
      
      if(m_minimumConsensusSize == 0) {
        // User didn't specify a minimum consensus size.  Compute one
        // assuming that the probability of a sample matching an
        // incorrect model is <= 0.5, and using requiredConfidence as
        // a rough measure of how confident we have to be that we've
        // found the right model.  This follows Fischler's paper
        // approximately.
        int extraSamples = static_cast<int>(
          std::log(1.0 - requiredConfidence) / std::log(0.5) + 0.5);
        if(extraSamples < 0) {
          extraSamples = 0;
        }
        m_minimumConsensusSize = sampleSize + extraSamples;
      }
      
    }


    template <class Problem>
    void
    Ransac<Problem>::
    computeConsensusSet(typename Ransac<Problem>::ResultType& model,
                        std::vector<bool>& consensusFlags)
    {
      if(m_problem.getInlierStrategy() != DLR_CV_NAIVE_ERROR_THRESHOLD) {
        DLR_THROW(NotImplementedException, "Ransac::computeConsensusSet()",
                  "Currently only naive error thresholding is supported.");
      }
      if(consensusFlags.size() != m_problem.getPoolSize()) {
        consensusFlags.resize(m_problem.getPoolSize());
      }
      
      // Apply error function to entire set.
      typename Problem::SampleSequenceType testSet = m_problem.getPool();
      std::vector<double> errorMetrics(m_problem.getPoolSize());
      m_problem.computeError(model, testSet, errorMetrics.begin());

      // Find out which samples are within tolerance.
      double threshold = m_problem.getNaiveErrorThreshold();
      std::transform(
        errorMetrics.begin(), errorMetrics.end(), consensusFlags.begin(),
        std::bind2nd(std::less<double>(), threshold));
    }
    
    
    template <class Problem>
    bool
    Ransac<Problem>::
    estimate(typename Ransac<Problem>::ResultType& model)
    {
      dlr::numeric::MaxRecorder<size_t, ResultType> maxRecorder;
      for(size_t iteration = 0; iteration < m_numberOfRandomSampleSets;
          ++iteration) {

        // Select samples
        typename ProblemType::SampleSequenceType trialSet =
          m_problem.getRandomSample(m_problem.getSampleSize());

        std::vector<bool> consensusFlags(m_problem.getPoolSize());
        std::vector<bool> previousConsensusFlags(m_problem.getPoolSize(),
                                                 false);
        size_t consensusSetSize = 0;
        size_t previousConsensusSetSize = 0;
        size_t strikes = 0;
        while(1) {
          // Fit the model to the reduced (randomly sampled) set.
          model = m_problem.estimateModel(trialSet);

          // Identify the consensus set, made up of samples that are
          // sufficiently consistent with the model estimate.
          this->computeConsensusSet(model, consensusFlags);

          // See if this iteration has converged yet.
          if(this->isConverged(consensusFlags, previousConsensusFlags,
                               consensusSetSize, previousConsensusSetSize,
                               strikes)) {
            break;
          }

          // Not converged yet... loop so we can recompute the model
          // using the new consensus set.
          trialSet = m_problem.getSubset(
            consensusFlags.begin(), consensusFlags.end());
        }

        // OK, we've converged to a "best" result for this iteration.
        // Is it good enough to terminate?
        if(consensusSetSize > m_minimumConsensusSize) {
          // Reference argument model is already set appropriately.
          return true;
        }

        // Not ready to terminate yet, but remember this model (if
        // it's the best so far) in case we don't find any better.
        maxRecorder.test(consensusSetSize, model);
      }

      // Looks like we never found a gold plated correct answer.  Just
      // return report the best we found, and return false to indicate
      // our frustration.
      model = maxRecorder.getPayload();
      return false;
    }


    template <class Problem>
    bool
    Ransac<Problem>::
    isConverged(std::vector<bool> const& consensusFlags,
                std::vector<bool>& previousConsensusFlags,
                size_t& consensusSetSize,
                size_t& previousConsensusSetSize,
                size_t& strikes)
    {
      // Do we even have enough matching points to continue
      // iteration?
      consensusSetSize = std::count(
        consensusFlags.begin(), consensusFlags.end(), true);
      if(consensusSetSize < m_problem.getSampleSize()) {
        // No. The model is so bad that not even the points we used to
        // estimate it are within the consensus set!  We're converged,
        // after a fashion.
        return true;
      }

      // If previousConsensusFlags isn't initialized yet, then we're
      // clearly not converged, and can skip the remaining tests.
      if(consensusFlags.size() == previousConsensusFlags.size()) {

        // Does it look like we're in a cycle of adding/subtracting the
        // same points?
        if(consensusSetSize <= previousConsensusSetSize) {
          ++strikes;
        }
        if(strikes > 10) {
          return true;
        }

        // Hmm.  Are we selecting the same set as last time?  If so,
        // we're converged.
        if(std::equal(consensusFlags.begin(), consensusFlags.end(),
                      previousConsensusFlags.begin())) {
          return true;
        }
      }
      
      // Finally, do the bookkeeping so that previousConsensusFlags
      // gets updated.
      previousConsensusFlags = consensusFlags;
      previousConsensusSetSize = consensusSetSize;

      return false;
    }
    
  } // namespace computerVision
  
} // namespace dlr

#endif /* #ifndef DLR_COMPUTERVISION_RANSAC_H */