MSAC.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 __MSAC_hh__
#define __MSAC_hh__

#include <Base/Common/BIASpragmaStart.hh>

#include <Base/Debug/Debug.hh>
#include <Base/Math/Random.hh>

#include <vector>
#include <iterator>
#include <limits>

namespace BIAS {

#define D_MSAC_SOLVEM  0x00000001
#define D_MSAC_SAMPLES 0x00000002
#define D_MSAC_INIT    0x00000004


/** @class MSAC
    @ingroup g_mathalgo
    @brief this class does something, but WHAT? Is it the M-Estimator SAmple Consensus ?? 
    @author MIP
*/

  template <class SolutionType>
  class BIASMathAlgo_EXPORT MSAC : public virtual BIAS::Debug
  {
  public:
    MSAC();
    virtual ~MSAC();

    // sets terminate_score and solution_count automatically
    int Init(int data_size);

    int Init(int data_size, double terminate_score, int solution_count);

    int SolveMaster(SolutionType &solution,
                    std::vector<bool> &inliers);

    virtual 
    int GetSampleSolution(std::vector<unsigned int> &which_samples,
                          SolutionType &solutions);

    virtual 
    bool EvaluateSolution(const SolutionType& solution,
                          std::vector<bool>& inlier, int& num_inliers,
                          double& evaluate_score);
    
    virtual 
    bool RefineSolution(std::vector<unsigned int>& which_samples,
                        SolutionType& solution,std::vector<bool>& new_inliers);

    virtual 
    bool GenerateSamples(int sample_index, 
                         std::vector<unsigned int>& which_samples);
    
 
  protected:
    bool _MSACRefineSolution;
    double _MSACConfidenceThresh; 
    double _MSACSigmaFac; 
    double _dExpectedSigma;
    double _dExpectedInlierFraction;
    int _iDataSize; // number of data
    int _iSolutionCount; // number of solutions to evaluate
    int _iSampleSize; // size of minimum data set needed to calc. sol
    double _dTerminateScore;
    
    bool _GenerateSamplesRandom(int sample_index,
                                std::vector<unsigned int>& which_samples);

    double _CalculateTerminateScore(double sigma, 
                                    double expected_inlier_fraction);

    int _CalculateSampleCount(double expected_inlier_fraction);

   }; // class





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

  template <class SolutionType>
  MSAC<SolutionType>::MSAC()
    : BIAS::Debug()
  {
    _MSACRefineSolution=false;
    _MSACConfidenceThresh=0.95;
    _MSACSigmaFac=1.96;
    _iDataSize=0;
    _iSolutionCount=1;
    _iSampleSize=1;
    _dTerminateScore=0.0;
    _dExpectedSigma=0.0;
    _dExpectedInlierFraction=0.8;
  }

  template <class SolutionType>
  MSAC<SolutionType>::~MSAC()
  {}

  
  template <class SolutionType>
  int MSAC<SolutionType>::Init(int data_size)
  {
    _iDataSize=data_size;
    _dTerminateScore=_CalculateTerminateScore(_dExpectedSigma, 
                                              _dExpectedInlierFraction);
    //    BIASERR("fast termination disabled"); _dTerminateScore=0.0;
    _iSolutionCount=_CalculateSampleCount(_dExpectedInlierFraction);

    return Init(data_size, _dTerminateScore, _iSolutionCount);
  }

  template <class SolutionType>
  int MSAC<SolutionType>::Init(int data_size, double terminate_score,
                               int solution_count)
  {
    _iDataSize=data_size;
    _dTerminateScore=terminate_score;
    //cout << "MSAC: aborting below score: "<<_dTerminateScore<<std::endl;
    _iSolutionCount=solution_count;
    //cout <<"MSAC: doing at most "<<_iSolutionCount<<" runs"<<std::endl;
    BIASCDOUT(D_MSAC_INIT, _iDataSize<<" data points whereof "
              <<_dExpectedInlierFraction*data_size<<" are expected to be "
              <<"inliers.\n Inliers are assumed normal distributed with sigma "
              <<_dExpectedSigma<<".\n Using confidence in solution of "
              <<_MSACConfidenceThresh
              <<" % with sample set size "<<_iSampleSize<<" leads to at most "
              <<_iSolutionCount
              <<" solutions to be evaluated.\n MSAC will abort on score below "
              <<_dTerminateScore<<"\n");
    BIASASSERT(_iDataSize>=_iSampleSize);
    return 0;
  }


  template <class SolutionType>
  int MSAC<SolutionType>::SolveMaster(SolutionType& solution,
                                      std::vector<bool>& inliers)
  {
    SolutionType best_sol, act_sol;
    std::vector<unsigned> which_samples;
    std::vector<bool> tmp(_iDataSize, false);
    std::vector<std::vector<bool> > inliervec(_iSolutionCount, tmp);
    double act_score, best_score=numeric_limits<double>::max();
    int sample_index, best_index=-1, act_num_inlier, best_num_inlier=0;

    which_samples.resize(_iSampleSize);
    inliers.resize(_iSolutionCount);

    for (sample_index = 0; sample_index < _iSolutionCount; sample_index++){
      //cout << "." << flush;
      //for (sample_index = 0; sample_index < 1; sample_index++){
      if (GenerateSamples(sample_index, which_samples)){
        if (!GetSampleSolution(which_samples, act_sol)){
          BIASERR("GetSampleSolution failed");
          BIASASSERT(false);
        } 
        if (!EvaluateSolution(act_sol, inliervec[sample_index], 
                              act_num_inlier, act_score)){
          BIASERR("Evaluate Solution failed");
          BIASASSERT(false);
        }
        BIASCDOUT(D_MSAC_SOLVEM, setw(3)<<sample_index<<"th solution: "
                  <<act_sol<<" with score "<<act_score<<" and "<<act_num_inlier
                  <<" inliers\n");

        if (_MSACRefineSolution && true){
          which_samples.clear();
          for (int i=0; i<_iDataSize; i++){
            if (inliervec[sample_index][i]){
              which_samples.push_back(i);
            }
          }
          
          if (!RefineSolution(which_samples, act_sol, 
                              inliervec[sample_index])){
            BIASERR("RefineSolution returned false");
            BIASASSERT(false);
          } 
          if (!EvaluateSolution(act_sol, inliervec[sample_index], 
                                act_num_inlier, act_score)){
            BIASERR("Evaluate Solution failed");
            BIASASSERT(false);
          }
          BIASCDOUT(D_MSAC_SOLVEM, setw(3)<<sample_index
                    <<"th refined solution: "
                    <<act_sol<<" with score "<<act_score<<" and "
                    <<act_num_inlier<<" inliers\n");
        }
        
        
#ifdef BIAS_DEBUG
        if (sample_index!=0){
          BIASCDOUT(D_MSAC_SOLVEM, "(best: "<<best_sol<<"/"<<best_score<<"/"
                    <<best_num_inlier<<std::endl);
        }
#endif
        if (best_score>act_score){
          best_score=act_score;
          best_index=sample_index;
          best_num_inlier=act_num_inlier;
          best_sol=act_sol;
        }
        if (best_score<_dTerminateScore){
          BIASCDOUT(D_MSAC_SOLVEM, "finished sampling, got a good score "
                    <<best_score<<std::endl);
          //cout << "rechaded score of "<<best_score<<", aborting\n";
          break;
        }
      } else { // if (GenerateSamples(sample_index, which_samples)){
        BIASERR("GenerateSamples failed");
        BIASASSERT(false);
      }
    }
    // now search best solution and refine it if not already done in loop
    if (!_MSACRefineSolution){
      solution=best_sol;
      BIASCDOUT(D_MSAC_SOLVEM, "now refining solution "<<solution<<std::endl);
      which_samples.clear();
      for (int i=0; i<_iDataSize; i++){
        if (inliervec[best_index][i]){
          which_samples.push_back(i);
        }
      }
      
      if (!RefineSolution(which_samples, solution, inliers)){
        BIASERR("RefineSolution returned false");
        BIASASSERT(false);
      } 
    } else {
      inliers=inliervec[best_index];
      solution=best_sol;
    }    
    BIASCDOUT(D_MSAC_SOLVEM, "finished: score "<<best_score<<" with "
              <<best_num_inlier<<" inlier, solution is "<<solution<<std::endl);
    return 0;
  }
  
  template <class SolutionType>
  int MSAC<SolutionType>::
  GetSampleSolution(std::vector<unsigned int> &which_samples, 
                    SolutionType &solutions)
  {
    BIASERR("this function should be implemented in derived class");
    return -1;
  }



  template <class SolutionType>
  bool MSAC<SolutionType>::
  EvaluateSolution(const SolutionType& solution, std::vector<bool>& inlier, 
                   int& num_inliers, double& evaluate_score)
  {
    BIASERR("this function should be implemented in derived class");
    return false;
  }




  template <class SolutionType>
  bool MSAC<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>
  bool MSAC<SolutionType>::
  GenerateSamples(int sample_index, std::vector<unsigned int>& which_samples)
  {
    return _GenerateSamplesRandom(sample_index, which_samples);
  }
  



  template <class SolutionType>
  bool MSAC<SolutionType>::
  _GenerateSamplesRandom(int sample_index, 
                         std::vector<unsigned int>& which_samples)
  {
    bool res=true;
#ifdef BIAS_DEBUG
    if (_iSampleSize==0){
      BIASERR("call Init or appropriate constructor befor using MSAC");
      BIASASSERT(false);
      res=false;
    }
    if (_iSampleSize>=_iDataSize){
      BIASERR("_iDataSize should be set in derived class befor using MSAC"
              <<" _iSampleSize="<<_iSampleSize<<"  _iDataSize : "
              <<_iDataSize);
      BIASASSERT(false);
      res=false;
    }
#endif
    // initialzing random with 0 and 1 does not seem to work well
    BIAS::Random ran(sample_index+rand());
    int count=0;
    unsigned int index;
    which_samples.clear();

    do {
      index = ran.GetUniformDistributedInt(0, _iDataSize-1);
      if (find(which_samples.begin(), which_samples.end(), index)==
          which_samples.end()){
        which_samples.push_back(index);
        count++;
      }
    } while (count<_iSampleSize);

#ifdef BIAS_DEBUG
    if ((GetDebugLevel() & D_MSAC_SAMPLES)!=0){
      BIASCDOUT(D_MSAC_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>
  double MSAC<SolutionType>::
  _CalculateTerminateScore(double sigma, double expected_inlier_fraction)
  {
    double res=0.0;
    res += _iDataSize * expected_inlier_fraction * sigma;
    res += _iDataSize * (1.0-expected_inlier_fraction) *  
      _MSACSigmaFac * sigma;
    return res;
  }




  template <class SolutionType>
  int MSAC<SolutionType>::
  _CalculateSampleCount(double expected_inlier_fraction)
  {
    if (expected_inlier_fraction == 1.0)
      // the analytic expression below cant cope with this so do it explicitly.
      return 1;

    // from Torr's thesis.
    int res = int(log (1.0 - (_MSACConfidenceThresh)) /
                  log (1.0 - pow (expected_inlier_fraction, 
                                  (double) _iSampleSize)));
    return res;
  }

} // namespace


#include <Base/Common/BIASpragmaEnd.hh>

#endif // __MSAC_hh__