PreemptiveRANSAC.hh

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

   Copyright (C) 2003, 2004    (see file CONTACTS 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 __PreemptiveRANSAC_hh__
#define __PreemptiveRANSAC_hh__

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

#include <Base/Debug/Debug.hh>
#include <Base/Math/Random.hh>
#include <MathAlgo/Median1D.hh>
#include <MathAlgo/RANSACEvaluatorInterface.hh>

#include <list>
#include <vector>
#include <limits>
#include <algorithm>

#ifndef NAN
# define NAN sqrt(-1.0)
#endif

#define PRANSAC_NO_SOLUTION -5
#define PRANSAC_SUBOPTIMAL_PARAMETERS -4
#define PRANSAC_INVALID_PARAMETERS -3
#define PRANSAC_NOTE_ENOUGH_SAMPLES -2
#define PRANSAC_NO_SOLUTION_LEFT -1
#define PRANSAC_OK 0
#define PRANSAC_NOT_ENOUGH_INLIERS 1
#define PRANSAC_SCORE_TOO_BIG 2
#define PRANSAC_ 2

namespace BIAS {


  /** @class PreemptiveRANSAC
      @ingroup g_mathalgo
      @brief Fast RANSAC after David Nister, "Preemptive RANSAC for Live
      Structure And Motion Estimation", Internation Conference on Computer
      Vision (ICCV) 2003

      The algorithm (preemptive RANSAC) is decoupled from the problem (for
      example HMatrix estimation) using a common interface class, the
      RANSACEvaluatorInterace. To use this class it is therefore necessary to
      derive from RANSACEvaluatorInterface and overload the functions:

      bool IsInlier(const SolutionType& solution,
                    const unsigned data_index,
                    double& score) = 0;

      int GetSampleSolutions(const std::vector<unsigned> &which_samples,
                             std::vector<SolutionType> &solutions)= 0;

      bool RefineSolution(const std::vector<bool>& inliers,
                          SolutionType& solution) = 0;

      Instantiate PreemptiveRANSAC with a pointer to the sibling of
      RANSACEvaluatorInterface as argment to the constructor and call
      alternatingly

      Init() and
      SolveMaster()

      @author woelk 2006? */

  template <class SolutionType>
  class PreemptiveRANSAC : virtual public Debug
  {
  public:
    PreemptiveRANSAC(RANSACEvaluatorInterface<SolutionType> *shi);

    virtual ~PreemptiveRANSAC() {};

    void Init();

    void AddSolutionGuesses(const std::vector<SolutionType>& guesses);

    /** @returns PRANSAC_OK when a solution could be found,
        a positive number when a solution could be found whcih did not met the
        minimum qulaity criteria (i.e. min_num_inliers and maximum score)
        and a negative number when absolutly no solution could be found */
    int SolveMaster(const double inlying_data_fraction, const double max_score,
                    SolutionType &solution, std::vector<bool> &inliers,
                    unsigned &num_inliers, double& score,
                    const bool auto_compute_max_samples = false);


    /** @brief 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(const unsigned max_samples)
    { _uiMaxSamples = max_samples; }

    /** @brief Set min number of samples.

        Ransacing will evaluate and compare solutions using sets of
        MinSamples solutions. */
    inline void SetMinSamples(const unsigned min_samples)
    { _uiMinSamples = min_samples; }

    inline void SetGreedy(const bool greedy)
    { _bGreedy = greedy; }

    /** the number of measurements that is initially evaluated */
    inline void SetNumInitialChecked(const unsigned num_initial_checked)
    { _uiNumInitialChecked = num_initial_checked; }

    /** when less than MinNumInitialInliers are found, the solution is
        rejected straight away */
    inline void SetMinNumInitialInliers(const unsigned min_num_initial_inl)
    { _uiMinNumInitialInliers = min_num_initial_inl; }

    inline void SetRefineSolutions(const bool refine_solutions)
    { _bRefineSolutions = refine_solutions; }

    /** after each EvaluationBlockSize-th measurement, the solutions are
        compared and only the better half is kept */
    inline void SetEvaluationBlockSize(const unsigned eval_block_size)
    { _uiEvaluationBlockSize = eval_block_size; }

  protected:
    /// randomizer instance used for evaluation order computation
    BIAS::Random randomizer_;
    /// the helper class for solution computation and sample evaluation
    RANSACEvaluatorInterface<SolutionType> *_RANSACEvaluator;
    /// the number of samples needed by GetSampleSolutions
    unsigned _uiSampleSize;
    /// the minimu and maximum number of samples investigated
    unsigned _uiMinSamples, _uiMaxSamples;
    /// the number of data for evaluation
    unsigned _uiDataSize;
    /// should termination be aborted when first good solution is found?
    bool _bGreedy;
    /// the number of datat points that is initially checked
    unsigned _uiNumInitialChecked;
    /// the minimum number of inlier from _uiNumInitialChecked
    unsigned _uiMinNumInitialInliers;
    /// should the solutions be refined?
    bool _bRefineSolutions;
    /// after EvaluationBlockSize, bad solutions are removed
    unsigned _uiEvaluationBlockSize;
    /// just to check if the class is used correctly
    bool _bInitialised;
    /// prevents that calls to GenerateSample_() generate the same random
    /// sample if sample solution creation fails!
    int sample_offset;

    struct SolStruct {
      SolutionType solution;
      /// evaluation of the solution has taken place up to eval_index
      unsigned eval_index;
      // the accumulated score of the so-far evaluated datas
      double score;
      // the number of inliers of the so-far evaluated datas
      unsigned num_inliers;
      /** inlier indicator for the data in the order given by _EvaluationOrder
          i.e. inliers[i] indicates if data[_EvaluationOrder[i]] is an inlier*/
      std::vector<bool> inliers;
      /// has a refined version of this solution lareday be added?
      bool refined;
    };

    /// all (remaining) solutions
    std::list<SolStruct> _Solutions;
    /// the order in which data is used for solutions evaluation
    std::vector<unsigned> _EvaluationOrder;

    void GenerateEvaluationOrder_(const unsigned data_size);

    /** returns the number of generated solution */
    int GenerateSolutions_(const unsigned block_num,
                           const unsigned sample_size,
                           const unsigned data_size);

    /** returns the number of succesfully refined solutions */
    int GenerateRefinedSolutions_(const unsigned data_size);

    /** returns the number of rejected solutions */
    int InitialRejectTcd_(const unsigned min_num_inliers,
                          const unsigned num_checked);

    void EvaluateSolutions_(const unsigned start_index,
                            const unsigned data_size);

    /** returns the number of rejected solutions */
    int RejectSolutions_(const unsigned eval_index);

    inline bool IsInlier_(typename std::list<SolStruct>::iterator solution,
                          const unsigned data_index, double& score);

    int GetSolution_(const unsigned min_num_inliers, const double max_score,
                     SolutionType& solution, std::vector<bool>& inliers,
                     unsigned& num_inliers, double & score) const;

    inline void ConvertInliers_(const std::vector<bool>& inliers_in_eval_order,
                                std::vector<bool>& inliers) const;

    const struct SolStruct *
    GetSolutionMaxInliers_(const unsigned min_num_inliers,
                           const double max_score) const
    {
      SolStruct const *best = NULL;
      unsigned max_inliers = 0;
      double min_score = std::numeric_limits<double>::max();
      typename std::list<SolStruct>::const_iterator it;
      for (it=_Solutions.begin(); it!=_Solutions.end();it++){
        BCDOUT(D_PRANSAC_GetSolution, "solution: "<<it->solution<<std::endl);
        if (it->num_inliers>max_inliers){
          max_inliers = it->num_inliers;
          min_score = it->score;
          BCDOUT(D_PRANSAC_GetSolution, "   is best (num inliers "<<max_inliers
            <<", score "<<min_score<<")"<<std::endl);
          best = &(*it);
        } else {
          BCDOUT(D_PRANSAC_GetSolution, "           (num inliers "<<max_inliers
            <<", score "<<min_score<<")"<<std::endl);
        }
        if (it->num_inliers==max_inliers && it->score < min_score){
          min_score = it->score;
          best = &(*it);
          BCDOUT(D_PRANSAC_GetSolution, "   new best score "<<min_score
            <<std::endl);
        }
      }

      return best;
    }

    const struct SolStruct *
    GetSolutionMinScore_(const unsigned min_num_inliers,
                         const double max_score) const
    {
      SolStruct const *best = NULL;
      double min_score = std::numeric_limits<double>::max();
      typename std::list<SolStruct>::const_iterator it;
      for (it=_Solutions.begin(); it!=_Solutions.end(); it++){
        BCDOUT(D_PRANSAC_GetSolution, "solution: "<<it->solution
          <<std::endl);
        if (it->score < min_score){
          min_score = it->score;
          best = &(*it);
          BCDOUT(D_PRANSAC_GetSolution, "      is best (min_score "<<min_score
            <<" / num inliers: "<<it->num_inliers<<" )"<<std::endl);
        } else {
          BCDOUT(D_PRANSAC_GetSolution, "   score: "<<it->score
            <<" / num inliers: "<<it->num_inliers<<std::endl);
        }
      }
      return best;
    }

  };

  ///////////////////////////////////////////////////////////////

  template <class SolutionType>
  PreemptiveRANSAC<SolutionType>::
  PreemptiveRANSAC(RANSACEvaluatorInterface<SolutionType> *shi)
    : _RANSACEvaluator(shi), _uiSampleSize(0u), _uiMinSamples(50),
      _uiMaxSamples(1000),
      _uiDataSize(0), _bGreedy(true), _uiNumInitialChecked(8),
      _uiMinNumInitialInliers(1),
      _bRefineSolutions(true), _uiEvaluationBlockSize(50),
      sample_offset(0)
  {
    if (!_RANSACEvaluator){
      BEXCEPTION("PreemptiveRANSAC::PreemptiveRANSAC(): Invalid "
                 <<"(null) argument.");
    }
    _uiSampleSize = _RANSACEvaluator->GetMinNumSamplesForSolutionComputation();

    NewDebugLevel("D_PRANSAC_SolveMaster");
    NewDebugLevel("D_PRANSAC_EvalOrder");
    NewDebugLevel("D_PRANSAC_Evaluate");
    NewDebugLevel("D_PRANSAC_InitialReject");
    NewDebugLevel("D_PRANSAC_Inlier");
    NewDebugLevel("D_PRANSAC_GetSolution");
    NewDebugLevel("D_PRANSAC_GenerateSolutions");
  }


  template <class SolutionType>
  void PreemptiveRANSAC<SolutionType>::
  Init()
  {
    BIASASSERT(_RANSACEvaluator);
    _uiDataSize = _RANSACEvaluator->GetNumMeasurements();
    _Solutions.clear();
    _EvaluationOrder.clear();
    GenerateEvaluationOrder_(_uiDataSize);
    _bInitialised = true;
  }


  template <class SolutionType>
  void PreemptiveRANSAC<SolutionType>::
  AddSolutionGuesses(const std::vector<SolutionType>& guesses)
  {
    BIASASSERT(_uiDataSize!=0);
    typename std::vector<SolutionType>::const_iterator it;
    SolStruct st;
    for (it=guesses.begin(); it!=guesses.end(); it++){
      st.solution = *it;
      st.eval_index = 0;
      st.score = 0.0;
      st.num_inliers = 0;
      st.inliers.resize(_uiDataSize);
      st.refined = false;
      _Solutions.push_back(st);
    }
  }


  template <class SolutionType>
  int PreemptiveRANSAC<SolutionType>::
  SolveMaster(const double inlying_data_fraction, const double max_score,
              SolutionType &solution, std::vector<bool> &inliers,
              unsigned &num_inliers, double& score,
              const bool auto_compute_max_samples)
  {
    BIASASSERT(_RANSACEvaluator);
    if (!_bInitialised){
      BIASERR("PreemptiveRANSAC::SolveMaster(): Call Init() before calling "
              <<"SolveMAster!");
    }
    BIASASSERT(_bInitialised);
    _bInitialised = false;

    unsigned oldMinSamples = _uiMinSamples;
    if (auto_compute_max_samples){
      // the following euqation stems from Phil Torr's thesis
      const double prob_of_good_solution = 0.9999;
      _uiMaxSamples =
        (unsigned)ceil(log(1.0-prob_of_good_solution)/
                       log(1.0-pow(inlying_data_fraction,
                                   (double)(_uiSampleSize))));
      if (_uiMinSamples>_uiMaxSamples){
        _uiMinSamples = _uiMaxSamples;
      }
    }

    if (_uiDataSize<_uiSampleSize){
      // need at least _uiSampleSize samples to generate a single solution
      return PRANSAC_NOTE_ENOUGH_SAMPLES;
    }
    const int min_num_inliers =
      (int)rint(inlying_data_fraction * (double)_uiDataSize);
    BIASASSERT(_uiDataSize!=0);
    BIASASSERT(_uiMaxSamples>=_uiMinSamples);
    const unsigned num_blocks = _uiMaxSamples / _uiMinSamples;

    unsigned oldNumInitialChecked = _uiNumInitialChecked;
    unsigned oldMinNumInitialInliers = _uiMinNumInitialInliers;
    if (_uiDataSize<_uiNumInitialChecked){
      std::cerr << "Only "<<_uiDataSize<<" measurements are available, but the "
                <<"initial check has been requested for "<<_uiNumInitialChecked
                <<" measurements. Checking all "<<_uiDataSize
                <<" available measurememts\n" ;
      _uiNumInitialChecked = _uiDataSize;
      unsigned num_initial_expected_inliers =
        (unsigned)floor(inlying_data_fraction * (double) _uiNumInitialChecked);
      if (_uiMinNumInitialInliers>=num_initial_expected_inliers){
        _uiMinNumInitialInliers = num_initial_expected_inliers;
        if (_uiMinNumInitialInliers>0){
          _uiMinNumInitialInliers--;
        }
      }
    }
    unsigned num_initial_expected_inliers =
      (unsigned)floor(inlying_data_fraction * (double) _uiNumInitialChecked);
    if (_uiMinNumInitialInliers>num_initial_expected_inliers){
      std::cerr << "the parameters inyling_data_fraction ("
                <<inlying_data_fraction<<") and the ratio of "
                <<"MinNumInitialInliers / NumInitialChecked ("
                <<(double)_uiMinNumInitialInliers/(double)_uiNumInitialChecked
                <<") are not consistent. The latter must be smaller than the "
                <<"former.\n";
      return PRANSAC_INVALID_PARAMETERS;
    }

    unsigned num_generated_solutions = 0;
    unsigned num_initially_rejected_solutions = 0;
    for (unsigned block=0; block<num_blocks; block++){
      BCDOUT(D_PRANSAC_SolveMaster,"\n########## block "<<block<<" #######\n");
      // generate and evaluate raw hypothesis
      num_generated_solutions +=
        GenerateSolutions_(block, _uiSampleSize, _uiDataSize);
      BCDOUT(D_PRANSAC_SolveMaster,"initially generated "<<_Solutions.size()
             <<" solutions\n");

      num_initially_rejected_solutions +=
        InitialRejectTcd_(_uiMinNumInitialInliers, _uiNumInitialChecked);
      BCDOUT(D_PRANSAC_SolveMaster, _Solutions.size()<<" solutions remained "
             "after initial rejection\n");
      if (_Solutions.empty()) continue;

      EvaluateSolutions_(_uiNumInitialChecked, _uiDataSize);
      BCDOUT(D_PRANSAC_SolveMaster, _Solutions.size()<<" solutions remained "
             "after evaluation of unrefined solutions\n");
      if (_Solutions.empty()) continue;

      // generate and evaluate the refined hypothesis
      if (_bRefineSolutions && !_Solutions.empty()){
        int num_refined = GenerateRefinedSolutions_(_uiDataSize);
        if (num_refined>0){
          BCDOUT(D_PRANSAC_SolveMaster, _Solutions.size()<<" solutions "
                 "after adding refinements\n");

          int num_rejected =
            InitialRejectTcd_(_uiMinNumInitialInliers, _uiNumInitialChecked);
          BCDOUT(D_PRANSAC_SolveMaster, _Solutions.size()<<" solutions "
                 "after initial rejection of refinements\n");
          if (_Solutions.empty()) continue;

          if (num_rejected!=num_refined){
            EvaluateSolutions_(_uiNumInitialChecked, _uiDataSize);
            BCDOUT(D_PRANSAC_SolveMaster, _Solutions.size()<<" solutions "
                   "after evaluation of refinements\n");
            if (_Solutions.empty()) continue;
          } else {
            BCDOUT(D_PRANSAC_SolveMaster, "no refined solution passed the "
                   "initial test\n");
          }
        } else {
          BCDOUT(D_PRANSAC_SolveMaster, "no refined solutions\n");
        }
      }

      int solution_res = GetSolution_(min_num_inliers, max_score,
                                      solution, inliers, num_inliers, score);
      if (_bGreedy && solution_res==PRANSAC_OK){
        BCDOUT(D_PRANSAC_SolveMaster, "solution found!\n");
        // restore internal state
        _uiNumInitialChecked = oldNumInitialChecked;
        _uiMinNumInitialInliers = oldMinNumInitialInliers;
        _uiMinSamples = oldMinSamples;
        return 0;
      }
    } /// for (int block=0; block<num_blocks; block++){

    // restore internal state
    _uiNumInitialChecked = oldNumInitialChecked;
    _uiMinNumInitialInliers = oldMinNumInitialInliers;
    _uiMinSamples = oldMinSamples;

    const double initially_rejected_solutions_ratio =
      (double)(num_initially_rejected_solutions) /
      (double)(num_generated_solutions);
    const double max_initially_rejected_solution_ratio = 0.95;
    if (initially_rejected_solutions_ratio >
        max_initially_rejected_solution_ratio){
      BIASERR("More than "<<max_initially_rejected_solution_ratio*100
              << "% of the solutions have been rejected by the Tcd test "
              << "(c="<<_uiMinNumInitialInliers<<", d="
              <<_uiNumInitialChecked<<").\n");
      // retry when the parameters are reasonable
      static int num_retries = 0;
      const unsigned reasonable_min_num_initial_checked = 5;
      const unsigned reasonable_min_num_initial_inliers = 2;
      if ( _uiMinNumInitialInliers <= reasonable_min_num_initial_inliers &&
           _uiNumInitialChecked >= reasonable_min_num_initial_checked){
        BIASERR("Retrying with different evaluation order\n");
        if (num_retries < 1 ){
          num_retries++;
          GenerateEvaluationOrder_(_uiDataSize);
          SolveMaster(inlying_data_fraction, max_score, solution,
                      inliers, num_inliers, score, auto_compute_max_samples);
        }
      } else {
        BIASERR("Rerunning PreemptiveRANSAC with more reasonable parameters "
                <<"for \"NumInitialChecked\" (is "<<_uiNumInitialChecked
                <<", should be at least "<<reasonable_min_num_initial_checked
                <<") and \"MinNumInitialInliers\" (is "
                <<_uiMinNumInitialInliers <<" an should be at most "
                <<reasonable_min_num_initial_inliers
                <<") could be successfull.");
        return PRANSAC_SUBOPTIMAL_PARAMETERS;
      }
    }

    // check if any solution is left
    int solution_res = GetSolution_(min_num_inliers, max_score,
                                    solution, inliers, num_inliers, score);
    if (solution_res==PRANSAC_OK){
      BCDOUT(D_PRANSAC_SolveMaster, "solution found!\n");
      return 0;
    } else if (solution_res>0){
      BCDOUT(D_PRANSAC_SolveMaster, "solution found, but minimum quality "
             <<"criteria not matched! Best score "<<score<<" (expected max "
             <<max_score<<") / num inliers "<<num_inliers<<" (expected min "
             <<min_num_inliers<<")\n");
      BIASERR("solution found, but minimum quality "
             <<"criteria not matched! Best score "<<score<<" (expected max "
             <<max_score<<") / num inliers "<<num_inliers<<" (expected min "
             <<min_num_inliers<<")");
      return solution_res;
    }


    BIASERR("PreemptiveRANSAC found no solution");
    return PRANSAC_NO_SOLUTION;
  }

  template <class SolutionType>
  void PreemptiveRANSAC<SolutionType>::
  GenerateEvaluationOrder_(const unsigned data_size)
  {    
    _EvaluationOrder.clear();
    const unsigned ds = data_size;
    std::list<unsigned> mlist;
    for (unsigned i=0; i<ds; i++)
      mlist.push_back(i);

    unsigned index;
    typename std::list<unsigned>::iterator it;
    BCDOUT(D_PRANSAC_EvalOrder, "evaluation order: ");
    for (unsigned i=0; i<ds; i++){
      index = randomizer_.GetUniformDistributedInt(0u, ds-i-1);
      BIASASSERT(index>=0 && index<ds-i);
      it = mlist.begin();
      for (unsigned k=0; k<index; k++) it++;
      _EvaluationOrder.push_back(*it);
      BCDOUT(D_PRANSAC_EvalOrder, _EvaluationOrder.back()<<" ");
      mlist.erase(it);
    }
    BCDOUT(D_PRANSAC_EvalOrder, std::endl);
  }


  template <class SolutionType>
  int PreemptiveRANSAC<SolutionType>::
  GenerateSolutions_(const unsigned block_num, const unsigned sample_size,
                     const unsigned data_size)
  {
    const unsigned max_sample = (block_num+1)*_uiMinSamples;
    std::vector<unsigned> which_samples;
    std::vector<SolutionType> solutions;
    typename std::vector<SolutionType>::const_iterator it;
    SolStruct st;

    if (block_num == 0) sample_offset = 0;
    const int max_tries = 25;
    int num, tries;
    int num_solutions = 0;
    BIASASSERT(_RANSACEvaluator);
    BCDOUT(D_PRANSAC_GenerateSolutions, "generating "<<block_num
           <<"-th block with "<<_uiMinSamples <<" solutions\n");
    for (unsigned sample=block_num * _uiMinSamples;
         sample<max_sample; sample++){
      tries = num = 0;
      do {
        if (_RANSACEvaluator->GenerateSamples(sample+sample_offset, sample_size,
                                        data_size, which_samples)) {
          num = _RANSACEvaluator->GetSampleSolutions(which_samples, solutions);
          BCDOUT(D_PRANSAC_GenerateSolutions, "samples: ");
          for (unsigned i=0; i<which_samples.size(); i++){
            BCDOUT(D_PRANSAC_GenerateSolutions, which_samples[i]<<" ");
          }
          BCDOUT(D_PRANSAC_GenerateSolutions, "  resulted in "
                 <<solutions.size()<<" solutions\n");
          for (it=solutions.begin(); it!=solutions.end(); it++){
            st.solution = *it;
            st.eval_index = 0;
            st.score = 0.0;
            st.num_inliers = 0;
            st.inliers.resize(data_size);
            st.refined = false;
            _Solutions.push_back(st);
            num_solutions++;
          }
        } else {
          BIASERR("sample generation failed!");
          return -1;
        }
        if (num == 0) {
          tries++;
          sample_offset++;
        }
      } while (num==0 && tries<max_tries);
    }
    BCDOUT(D_PRANSAC_GenerateSolutions, "generated "<<block_num
           <<"-th block containing "<<num_solutions<<" solutions\n");
    return num_solutions;
  }

  template <class SolutionType>
  int PreemptiveRANSAC<SolutionType>::
  GenerateRefinedSolutions_(const unsigned data_size)
  {
    typename std::list<SolStruct>::iterator it;
    SolStruct st;
    st.refined = true;
    std::vector<bool> inliers;
    int num_refined = 0;
    BIASASSERT(_RANSACEvaluator);
    for (it=_Solutions.begin(); it!=_Solutions.end(); it++){
      if (!it->refined){
        BIASASSERT(it->eval_index == data_size);
        ConvertInliers_(it->inliers, inliers);
        st.solution = it->solution;
        if (_RANSACEvaluator->RefineSolution(inliers, st.solution)){
          st.eval_index = 0;
          st.score = 0.0;
          st.num_inliers = 0;
          st.inliers.resize(data_size);
          _Solutions.push_back(st);
          num_refined++;
        }
      }
    }
    return num_refined;
  }

  template <class SolutionType>
  int PreemptiveRANSAC<SolutionType>::
  InitialRejectTcd_(const unsigned min_num_inliers,
                    const unsigned num_checked)
  {
    BIASASSERT(min_num_inliers<=num_checked);
    typename std::list<SolStruct>::iterator it;
    unsigned data = 0;
    double score = 0.0;
    int num_rejected = 0;
    BCDOUT(D_PRANSAC_InitialReject, "initial reject, checking samples: ");
    for (data=0; data<num_checked; data++){
      BCDOUT(D_PRANSAC_InitialReject, _EvaluationOrder[data]<<" ");
    }
    BCDOUT(D_PRANSAC_InitialReject, std::endl);

    for (it=_Solutions.begin(); it!=_Solutions.end();){
      if (it->eval_index>num_checked) { it++; continue; }
      BCDOUT(D_PRANSAC_InitialReject, " (");
      for (data=0; data<num_checked; data++){
        IsInlier_(it, data, score);
        BCDOUT(D_PRANSAC_InitialReject, ((it->inliers[data])?("+"):("-")));
      }
      if (it->num_inliers<min_num_inliers){
        it = _Solutions.erase(it);
        num_rejected++;
        BCDOUT(D_PRANSAC_InitialReject, ")*");
      } else {
        it++;
        BCDOUT(D_PRANSAC_InitialReject, ").");
      }
    }
    BCDOUT(D_PRANSAC_InitialReject, "\n");
    if (_Solutions.empty()){
      std::cout << "initially checked "<<num_checked
                <<" samples and expected at least "<<min_num_inliers
                <<" inliers. No solutions remaining.\n";
    }
    return num_rejected;
  }

  template <class SolutionType>
  bool PreemptiveRANSAC<SolutionType>::
  IsInlier_(typename std::list<SolStruct>::iterator solution,
            const unsigned eval_index, double& score)
  {
    BCDOUT(D_PRANSAC_Inlier, "eval_index: "<<eval_index<<"  ");
    BCDOUT(D_PRANSAC_Inlier, "solution->eval_index: "<<solution->eval_index);
    if (solution->eval_index<=eval_index){
      // cannot leave indices unevaluated
      //std::cerr<<solution->eval_index<<"  "<<eval_index<<std::endl;
      BIASASSERT(solution->eval_index==eval_index);
      BIASASSERT(solution->inliers.size()>eval_index);
      BIASASSERT(_EvaluationOrder.size()>eval_index);
      BCDOUT(D_PRANSAC_Inlier, " (new score) "<<std::endl);
      BCDOUT(D_PRANSAC_Inlier, "_EvaluationOrder[eval_index] = "
             <<_EvaluationOrder[eval_index]<<std::endl);
      BIASASSERT(_RANSACEvaluator);
      solution->inliers[eval_index] =
        _RANSACEvaluator->IsInlier(solution->solution,
                                   _EvaluationOrder[eval_index], score);

      if (solution->inliers[eval_index]){
        solution->score += score;
        solution->num_inliers++;
      }
      solution->eval_index++;
    } else {
      BCDOUT(D_PRANSAC_Inlier, " (already scored) "<<std::endl);
      score = NAN;
    }
    BCDOUT(D_PRANSAC_Inlier, "inlier: "<<std::boolalpha
           <<solution->inliers[eval_index]<<std::endl);
    return solution->inliers[eval_index];
  }


  template <class SolutionType>
  void PreemptiveRANSAC<SolutionType>::
  EvaluateSolutions_(const unsigned start_index, const unsigned data_size)
  {
    if (start_index>=data_size){
      return;
    }
    typename std::list<SolStruct>::iterator it;
    double score = 0.0;
    for (unsigned data=start_index; data<data_size; data++){
      for (it=_Solutions.begin(); it!=_Solutions.end(); it++){
        if (it->eval_index>data) { continue; }
        IsInlier_(it, data, score);
      }
      if ( ( ((int)data+1-(int)start_index) % (int)_uiEvaluationBlockSize)
           == 0){
        BCDOUT(D_PRANSAC_Evaluate, "--- eval "<<data<<" ---\n");
        RejectSolutions_(data);
      }
    }
  }

  template <class SolutionType>
  int PreemptiveRANSAC<SolutionType>::
  RejectSolutions_(const unsigned eval_index)
  {
    // always keep the better half
    std::vector<double> scores;
    typename std::list<SolStruct>::iterator it;
    for (it=_Solutions.begin(); it!=_Solutions.end(); it++){
      if (it->eval_index>eval_index+1) { continue; }
      scores.push_back(it->score);
    }
    if (scores.empty()) {
      BCDOUT(D_PRANSAC_Evaluate, "no solutions left to evaluate\n");
      return 0;
    }
    Median1D<double> med;
    med.Compute(scores);
    const double thresh = med.GetMedian();
    BCDOUT(D_PRANSAC_Evaluate, "score threshold = "<<thresh<<std::endl);
    int num_rejected=0;
    for (it=_Solutions.begin(); it!=_Solutions.end();){
      if (it->score > thresh){
        it = _Solutions.erase(it);
        BCDOUT(D_PRANSAC_Evaluate, "*");
        num_rejected++;
      } else {
        it++;
        BCDOUT(D_PRANSAC_Evaluate, ".");
      }
    }
    BCDOUT(D_PRANSAC_Evaluate, std::endl);
    BCDOUT(D_PRANSAC_Evaluate, _Solutions.size()<<" solutions remaining\n");

    return num_rejected;
  }


  template <class SolutionType>
  int PreemptiveRANSAC<SolutionType>::
  GetSolution_(const unsigned min_num_inliers, const double max_score,
               SolutionType& solution, std::vector<bool>& inliers,
               unsigned& num_inliers, double & score) const
  {
    if (_Solutions.empty()) {
      return PRANSAC_NO_SOLUTION_LEFT;
    }

    const SolStruct *best_sol =
      GetSolutionMinScore_(min_num_inliers, max_score);
    //GetSolutionMaxInliers_(min_num_inliers, max_score);

    if (best_sol){
      score = best_sol->score;
      num_inliers = best_sol->num_inliers;
      solution = best_sol->solution;
      BCDOUT(D_PRANSAC_GetSolution, "\nbest solution is: "
             <<solution<<"  score: "<<score<<" / num inliers: "
             <<num_inliers<<std::endl);
      ConvertInliers_(best_sol->inliers, inliers);
      bool enough_inliers = (best_sol->num_inliers >= min_num_inliers);
      bool score_smaller_than_max_score = (best_sol->score <= max_score);
      if (enough_inliers && score_smaller_than_max_score){
        BCDOUT(D_PRANSAC_GetSolution, "num_inliers: "<<num_inliers
               <<"\nscore: "<<score<<"\n");
        return PRANSAC_OK;
      } else {
        int res = 0;
        BCDOUT(D_PRANSAC_GetSolution, "minimum quality criteria not met "
               "(max_score: "<<max_score<<" / min inliers "<<min_num_inliers
               <<")\n");
        if (!enough_inliers){
          BCDOUT(D_PRANSAC_GetSolution, "not enough inliers "
                 <<"(found: "<<best_sol->num_inliers<<" / expected: "
                 <<min_num_inliers<<")\n");
          res = res | PRANSAC_NOT_ENOUGH_INLIERS;
        }
        if (!score_smaller_than_max_score){
          BCDOUT(D_PRANSAC_GetSolution, "score too big "
                 <<"(found: "<<best_sol->score<<" / expected: "
                 <<max_score<<")\n");
          res = res | PRANSAC_SCORE_TOO_BIG;
        }
        return res;
      }
    } else {
      BIASERR("no best solution returned");
      BIASABORT;
    }

    return PRANSAC_NO_SOLUTION_LEFT;
  }


  template <class SolutionType>
  void PreemptiveRANSAC<SolutionType>::
  ConvertInliers_(const std::vector<bool>& inliers_in_eval_order,
                  std::vector<bool>& inliers) const
  {
    // convert the inlier vector
    inliers.resize(inliers_in_eval_order.size());
    const unsigned data_size = (unsigned)inliers.size();
    BIASASSERT(data_size == _EvaluationOrder.size());
    for (unsigned i=0; i<data_size; i++){
      BIASASSERT(_EvaluationOrder[i]<data_size);
      inliers[_EvaluationOrder[i]] = inliers_in_eval_order[i];
    }
  }

}
#endif // __PreemptiveRANSAC_hh__