RANSACPreKnowledge.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 __RANSACPREKNOWLEDGE_hh__
#define __RANSACPREKNOWLEDGE_hh__

#include "RANSAC.hh"

namespace BIAS {

  /** @class RANSACPreKnowledge
      @ingroup g_mathalgo
      @brief Extension of RANSAC algorithm: user-supplied guesses are evaluated
             and refined using RANSAC criterions before actually starting the
             RANSAC algorithm
             
      @author koeser 11/2003 */

  template <class SolutionType>
  class /*BIASMathAlgo_EXPORT*/ RANSACPreKnowledge : public RANSAC<SolutionType>
  {
  public:
    using RANSAC<SolutionType>::_uiSampleSize;
    using RANSAC<SolutionType>::_uiMaxSolutionsPerSample;
    using RANSAC<SolutionType>::_uiDataSize;
    using RANSAC<SolutionType>::_bRefineSolution;
    using RANSAC<SolutionType>::_iBestSampleAcceptCount;
    using RANSAC<SolutionType>::_dBestSampleEvaluateScore;
    using RANSAC<SolutionType>::_iBestInlierAcceptCount;
    using RANSAC<SolutionType>::_dBestInlierEvaluateScore;
    using RANSAC<SolutionType>::_uiMaxSamples;
    using RANSAC<SolutionType>::_uiExplicitSampleNo;
    using RANSAC<SolutionType>::_dProbabilityOfGoodSolution;
    using RANSAC<SolutionType>::_dSoSmallSolutionChange;
    using RANSAC<SolutionType>::_got_solution_flag;
    using RANSAC<SolutionType>::_greedy;

    RANSACPreKnowledge() : RANSAC<SolutionType>() 
    { Debug::NewDebugLevel("D_RANSAC_PRE_KNOWLEDGE"); };
    
    /** 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. */
    RANSACPreKnowledge(unsigned int sample_size,
                       unsigned int max_solutions_per_sample = 1,
                       bool refine_solution = false): 
      RANSAC<SolutionType>(sample_size, max_solutions_per_sample, 
                           refine_solution)
    { Debug::NewDebugLevel("D_RANSAC_PRE_KNOWLEDGE"); };

    /** @brief default destructor does nothing */
    virtual ~RANSACPreKnowledge() { };

    /** @brief add some guessed solutions which are tested by RANSAC later */
    virtual inline void AddGoodGuesses(const std::vector<SolutionType> &Guesses) {
      UserGuesses_.reserve(UserGuesses_.size() + Guesses.size());
      for (typename std::vector<SolutionType>::const_iterator 
             it = Guesses.begin(); it!=Guesses.end(); it++)
        UserGuesses_.push_back(*it);
    }

    /** @brief clear all guesses */
    virtual inline void ClearGuesses() {
      UserGuesses_.clear();
    }

    /** @brief checks UserGuesses and calls RANSAC::SolveMaster
        @author koeser */
    virtual inline int SolveMaster(double inlying_data_fraction,
                                   SolutionType &solution,
                                   std::vector<bool> &inliers);

  protected:
    /// vector of guesses to be checked, which were manually supplied by user
    std::vector<SolutionType> UserGuesses_;

  };

  template <class SolutionType> inline int
  RANSACPreKnowledge<SolutionType>::SolveMaster(double inlying_data_fraction,
                                                SolutionType &solution,
                                                std::vector<bool> &inliers) {
    BCDOUT(D_RANSAC_PRE_KNOWLEDGE, 
           "RANSACPreKnowledge::SolveMaster() -----------------------------"
           "-----------------\n");
    BCDOUT(D_RANSAC_SAMPLE_QUALITY, "|||||||||||||||||||||\n\n");
    // in order to do a real RANSAC run without high thresholds of 
    // pre-guesses, we clean up everything
    _iBestSampleAcceptCount   = -1;
    _dBestSampleEvaluateScore = -1;
    _iBestInlierAcceptCount   = -1;
    _dBestInlierEvaluateScore = -1;
    _got_solution_flag = false;

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

    // vector indicating which ssamples are inliers
    std::vector<bool> inlier_set(_uiDataSize, false);

    // 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;

    // 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_PRE_KNOWLEDGE,"checking "<<UserGuesses_.size()<<
              " user guesses\n");
    // run over all user guesses and evaluate using RANSAC criterions:
    for (typename std::vector<SolutionType>::iterator 
           it = UserGuesses_.begin(); it!=UserGuesses_.end(); it++) {
      

      ok_to_terminate_flag = false;
      sample_evaluate_score = DBL_MAX;
      good_flag = this->EvaluateSolution(*it, _got_solution_flag, 
                                   inlier_set, sample_accept_count, 
                                   sample_evaluate_score, 
                                   ok_to_terminate_flag); 

      BCDOUT(D_RANSAC_PRE_KNOWLEDGE,"PreGuess. "
               <<" solution: "<<(*it)
               <<" inliers: " <<sample_accept_count<<"/"<<_uiDataSize
               <<" score: "<< sample_evaluate_score<<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 = *it;
        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);
        if (this->RefineSolution(which_samples, *it, 
                           inlier_set)){
          if (_got_solution_flag)
            inlier_accept_count = _iBestInlierAcceptCount;
          good_flag = 
            this->EvaluateSolution (*it, _got_solution_flag,
                              inlier_set, inlier_accept_count,
                              inlier_evaluate_score, ok_to_terminate_flag);
          if (good_flag) {
            BCDOUT(D_RANSAC_PRE_KNOWLEDGE, 
                   "RANSACPreKnowledge: minimal/refined "
                     <<sample_accept_count<<"/"
                     <<inlier_accept_count<<" "<<sample_evaluate_score<<"/"
                     <<inlier_evaluate_score<<std::endl);
          }
        } else { 
          // RefineSolution did not succeed, we do not use the solution
          good_flag = false;
          BCDOUT(D_RANSAC_PRE_KNOWLEDGE,
                 "RANSACPreKnowledge: (note) refine_solution returned false"
                 <<std::endl);
        }

        // 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;
          *it = samplesolution;
          ok_to_terminate_flag = ok_to_terminate_with_linear;
        }
      } else { // if (good_flag && _bRefineSolution) { 
        BCDOUT(D_RANSAC_PRE_KNOWLEDGE,
                 "RANSACPreKnowledge: (note) either good_flag=false or "
                 <<"_bRefineSolution=false"<<std::endl);
      }
    
      // 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) {
          _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) {
          _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;
              
          // save inliers and solution
          inliers = inlier_set;
          solution = *it;
        
          if (_bRefineSolution){
            BCDOUT(D_RANSAC_PRE_KNOWLEDGE,"RANSACPreKnowledge: PreGuess "
                     <<" is best so far.  " << "Inliers/score "
                     <<_iBestInlierAcceptCount << "/" 
                     <<_dBestInlierEvaluateScore<<": "<<solution<<std::endl);
          } else {
            BCDOUT(D_RANSAC_PRE_KNOWLEDGE, "RANSACPreKnowledge: PreGuess "
                     <<" is best so far.  " << "Inliers/score "
                     << _iBestSampleAcceptCount << "/"
                     <<_dBestSampleEvaluateScore<<": "<<solution<<std::endl);
          }
              
          if (ok_to_terminate_flag){
            termination_check_active_flag = true;
            BCDOUT(D_RANSAC_PRE_KNOWLEDGE, 
                   "RANSACPreKnowledge: termination_check_active_flag = true");
            break; // do not evaluate the remaining solutions
          }
        } // if (best_so_far_flag) {
      } else { // if (good_flag) {
        BCDOUT(D_RANSAC_SAMPLE_QUALITY, "*"<<std::flush);
      }
    } // Guesses
    
    // check whether our guesses yielded a good solution ...
    if  (termination_check_active_flag) {
      BCDOUT(D_RANSAC_SAMPLE_QUALITY, "P" << std::endl);
      BCDOUT(D_RANSAC_PRE_KNOWLEDGE,
               "VERY GOOD preguess! Skipping RANSAC." << std::endl);
      return 0;
    }

    BCDOUT(D_RANSAC_PRE_KNOWLEDGE, 
             "RANSACPreKnowledge: No good preguess. Calling RANSAC without "
           "preknowledge.");
    // ok, we have no really good solution, call RANSAC algorithm:
    return  RANSAC<SolutionType>::SolveMaster(inlying_data_fraction, 
                                              solution, inliers);
  }

} // end namespace

#endif // header defined