MeanDoubleRANSACEvaluator.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 __MeanDoubleRANSACEvaluator_hh__
#define __MeanDoubleRANSACEvaluator_hh__

#include <MathAlgo/RANSACEvaluatorInterface.hh>
#include <Base/Debug/Error.hh>
#include <Base/Debug/Exception.hh>
#include <Base/Common/CompareFloatingPoint.hh>

#include <vector>
#include <cmath>

namespace BIAS {

  /** @class MeanDoubleRANSACEvaluator
      @brief helper class for implementation of PreemptievRANSAC and COSAC
      for robust computation of the mean of a vector of doubles
      @relates TestCOSAC, TestPreemptiveRANSAC, RANSACEvaluatorInterface, COSAC, 
      PreemptiveRANSAC
      @author woelk 01/2010 (c) www.vision-n.de */
  class MeanDoubleRANSACEvaluator
    : public RANSACEvaluatorInterface<double>
  {
  public:
    MeanDoubleRANSACEvaluator();

    virtual ~MeanDoubleRANSACEvaluator();

    int Init(const std::vector<double> &data, const double &sigma3);

    unsigned GetNumMeasurements() const { return Data_.size(); }

    unsigned GetMinNumSamplesForSolutionComputation() const { return 2u; }

    inline bool IsInlier(const double& solution, const unsigned data_index,
                         double& score);
    
    inline int GetSampleSolutions(const std::vector<unsigned> &which_samples,
                                  std::vector<double> &solutions);
  
    inline bool RefineSolution(const std::vector<bool>& inliers,
                               double& solution);        

  protected:
    std::vector<double> Data_;
    double Sigma3_;

  }; // class

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

  MeanDoubleRANSACEvaluator::
  MeanDoubleRANSACEvaluator()
    :  RANSACEvaluatorInterface<double>(), Data_(), Sigma3_(1.0)
  {}


  int MeanDoubleRANSACEvaluator::
  Init(const std::vector<double> &data, const double &sigma3)
  {
    Data_ = data;
    Sigma3_ = sigma3;
    if (data.empty()){
      //return -1;
      BEXCEPTION("MeanDoubleRANSACEvaluator::MeanDoubleRANSACEvaluator(): No measurements");
    }
    if (data.size()<GetMinNumSamplesForSolutionComputation()){
      //return -2;
      BEXCEPTION("MeanDoubleRANSACEvaluator::MeanDoubleRANSACEvaluator(): Not enough "
                 <<"measurements");
    }
    if (sigma3<0.0){
      //return -3;
      BEXCEPTION("MeanDoubleRANSACEvaluator::MeanDoubleRANSACEvaluator(): Sigma3 must be "
                 <<"greater or equal to zero.");
    }
    return 0;
  }


  MeanDoubleRANSACEvaluator::
  ~MeanDoubleRANSACEvaluator()
  {}


  bool MeanDoubleRANSACEvaluator::
  IsInlier(const double &solution, const unsigned data_index, double &score)
  {
    if (Data_.empty()){
      BEXCEPTION("MeanDoubleRANSACEvaluator::IsInlier(): Not initialized\n");
    }
    BIASASSERT(data_index<Data_.size());
  
    double dist = fabs(solution - Data_[data_index]);
    const double eps = 1e-13;
    if (Equal(dist, 0.0, eps)) {
      dist = eps;
    }
    score = log(dist / Sigma3_);
    if (isnan(score) || isinf(score)){
      BEXCEPTION("MeanDoubleRANSACEvaluator::IsInlier_(): Cannot compute score, "
                 <<"maybe expected sigma ("<<Sigma3_<<") is invalid? dist = "
                 <<dist<<"\t"<<solution<<"\t"<<Data_[data_index]);
    }
    bool is_inlier = (dist < Sigma3_);
    return is_inlier;
  }
  

  int MeanDoubleRANSACEvaluator::
  GetSampleSolutions(const std::vector<unsigned> &which_samples,
                     std::vector<double> &solutions)
  {
    if (Data_.empty()){
      BEXCEPTION("MeanDoubleRANSACEvaluator::IsInlier(): Not initialized\n");
    }
    // check for duplicate samples
    if (which_samples.size()>1 && which_samples[0]==which_samples[1]){
      BIASABORT;
    }
    BIASASSERT(which_samples.size()==GetMinNumSamplesForSolutionComputation());
    BIASASSERT(!which_samples.empty());
    double sum = 0.;
    std::vector<unsigned>::const_iterator it;
    for (it = which_samples.begin(); it!=which_samples.end(); it++){
      BIASASSERT(*it<Data_.size());
      sum += Data_[*it];
    }
    double mean = sum / (double)(which_samples.size());
    BIASASSERT(!isinf(mean) && !isnan(mean));
    // check if all data points from which the solution has been generated are 
    // inliers to the solution
    double score;
    for (it = which_samples.begin(); it!=which_samples.end(); it++){
      if (!IsInlier(mean, *it, score)){
        return false;
      }
    }

    solutions.clear();
    solutions.push_back(mean);

    return 0;
  }
  

  bool MeanDoubleRANSACEvaluator::
  RefineSolution(const std::vector<bool>& inliers, double& solution)
  {
    if (Data_.empty()){
      BEXCEPTION("MeanDoubleRANSACEvaluator::IsInlier(): Not initialized\n");
    }
    double sum = 0.;
    unsigned count = 0;
    BIASASSERT(inliers.size()==Data_.size());
    for (unsigned i=0; i<inliers.size(); i++){
      if (inliers[i]){
        sum += Data_[i];
        count++;
      }
    }
    if (count==0){ return false; }
    solution = sum / (double)count;
    BIASASSERT(!isinf(solution) && !isnan(solution));
  
    return true;
  }

} // namespace

#endif // __MeanDoubleRANSACEvaluator_hh__