COSAC.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 __COSAC_hh__
#define __COSAC_hh__

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

#include <Base/Debug/Debug.hh>
#include <Base/Debug/Error.hh>
#include <Base/Debug/Exception.hh>
#include <Base/Math/BinomialCoefficient.hh>
#include <MathAlgo/RANSACEvaluatorInterface.hh>

#include <vector>
#include <list>
#include <limits>
#include <cmath>

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

#define COSAC_TOO_MANY_SOLUTIONS -5
#define COSAC_NOT_ENOUGH_MEASUREMENTS -4
#define COSAC_NO_SOLUTION_LEFT -3
#define COSAC_INVALID_ARGUMENTS -2
#define COSAC_NO_SOLUTIONS -1
#define COSAC_OK 0
#define COSAC_NOT_ENOUGH_INLIERS 1
#define COSAC_SCORE_TOO_BIG 2

namespace BIAS {

  /** @class COSAC
      @brief Complete Sampling Consesus
      
      Samples the space of possible solutions completely. The algorithm 
      (complete sampling and evaluation) 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;

      Afterwards instantiate COSAC with a pointer to the sibling of 
      RANSACEvaluatorInterface as argment to the constructor and call 

      SolveMaster()

      @author woelk 01/2010 (c) www.vision-n.de */
  template <class SolutionType>
  class COSAC : public Debug
  {
  public:
    /// c'tor
    COSAC(RANSACEvaluatorInterface<SolutionType> *shi);

    /// d'tor
    virtual ~COSAC() {};

    /** @returns COSAC_OK when a solution could be found,
        a positive number when a solution could be found which 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);

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

  protected:
    /// the helper class for solution computation and sample evaluation
    RANSACEvaluatorInterface<SolutionType> *RANSACEvaluator_;
    /// the number of samples needed by GetSampleSolutions
    unsigned _uiSampleSize;
    /// the number of data for evaluation
    unsigned _uiDataSize;
    /// should the solutions be refined?
    bool _bRefineSolutions;

    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[i] is an inlier*/
      std::vector<bool> inliers;
      /// has a refined version of this solution already been added?
      bool refined;
    };
    
    /// all solutions
    std::list<SolStruct> _Solutions;

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

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

    /** evaluates the solutions */
    void EvaluateSolutions_(const unsigned data_size);

    /** Returns the next combination of samples from which a solution can be 
        computed. Returns false when all combinations have been returned.
        The argument vector which_samples must be empty on initial call */
    bool GetNextCombination_(const unsigned sample_size,
                             const unsigned data_size,
                             std::vector<unsigned> &which_samples);

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

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

    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_COSAC_GetSolution, "solution: "<<it->solution<<std::endl);
        if (it->num_inliers>max_inliers){
          max_inliers = it->num_inliers;
          min_score = it->score;
          BCDOUT(D_COSAC_GetSolution, "   is best (num inliers "<<max_inliers
                 <<", score "<<min_score<<")"<<std::endl);
          best = &(*it);
        } else {
          BCDOUT(D_COSAC_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_COSAC_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_COSAC_GetSolution, "solution: "<<it->solution
               <<std::endl);
        if (it->score < min_score){
          min_score = it->score;
          best = &(*it);
          BCDOUT(D_COSAC_GetSolution, "      is best (min_score "<<min_score
                 <<" / num inliers: "<<it->num_inliers<<" )"<<std::endl);
        } else {
          BCDOUT(D_COSAC_GetSolution, "   score: "<<it->score
                 <<" / num inliers: "<<it->num_inliers<<std::endl);
        }
      }
      return best;
    }

  }; // class

  ////////////////////////////////////////////////////////////////////////////
  // implementation
  ///////////////////////////////////////////////////////////////////////////

  template <class SolutionType>
  COSAC<SolutionType>::
  COSAC(RANSACEvaluatorInterface<SolutionType> *shi)
    : RANSACEvaluator_(shi), _uiSampleSize(0), _uiDataSize(0), 
      _bRefineSolutions(true), _Solutions()
  { 
    if (!RANSACEvaluator_){
      BEXCEPTION("COSAC::COSAC(): Invalid (null) argument.");
    }
    _uiSampleSize = RANSACEvaluator_->GetMinNumSamplesForSolutionComputation();

    NewDebugLevel("D_COSAC_Inlier");
    NewDebugLevel("D_COSAC_GetSolution");
    NewDebugLevel("D_COSAC_SolveMaster");
    NewDebugLevel("D_COSAC_Evaluate");
  }


  template <class SolutionType>
  int COSAC<SolutionType>::
  SolveMaster(const double inlying_data_fraction, const double max_score,
              SolutionType &solution, std::vector<bool> &inliers, 
              unsigned &num_inliers, double& score)
  {
    _uiDataSize = RANSACEvaluator_->GetNumMeasurements();
    if (_uiDataSize < _uiSampleSize){
      return COSAC_NOT_ENOUGH_MEASUREMENTS;
    }
    _Solutions.clear();

    try {
      unsigned num_solutions = 
        BinomialCoefficient::Compute(_uiDataSize, _uiSampleSize);
      const unsigned max_num_solutions = 5000;
      if (num_solutions>max_num_solutions){
        BIASERR("COSAC::SolveMaster(): The number of solutions is very big "
                <<"(>"
                <<max_num_solutions<<") and evaluation of every solution "
                <<"might take a very long time. The usage of "
                <<"PreemptiveRANSAC instead of COSAC is suggested for this"
                <<" case.");
        return COSAC_TOO_MANY_SOLUTIONS;
      }
    } catch (BaseException &ex){
      BIASERR("COSAC::SolveMaster(): The number of solutions is very big (>"
              <<std::numeric_limits<unsigned>::max()
              <<") and evaluation of every solution "
              <<"might take a very long time. The usage of "
              <<"PreemptiveRANSAC instead of COSAC is suggested for this"
              <<" case."<<ex.What());
      return COSAC_TOO_MANY_SOLUTIONS;
    }

    if (inlying_data_fraction<0.0 || inlying_data_fraction>1.0){
      return COSAC_INVALID_ARGUMENTS;
    }
    GenerateSolutions_(_uiSampleSize, _uiDataSize);
    if (_Solutions.empty()){
      return COSAC_NO_SOLUTIONS;
    }

    EvaluateSolutions_(_uiDataSize);

    if (_bRefineSolutions){
      GenerateRefinedSolutions_(_uiDataSize);
      if (_Solutions.empty()){
        return COSAC_NO_SOLUTIONS;
      }
      
      EvaluateSolutions_(_uiDataSize);
    }

    const unsigned min_num_inliers = 
      (unsigned)floor(inlying_data_fraction*_uiDataSize);
    BIASASSERT(min_num_inliers<=_uiDataSize);
    int res = GetSolution_(min_num_inliers, max_score, 
                           solution, inliers, num_inliers, score);
    
    return res;
  }


  template <class SolutionType>
  int COSAC<SolutionType>::
  GenerateSolutions_(const unsigned sample_size, const unsigned data_size)
  {
    unsigned num_solutions = 0;
    std::vector<unsigned> which_samples;
    std::vector<SolutionType> solutions;
    SolStruct st;
    st.refined = false;
    st.eval_index = 0;
    st.score = 0.0;
    st.num_inliers = 0;
    st.inliers.resize(data_size);
    while (GetNextCombination_(sample_size, data_size, which_samples)){
      int res = RANSACEvaluator_->GetSampleSolutions(which_samples, solutions);
      if (res == 0){
        for (unsigned sol_idx=0; sol_idx<solutions.size(); sol_idx++){
          st.solution = solutions[sol_idx];
          BIASASSERT(st.inliers.size()==data_size);
          _Solutions.push_back(st);
          num_solutions++;
        }
      }
    }
    return num_solutions; 
  }


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


  template <class SolutionType>
  void COSAC<SolutionType>::
  EvaluateSolutions_(const unsigned data_size)
  {
    typename std::list<SolStruct>::iterator it;
    double score = 0.0;
    for (unsigned data_idx=0; data_idx<data_size; data_idx++){
      for (it=_Solutions.begin(); it!=_Solutions.end(); it++){
        IsInlier_(it, data_idx, score);
      }
//       bool rejet_solutions = 
//         ((data_idx%_uiEvaluationBlockSize)==0) && (data_idx!=0);
//       if (reject_solutions){
//         RejectSolutions_(data);
//       }
    }
  }


//   template <class SolutionType>
//   int COSAC<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_COSAC_Evaluate, "no solutions left to evaluate\n");
//       return 0;
//     }
//     Median1D<double> med;
//     med.Compute(scores);
//     const double thresh = med.GetMedian();
//     BCDOUT(D_COSAC_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_COSAC_Evaluate, "*");
//         num_rejected++;
//       } else {
//         it++;
//         BCDOUT(D_COSAC_Evaluate, ".");
//       }
//     }
//     BCDOUT(D_COSAC_Evaluate, std::endl);
//     BCDOUT(D_COSAC_Evaluate, _Solutions.size()<<" solutions remaining\n");

//     return num_rejected;
//   }


  template <class SolutionType>
  bool COSAC<SolutionType>::
  IsInlier_(typename std::list<SolStruct>::iterator solution, 
            const unsigned eval_index, double& score)
  {
    BCDOUT(D_COSAC_Inlier, "eval_index: "<<eval_index<<"  ");
    BCDOUT(D_COSAC_Inlier, "solution->eval_index: "<<solution->eval_index);
    if (solution->eval_index<=eval_index){
      // cannot leave indices unevaluated
      if (solution->inliers.size()<=eval_index){
        std::cout << "eval_index: "<<eval_index<<"\ninliers.size(): "
                  <<solution->inliers.size()<<std::endl;
      }
      BIASASSERT(solution->eval_index==eval_index);
      BIASASSERT(solution->inliers.size()>eval_index);
      BCDOUT(D_COSAC_Inlier, " (new score) "<<std::endl);
      solution->inliers[eval_index] = 
        RANSACEvaluator_->IsInlier(solution->solution, eval_index, score);
      solution->score += score;
      if (solution->inliers[eval_index]){
        solution->num_inliers++;
      }
      solution->eval_index++;
    } else {
      BCDOUT(D_COSAC_Inlier, " (already scored) "<<std::endl);
      score = NAN;
    }
    BCDOUT(D_COSAC_Inlier, "inlier: "<<std::boolalpha
           <<solution->inliers[eval_index]<<std::endl);
    return solution->inliers[eval_index];
  }


  template <class SolutionType>
  int COSAC<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 COSAC_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_COSAC_GetSolution, "\nbest solution is: "
             <<solution<<"  score: "<<score<<" / num inliers: "
             <<num_inliers<<std::endl);
      inliers = best_sol->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_COSAC_GetSolution, "num_inliers: "<<num_inliers
               <<"\nscore: "<<score<<"\n");
        return COSAC_OK;
      } else {
        int res = 0;
        BCDOUT(D_COSAC_GetSolution, "minimum quality criteria not met "
               "(max_score: "<<max_score<<" / min inliers "<<min_num_inliers
               <<")\n");
        if (!enough_inliers){
          BCDOUT(D_COSAC_GetSolution, "not enough inliers "
                 <<"(found: "<<best_sol->num_inliers<<" / expected: "
                 <<min_num_inliers<<")\n");
          res = res | COSAC_NOT_ENOUGH_INLIERS;
        }
        if (!score_smaller_than_max_score){
          BCDOUT(D_COSAC_GetSolution, "score too big "
                 <<"(found: "<<best_sol->score<<" / expected: "
                 <<max_score<<")\n");
          res = res | COSAC_SCORE_TOO_BIG;
        }
        return res;
      }
    } else {
      BIASERR("no best solution returned");
      BIASABORT;
    }

    return COSAC_NO_SOLUTION_LEFT;
  }

  
  template <class SolutionType>
  bool COSAC<SolutionType>::
  GetNextCombination_(const unsigned sample_size,
                      const unsigned data_size,
                      std::vector<unsigned> &which_samples)
  {
    if (data_size<sample_size) {
      return false;
    }
    if (sample_size<=0) { return false; }
    if (which_samples.empty()){
      for (unsigned i=0; i<sample_size; i++){
        which_samples.push_back(i);
      }
      return true;
    }
    BIASASSERT(which_samples.size()==sample_size);

    for (int i=0; i<(int)sample_size; i++){
      int index = sample_size-i-1;
      if (which_samples[index]+1 < data_size-i){
        which_samples[index]++;
        BIASASSERT(which_samples[index]<data_size);
        for (int k=index+1; k<(int)sample_size; k++){
          which_samples[k] = which_samples[index]+k-index;
          BIASASSERT(which_samples[k]<data_size);
        }
        return true;
      }
    }
    return false;
  }


} //namespace

#endif // __COSAC_hh__