RANSAC.hh

/*
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


#ifndef __RANSAC_hh__
#define __RANSAC_hh__

#include <bias_config.h>
#include <Base/Common/BIASpragmaStart.hh>

#include <Base/Debug/Debug.hh>
#include <Base/Math/Random.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Base/Math/Matrix.hh>

#include <algorithm>
#include <float.h>
#include <iterator>
#include <vector>
#include <iomanip>

#define RANSAC_SAMPLE_PROGRESS_EVERY 100

#define RANSAC_DEFAULT_PROBOFGOODSOLUTION 0.98
#define RANSAC_DEFAULT_SOSMALLSOLUTIONCHANGE 0.02

namespace BIAS {
  
  /** @class RANSAC
      @ingroup g_mathalgo
      @brief Generic abstract base class for RANSAC (RANdom SAmple Consensus)
             estimators.

      To use this class, derive a class from it in which the following methods
      are overloaded:
      - RANSAC::GetSampleSolutions : Computes solution(s) from a set of input
                                     samples.
      - RANSAC::EvaluateSolution : Evaluate input samples with respect to a
                                   given solution and classify inliers/outliers
      - RANSAC::RefineSolution : Refine a given solution using all inliers

      RANSAC::GenerateSamples can be overloaded e.g. if for an instance an even-
      spaced sampling is desired.

      For examples how to use this class see
      - MathAlgo/Examples/ExampleRANSAC_double.cpp
      - MathAlgo/Examples/ExampleRANSACPlane.cpp

      The function SolveMaster was ported from TargetJR -> PMatrixRANSAC by
      frahm and from there on further ported to this class from woelk.

      @note Since this is a generic templated class the implementation
      has to go into the header file.

      @author frahm, woelk
  */

  template <class SolutionType>
  class /*BIASMathAlgo_EXPORT*/ RANSAC : virtual public Debug
  {

  public:

    RANSAC();

    /** Construct a RANSAC object that takes samples of size sample_size,
        returning at most max_solutions_per_sample solutions each time.
        If the refine_solution flag is true, a routine has been supplied to
        refine the initial fits before evaluation.

        In order to use the RANSAC, a child class from class RANSAC or class
        RANSACPreKnowledge has to be implemented with the methods Evalulate Solution,
        GetSampleSolutions, and RefineSolution.
        A Compute method needs to do all the preparations and call the SolveMaster
        method.
        See ExampleRANSACPlane to get an idea of how to use the parameters.
        */
    RANSAC(unsigned int sample_size, unsigned int max_solutions_per_sample = 1,
           bool refine_solution = false, bool greedy = false);

    virtual ~RANSAC();

    inline void Init(unsigned int sample_size, unsigned int max_solutions_per_sample = 1,
                     bool refine_solution = false, bool greedy = false);

    /** @brief Explicitly set max number of samples

        Ransacing will stop after max_samples samples, regardless
        of solution quality.  Normally, more than a few thousand samples
        signifies a problem. */
    inline void SetMaxSamples(unsigned int max_samples)
    { _uiMaxSamples = max_samples; }

    /** @brief Explicitly set number of samples

        Explicitly set number of samples to make in SolveMaster, i.e.
        Value of ComputeSampleCount() is ignored if this function is called
        with a strictly positive value uiSamples.
        Note:
        SolveMaster performs at most min(_uiMaxSamples, uiSamples) iterations
        @author koeser
     */
    inline void SetSampleCount(unsigned int uiSamples) {
      _uiExplicitSampleNo = uiSamples;
    }

    /** @brief Main computation function called by user.

        This function is called to run the computation.
        On output, solution is set to the best solution found and
        inliers indicate which data were inliers to that solution.
        The inlying_data_fraction is responsible for the maximal number of
        sample sets taken from the data. The smaller the
        inlying_data_fraction, the more sample sets are taken

        @attention Do NOT override this function in derived classes, unless
        you want to do modifications in the RANSAC _algorithm_ itself (and
        usually you dont want to do this ...)
        input : inlying_data_fraction   expected inlier fraction
        output: solution                solution
                inliers                 inlier flags
        @return <0 indicates an error, otherwise number of chosen sample
        @author frahm, woelk */
    inline virtual int SolveMaster(double inlying_data_fraction,
                                   SolutionType &solution,
                                   std::vector<bool> &inliers);

    /** @brief Compute solution(s) for the given set of samples
        @return the number of solutions found.

        The which_samples array will be of length sample_size, selecting the
        indices to be fit to from the subclass's data array. */
    virtual inline int GetSampleSolutions(std::vector<unsigned int> &which_samples,
                                          std::vector<SolutionType> &solutions);

    /** @brief evaluate the goodness of a solution
     *
     *  For the specified solution, set
     *  - an accept count indicating the number of data which are inliers
     *    to the given solution,
     *  - the inlier flag for each datum to true if the datum is inlying
     *  - an evaluate score (typically the average residual) indicating
     *    the goodness of fit of the inliers - the solver code requires a
     *    positive number where zero is a perfect solution, larger positive
     *    scores mean a worse solution.
     *  - an ok_to_terminate flag which is set TRUE if the solution
     *    is good enough that the robust solver can discontinue search for
     *    a better solution. Only happens, if greedy is set to true.
     *  - The return value tells the algorithm, if the solution is
     *    good enough to be refined further - meaning, if set to true and
     *    if RefineSolution_ is set to true, the solution will be refined.
     *    In the example below, true is returned even if only one samples
     *    has been found to be an inlier. Because refinement usually takes
     *    quite a while, it is sensible to use a higher threshold. For example
     *    the use of a minimum inlier fraction via a private variable in the
     *    child class can be tested on.
     *
     *  If existing_solution_flag is true, then the best accept count
     *  so far is in accept_count.  The function can terminate early if
     *  it is not going to beat this.
     *
     *  A skeleton implementation of this function would be written as follows:
     *  \verbatim
     *  int my_accept_count = 0;
     *  for (unsigned int i=0; i<_uiDataSize; i++) {
     *    inlier[i] = Consistent(corner_matches[i], solution);
     *    if (inlier[i]) {
     *      my_accept_count++;
     *      evaluate_score += Residual(corner_matches[i], solution);
     *      if (existing_solution_flag && my_accept_count>accept_count)
     *        break;
     *    }
     *  }
     *  accept_count=my_accept_count;
     *  evaluate_score=(accept_count!=0)?(evaluate_score/accept_count):
     *    (DBL_MAX);
     *  if (evaluate_score < solution_error_threshold &&
     *      (double)accept_count/(double)_uiDataSize > min_inlier_fraction)
     *    ok_to_terminate_flag=true;
     *  else
     *    ok_to_terminate_flag=false;
     *  return (accept_count!=0) // maybe even (accept_count>inl_frac)
     *  \endverbatim
     *
     *  The line if (existing_solution_flag && my_accept_count>accept_count)
     *        break;
     *  causes the algorithm to not determine the score and acceptcount correctly,
     *  only so far as to top the so far best solution. It can be used for very fast
     *  solutions, however if the exact inliers are needed and refineSolution_ is set
     *  to false, the evaluation should not be omnitted.
     *
     *  @param solution (in) solution to be evaluated
     *  @param existing_solution_flag (in) if true the best accept_cout so far
     *                                is given in accept_count
     *  @param accept_count (in/out) input: best accept_cout so far if
     *                      existing_solution_flag==true, output: number of
     *                      inliers to the current solution
     *  @param inlier (out) inlier[i] ==> datum i is inlying
     *  @param evaluate_score (out) a score for the solution, 0 means ideal
     *                        solution, larger score means worse solution
     *  @param ok_to_termiate_flag (out) if true, RANSACing will stop */
    virtual inline
    bool EvaluateSolution(const SolutionType& solution,
                          bool existing_solution_flag,
                          std::vector<bool>& inlier, int& accept_count,
                          double& evaluate_score, bool& ok_to_terminate_flag);

    /** @brief refine previously computed solution based on its inliers

        If implemented, this function should compute a more accurate solution,
        e.g. a least squares on all inliers, possibly using as an initial
        estimate the solution in solution.
        The output array new_inliers should be set to reflect the inliers to
        the new solution.
        @return false if no better solution than the initial could be found
    */
    virtual inline
    bool RefineSolution(std::vector<unsigned int>& which_samples,
                        SolutionType& solution,
                        std::vector<bool>& new_inliers);

    /** @brief pick a set of samples

        Override this to specialize the technique used to generate samples.
        By default, random sampling is assumed but implementations may wish to
        use the latin-square sampling provided by generate_sample_evenspace
        (below), or some other problem-specific sampling strategy. */
    virtual inline
    bool GenerateSamples(int sample_index,
                         std::vector<unsigned int>& which_samples);

    inline bool GenerateSamplesEvenspace(int sample_index,
                                  std::vector<unsigned int>& sample_table);

    inline bool GenerateSamplesRandom(int sample_index,
                               std::vector<unsigned int>& sampletable);

    // Return the best EvaluateScore obtained by a previous call of SolveMaster
    inline double GetFinalScore() const { return _dLastBestSampleEvaluateScore; }

  protected:
    /// number of samples available
    unsigned _uiSampleSize;

    /// max number of solutions that are to be computed for one sample set
    unsigned _uiMaxSolutionsPerSample;

    /// number of data elements needed to compute a solution
    unsigned _uiDataSize;

    /// do we want to refine a good sample (one for which evaluate solution returned true)?
    bool _bRefineSolution;

    /// sampled best values
    int    _iBestSampleAcceptCount;
    double _dBestSampleEvaluateScore;
    double _dLastBestSampleEvaluateScore;

    /// refined best values
    int    _iBestInlierAcceptCount;
    double _dBestInlierEvaluateScore;

    /// absolute max number of samples
    unsigned int _uiMaxSamples;

    /// use this number of samples
    unsigned int _uiExplicitSampleNo;

    /// prob. for computing number of samples, set close to 1
    double _dProbabilityOfGoodSolution;

    /// If all other conditions are right, and the fractional change in the
    /// solution statistics is less than this, stop sampling.
    double _dSoSmallSolutionChange;

    /// if we already have a solution for which EvaluateSolution returned true
    bool _got_solution_flag;

    /// if true, the first acceptable solution is returned
    bool _greedy;

  protected:
    static inline int ComputeSampleCount(double inlying_data_fraction,
                                  double desired_prob_good,
                                  unsigned int sample_size);
  };



  /************************************************************/
  /* now the implementation                                   */
  /************************************************************/


  template <class SolutionType> inline
  RANSAC<SolutionType>::RANSAC()
  {
    _dProbabilityOfGoodSolution = RANSAC_DEFAULT_PROBOFGOODSOLUTION;
    _dSoSmallSolutionChange = RANSAC_DEFAULT_SOSMALLSOLUTIONCHANGE;
    _uiSampleSize=_uiMaxSolutionsPerSample=_uiDataSize=_uiMaxSamples=0;
    _uiExplicitSampleNo = 0;
    _bRefineSolution = false;
    _iBestSampleAcceptCount   = _iBestInlierAcceptCount   = -1;
    _dBestSampleEvaluateScore = _dBestInlierEvaluateScore = -1.0;
    _got_solution_flag = false;
    _greedy = false;
    NewDebugLevel("D_RANSAC_INLIERS");
    NewDebugLevel("D_RANSAC_SAMPLES");
    NewDebugLevel("D_RANSAC_SOLVE_MASTER");
    NewDebugLevel("D_RANSAC_SAMPLE_QUALITY");
    NewDebugLevel("D_RANSAC_SAMPLE_PROGRESS");
    NewDebugLevel("D_RANSAC_SOLUTION");
    //AddDebugLevel("D_RANSAC_SOLUTION");
  }

  template <class SolutionType> inline
  RANSAC<SolutionType>::RANSAC(unsigned int sample_size,
                               unsigned int max_solutions_per_sample,
                               bool refine_solution, bool greedy)
  {
    _dProbabilityOfGoodSolution = RANSAC_DEFAULT_PROBOFGOODSOLUTION;
    _dSoSmallSolutionChange = RANSAC_DEFAULT_SOSMALLSOLUTIONCHANGE;
    Init(sample_size, max_solutions_per_sample, refine_solution);
    _uiDataSize = 0;
    _iBestSampleAcceptCount   = _iBestInlierAcceptCount   = -1;
    _dBestSampleEvaluateScore = _dBestInlierEvaluateScore = -1.0;
    _got_solution_flag = false;
    _greedy = greedy;
    NewDebugLevel("D_RANSAC_INLIERS");
    NewDebugLevel("D_RANSAC_SAMPLES");
    NewDebugLevel("D_RANSAC_SOLVE_MASTER");
    NewDebugLevel("D_RANSAC_SAMPLE_QUALITY");
    NewDebugLevel("D_RANSAC_SAMPLE_PROGRESS");
    NewDebugLevel("D_RANSAC_SOLUTION");
    //AddDebugLevel("D_RANSAC_SOLUTION");
  }

  template <class SolutionType> inline
  void RANSAC<SolutionType>::Init(unsigned int sample_size,
                                  unsigned int max_solutions_per_sample,
                                  bool refine_solution, bool greedy)
  {
    _uiSampleSize=sample_size;
    _uiMaxSolutionsPerSample=max_solutions_per_sample;
    _bRefineSolution=refine_solution;
    _uiMaxSamples = 0;
    _uiExplicitSampleNo = 0;
    _greedy = greedy;
  }

  template <class SolutionType> inline
  RANSAC<SolutionType>::~RANSAC()
  {}

  /* @author frahm, woelk */
  template <class SolutionType> inline
  int RANSAC<SolutionType>::SolveMaster(double inlying_data_fraction,
                                        SolutionType& solution,
                                        std::vector<bool>& inliers)
  {
    BCDOUT(D_RANSAC_SOLVE_MASTER, "RANSAC::SolveMaster() --------------------"
           "--------------------------------------\n");

#ifdef BIAS_DEBUG
    if (_uiDataSize==0){
      BIASERR("_uiDataSize should be set in derived class first "
              <<_uiDataSize);
      BIASABORT;
    }
    if (inlying_data_fraction>1.0 || inlying_data_fraction<0.0){
      BIASERR("invalid inlying_data_fraction: "<<inlying_data_fraction);
      BIASABORT;
    }
#endif
    if (_uiDataSize < _uiSampleSize) {
      BIASERR("SolveMaster: _uiDataSize < _uiSampleSize !");
      return -1;
    }

    std::vector<bool> inlier_set(_uiDataSize, false);

    std::vector<SolutionType> multisoln_table(_uiMaxSolutionsPerSample);

    // check whether _uiExplicitSampleNo has been set (to a positive value)
    int sample_count;
    if (_uiExplicitSampleNo>0) {
      // yes the user specified to make exactly _uiExplicitSampleNo samples
      sample_count = _uiExplicitSampleNo;
      BCDOUT(D_RANSAC_SOLVE_MASTER, "RANSAC: Using "<< sample_count
             <<" samples (explicitly specified).");
    } else {
      // compute the number of samples needed using Torr's thesis
      sample_count = ComputeSampleCount(inlying_data_fraction,
                                        _dProbabilityOfGoodSolution,
                                        _uiSampleSize);

      BCDOUT(D_RANSAC_SOLVE_MASTER, "RANSAC: Need at most "<< sample_count
               <<" samples for "<< inlying_data_fraction <<" inlier fraction");

    }

    // Override computed bailout if specified
    if (_uiMaxSamples != 0 && (unsigned)sample_count > _uiMaxSamples) {
      sample_count = _uiMaxSamples;
      BCDOUT(D_RANSAC_SOLVE_MASTER,"RANSAC: But will bail out after "
               << sample_count );
    }

    // offsets of samples for current solution
    std::vector<unsigned int> which_samples;

    // number of solutions per sample
    int nsolutions;

    // number of inliers for current solution
    int sample_accept_count;

    // score for sample set(resp. current solution), is set by EvaluateSolution
    double sample_evaluate_score;

    // found a solution that good that we can finish, set by EvaluateSolution
    bool ok_to_terminate_flag;

    // current solution is accepted by EvaluateSolution
    bool good_flag;

    // number of inliers for current solution after refinement
    int inlier_accept_count;

    // score for current solution after refinement
    double inlier_evaluate_score;

    // indicates that the current is the best solution so far
    bool best_so_far_flag;

    // indicate whether we have ever reached the checkpoint before
    // fast termination can only happen at the second time we reach the
    // checkpoint (or if _greedy is set)
    bool checkpoint_set_flag = false;

    // backup of solution evaluation at last time we were at checkpoint
    int sample_checkpoint_count = 0;
    int inlier_checkpoint_count = 0;

    // if ok_to_terminate is set true, fast termination is enabled if
    // termination_check_active_flag is true.
    bool termination_check_active_flag = false;
    BCDOUT(D_RANSAC_SAMPLE_QUALITY, "\nchecking at most "<<sample_count
              <<" samples\n");

    // index of best sample so far
    int best_sample_index = -1;

    int sample_index;
    for (sample_index = 0; sample_index < sample_count; sample_index++){
   
      if (sample_index % RANSAC_SAMPLE_PROGRESS_EVERY ==
          RANSAC_SAMPLE_PROGRESS_EVERY-1) {
        BCDOUT(D_RANSAC_SAMPLE_PROGRESS, "RANSAC sample "
               <<sample_index+1<<" of "<<sample_count << std::endl);
      }
      
      // jmf for skipping if we haven't enough matches
      if (GenerateSamples(sample_index, which_samples)){
//        std::cout << std::setw(3)<<sample_index<<" : ";
//        copy(which_samples.begin(), which_samples.end(),
//             std::ostream_iterator<int>(std::cout, "  "));
//             std::cout << std::endl;
        nsolutions = GetSampleSolutions(which_samples, multisoln_table);
        if (nsolutions == 0) {
          BCDOUT(D_RANSAC_SAMPLE_QUALITY, "o"<<std::flush);
        }
        for (int solution_index=0; solution_index<nsolutions;solution_index++){
          BCDOUT(D_RANSAC_SOLVE_MASTER, "evaluating solution "
                    <<solution_index+1<<"/"<<nsolutions<<std::endl);
          SolutionType* active_solution_ptr = &multisoln_table[solution_index];

          if (_got_solution_flag) // ?
            sample_accept_count = _iBestSampleAcceptCount;

          ok_to_terminate_flag = false;
          sample_evaluate_score = DBL_MAX;
          good_flag =
            EvaluateSolution(*active_solution_ptr, _got_solution_flag,
                             inlier_set, sample_accept_count,
                             sample_evaluate_score, ok_to_terminate_flag);

          BCDOUT(D_RANSAC_SOLVE_MASTER,"sample: "<<sample_index<<"/"
                    <<sample_count<<" ("
                    <<solution_index<<")"
                    <<" solution: "<<(*active_solution_ptr)
                    <<" inliers: " <<sample_accept_count<<"/"<<_uiDataSize
                    <<" score: "<< sample_evaluate_score<<" good_flag: "
                    <<std::boolalpha<<good_flag<<std::endl);
          inlier_accept_count=-1;
          inlier_evaluate_score = 1e20;
          if (good_flag && _bRefineSolution) {
            // backup linear solution in case refining doesnt improve results
            SolutionType samplesolution = *active_solution_ptr;
            bool ok_to_terminate_with_linear = ok_to_terminate_flag;

            which_samples.clear();
            for (unsigned int i=0; i<_uiDataSize; i++)
              if (inlier_set[i]) which_samples.push_back(i);
            BCDOUT(D_RANSAC_SOLVE_MASTER, "refining solution "
                      <<*active_solution_ptr<<std::endl);
            if (RefineSolution(which_samples, *active_solution_ptr,
                               inlier_set)){
              if (_got_solution_flag)
                inlier_accept_count = _iBestInlierAcceptCount;
              BCDOUT(D_RANSAC_SOLVE_MASTER, "evaluating refined solution: "
                        <<*active_solution_ptr<<std::endl);

              good_flag =
                EvaluateSolution (*active_solution_ptr, _got_solution_flag,
                                  inlier_set, inlier_accept_count,
                                  inlier_evaluate_score, ok_to_terminate_flag);
              if (good_flag) {
                BCDOUT(D_RANSAC_SOLVE_MASTER, "RANSAC: minimal/refined "
                          <<sample_accept_count<<"/"
                          <<inlier_accept_count<<" "
                          <<sample_evaluate_score<<"/"
                          <<inlier_evaluate_score<<std::endl);
              } else {
                BCDOUT(D_RANSAC_SOLVE_MASTER, "RANSAC: (note) "
                          <<"EvaluateSolution returned false\n");
              }
            } else {
              // RefineSolution did not succeed, we do not use the solution
              good_flag = false;
              BCDOUT(D_RANSAC_SOLVE_MASTER, "RANSAC: (note) RefineSolution"
                        <<" returned false\n");

            }
            // refining didnt improve results, use linear solution:
            if (!good_flag ||  (inlier_accept_count < sample_accept_count)) {
              BCDOUT(D_RANSAC_SOLVE_MASTER, "RANSAC: (note) Linear solution"
                     <<" overrides refined !!! \n");
              good_flag = true;
              inlier_accept_count = sample_accept_count;
              inlier_evaluate_score =  sample_evaluate_score;
              *active_solution_ptr = samplesolution;
              ok_to_terminate_flag = ok_to_terminate_with_linear;
            }
          } else { // if (good_flag && _bRefineSolution) {
            BCDOUT(D_RANSAC_SOLVE_MASTER, "RANSAC: (note) either good_flag"
                      <<"=false or _bRefineSolution=false\n");
          }

          // the criteria for deciding the best solution is the following -
          // (a) if the accept_count is higher than the previous best, take it,
          // (b) if the accept_count is equal to the previous best, and
          //     the evaluate score is smaller than the previous best, take it.
          if (good_flag) {
            BCDOUT(D_RANSAC_SAMPLE_QUALITY, "."<<std::flush);
            best_so_far_flag = false;

            if (!_got_solution_flag) {
              BCDOUT(D_RANSAC_SOLVE_MASTER, "setting _got_solution_flag to "
                        "true\n");
              _got_solution_flag = true;
              best_so_far_flag = true;
            } else {
              if (!_bRefineSolution) {
                if (sample_accept_count > _iBestSampleAcceptCount ||
                    (sample_accept_count == _iBestSampleAcceptCount &&
                     sample_evaluate_score < _dBestSampleEvaluateScore))
                  best_so_far_flag = true;
              } else {
                if (inlier_accept_count > _iBestInlierAcceptCount ||
                    (inlier_accept_count == _iBestInlierAcceptCount &&
                     inlier_evaluate_score < _dBestInlierEvaluateScore))
                  best_so_far_flag = true;
              }
            }

            // save all the scores associated with this result.
            // future evaluations will be tested against these accept scores.
            if (best_so_far_flag) {
              BCDOUT(D_RANSAC_SAMPLE_QUALITY, "b"<<std::flush);
              _iBestSampleAcceptCount = sample_accept_count;
              _dBestSampleEvaluateScore =
                (sample_evaluate_score<inlier_evaluate_score) ?
                sample_evaluate_score : inlier_evaluate_score;
              _iBestInlierAcceptCount = inlier_accept_count;
              _dBestInlierEvaluateScore = inlier_evaluate_score;
              best_sample_index = sample_index;

              // save inliers and solution
              inliers = inlier_set;
              solution = *active_solution_ptr;

              BCDOUT(D_RANSAC_SOLVE_MASTER, "RANSAC: Sample "<<sample_index
                        <<" is best so far.  " << "Inliers/score "
                        << _iBestSampleAcceptCount << "/"
                        <<_dBestSampleEvaluateScore<<": "
                        <<solution<<std::endl);
              if (_bRefineSolution){
                BCDOUT(D_RANSAC_SOLVE_MASTER,"  refined inliers/score "
                         <<_iBestInlierAcceptCount << "/"
                         <<_dBestInlierEvaluateScore<<std::endl);
              }


              if (ok_to_terminate_flag){
                BCDOUT(D_RANSAC_SAMPLE_QUALITY, "x"<<std::flush);
                termination_check_active_flag = true;
                if (_greedy)
                  break; // exit for (int solution_index=0; solution_index<...
                BCDOUT(D_RANSAC_SOLVE_MASTER,
                         "RANSAC: termination_check_active_flag = true");
              }
            } // if (best_so_far_flag) {
          } else { // if (good_flag) {
            BCDOUT(D_RANSAC_SAMPLE_QUALITY, "*"<< std::endl << std::flush);
          }
        } // for (int solution_index=0; solution_index<nsolutions ...

        // this is all fast termination stuff
        if (_got_solution_flag) {
          // in greedy mode, we dont want to check the improvement
          if (_greedy && termination_check_active_flag) {
            BCDOUT(D_RANSAC_SAMPLE_QUALITY, "X"<<std::flush);
            BCDOUT(D_RANSAC_SOLVE_MASTER," found solution, stopping sample "
                      "generation\n");
            break;
          }
          if (!checkpoint_set_flag) {
            checkpoint_set_flag = true;
          } else if (_greedy) {
            // check if we have already found such a good solution that we can
            // terminate (termination_check_active_flag)
            // and if the best-so-far-solution has not increased
            // significantly compared to the previous solution
            if (!_bRefineSolution) {
              if (termination_check_active_flag &&
                  ((double) (_iBestSampleAcceptCount - sample_checkpoint_count)
                   /(double)sample_checkpoint_count <_dSoSmallSolutionChange)){
                BCDOUT(D_RANSAC_SAMPLE_QUALITY, "S"<<std::flush);
                break;
              }
            } else {
              if (termination_check_active_flag &&
                  ((double) (_iBestInlierAcceptCount - inlier_checkpoint_count)
                   /(double)inlier_checkpoint_count <_dSoSmallSolutionChange)){
                BCDOUT(D_RANSAC_SAMPLE_QUALITY, "S"<<std::flush);
                break;
              }
            }
          }

          // save the current best values to measure the improvement next time
          inlier_checkpoint_count = _iBestInlierAcceptCount;
          sample_checkpoint_count = _iBestSampleAcceptCount;
        }
      } else { // if (GenerateSamples
        BIASERR("GenerateSamples failed");
      }

      // -------------- debug output (kk) ------------------

      if (sample_index % RANSAC_SAMPLE_PROGRESS_EVERY ==
          RANSAC_SAMPLE_PROGRESS_EVERY-1) {
        if (_bRefineSolution){
          BCDOUT(D_RANSAC_SAMPLE_PROGRESS,"Refined inliers/score "
                    <<_iBestInlierAcceptCount << "/"
                    <<_dBestInlierEvaluateScore<<"\n");
        } else {
          BCDOUT(D_RANSAC_SAMPLE_PROGRESS, " Unrefined inliers/score "
                    << _iBestSampleAcceptCount << "/"
                    <<_dBestSampleEvaluateScore<<" as solution : "
                    <<solution<<"\n");
        }
      }
      //  -------------- debug output ---------------------

    } // for (int sample_index = 0 ...
    BCDOUT(D_RANSAC_SAMPLE_QUALITY, "\n");
    BCDOUT(D_RANSAC_SAMPLE_QUALITY, "stopped after "<<sample_index
              <<" sampels\n");

    if (!_got_solution_flag) {
      BCDOUT(D_RANSAC_SOLUTION, "RANSAC: no solution found\n");
      BIASWARN("RANSAC: no solution found, return -2");
      // prepare return value "failed"
      best_sample_index = -2;
    } else {
      if (_greedy && sample_index==sample_count){
        BCDOUT(D_RANSAC_SOLUTION,
               "RANSAC: early termination not successful\n");
        BCDOUT(D_RANSAC_SOLUTION,
               "RANSAC: no sufficient solution found\n");
      }
      BCDOUT(D_RANSAC_SOLVE_MASTER, "\nRANSAC: using sample "<<best_sample_index
                <<" with inliers/score "<< _iBestSampleAcceptCount << "/"
                <<_dBestSampleEvaluateScore<<" as solution : "
                 <<solution<<"\n");
      if (_bRefineSolution){
        BCDOUT(D_RANSAC_SOLVE_MASTER,"   refined inliers/score "
                  <<_iBestInlierAcceptCount << "/"
                  <<_dBestInlierEvaluateScore<<"\n");
      }
    }
    _dLastBestSampleEvaluateScore = _dBestSampleEvaluateScore;
    // reset best so far values
    _iBestSampleAcceptCount   = -1;
    _dBestSampleEvaluateScore = -1;
    _iBestInlierAcceptCount   = -1;
    _dBestInlierEvaluateScore = -1;
    _got_solution_flag = false;
    return best_sample_index;
  }


  template <class SolutionType> inline
  int RANSAC<SolutionType>::
  GetSampleSolutions(std::vector<unsigned int> &which_samples,
                     std::vector<SolutionType> &solutions)
  {
    BIASERR("this function should be implemented in derived class");
    return -1;
  }


  template <class SolutionType> inline
  bool RANSAC<SolutionType>::
  EvaluateSolution(const SolutionType& solution, bool existing_solution_flag,
                   std::vector<bool>& out_inlier_flags, int& accept_count_ptr,
                   double& evaluate_score_ptr, bool& ok_to_terminate_flag_ptr)
  {
    BIASERR("this function should be implemented in derived class");
    return false;
  }

  template <class SolutionType> inline
  bool RANSAC<SolutionType>::
  RefineSolution(std::vector<unsigned int>& which_samples,
                 SolutionType& solution,
                 std::vector<bool>& new_inliers)
  {
    BIASERR("this function should be implemented in derived class");
    return false;
  }

  template <class SolutionType> inline
  bool RANSAC<SolutionType>::
  GenerateSamples(int sample_index, std::vector<unsigned int>& which_samples)
  {
    return GenerateSamplesRandom(sample_index, which_samples);
  }

  template <class SolutionType> inline
  bool RANSAC<SolutionType>::
  GenerateSamplesEvenspace(int sample_index,
                           std::vector<unsigned int>& which_samples)
  {
    bool res=false;
    BIASERR("unfinished code");
    /*
    // from sa_generate_sample_evenspace

    assert((int)_sample_size <= sample_table.length());
    assert((int)_sample_size < data_size);

    // basic_offset starts sample_size samples evenly spaced from the data_size set.
    unsigned basic_offset = data_size / _sample_size;
    unsigned offset = basic_offset + sample_index / basic_offset;
    unsigned data_index = sample_index % data_size;

    if (offset < basic_offset + data_size) {
    for(unsigned count = 0; count < _sample_size; ++count) {
    sample_table[count] = data_index;

    data_index += offset;

    if ((int)data_index >= data_size)
    data_index = data_index % data_size;
    }
    }
    else {
    if (offset == basic_offset + data_size && (sample_index % basic_offset == 0))
    cerr << "RANSAC: Evenspace sampling has reached repetition stage, changing to random sampling\n";

    generate_sample_random (sample_index, sample_table);
    }
    */
    return res;
  }

  template <class SolutionType> inline
  bool RANSAC<SolutionType>::
  GenerateSamplesRandom(int sample_index,
                        std::vector<unsigned int>& which_samples)
  {
    bool res=true;
#ifdef BIAS_DEBUG
    if (_uiSampleSize==0){
      BIASERR("call Init or appropriate constructor before using RANSAC "
              <<"(_uiSampleSize==0");
      res=false;
      BIASABORT;
    }
    if (_uiSampleSize>=_uiDataSize){
      BIASERR("_uiDataSize should be set in derived class before using RANSAC "
              <<"\t_uiSampleSize: "<<_uiSampleSize<<"\t_uiDataSize: "
              <<_uiDataSize);
      res=false;
      BIASABORT;
    }
#endif
    // initialzing random with 0 and 1 does not seem to work well
    Random ran(sample_index+rand());
    unsigned int count=0;
    unsigned int index;
    which_samples.clear();
    do {
      index = ran.GetUniformDistributedInt((unsigned int)0, _uiDataSize-1);
      if (find(which_samples.begin(), which_samples.end(), index)==
          which_samples.end()){
        which_samples.push_back(index);
        count++;
        //std::cout << index << "  ";
      }
    } while (count<_uiSampleSize);
    //std::cout << std::endl;
#ifdef BIAS_DEBUG
    if (DebugLevelIsSet("D_RANSAC_SAMPLES")){
      BCDOUT(D_RANSAC_SAMPLES, "---------- sample "<<sample_index
               <<" ---------------------------------"<<"\nnow using data: ");
      std::ostream_iterator<int> oi(GetDebugStream(), " ");
      std::copy(which_samples.begin(), which_samples.end(), oi);
      GetDebugStream() << std::endl;
    }
#endif

    return res;
  }


  template <class SolutionType> inline
  int RANSAC<SolutionType>::
  ComputeSampleCount(double inlying_data_fraction, double desired_prob_good,
                     unsigned int sample_size)
  {
    if (inlying_data_fraction == 1.0)
      // the analytic expression below cant cope with this so do it explicitly.
      return 1;

    // from Torr's thesis.
    return int(log (1.0 - desired_prob_good) /
               log (1.0 - pow (inlying_data_fraction, (double) sample_size)));
  }


} // namespace

#include <Base/Common/BIASpragmaEnd.hh>

#endif   //__RANSAC_hh__