RansacFitter.h

Go to the documentation of this file.

/********************************************************************
**                Image Component Library (ICL)                    **
**                                                                 **
** Copyright (C) 2006-2013 CITEC, University of Bielefeld          **
**                         Neuroinformatics Group                  **
** Website: www.iclcv.org and                                      **
**          http://opensource.cit-ec.de/projects/icl               **
**                                                                 **
** File   : ICLMath/src/ICLMath/RansacFitter.h                     **
** Module : ICLMath                                                **
** Authors: Christof Elbrechter                                    **
**                                                                 **
**                                                                 **
** GNU LESSER GENERAL PUBLIC LICENSE                               **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the                  **
**                                                                 **
** Free Software Foundation and appearing in the file LICENSE.GPL  **
** included in the packaging of this file.  Please review the      **
** following information to ensure the license requirements will   **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt                **
**                                                                 **
** The development of this software was supported by the           **
** Excellence Cluster EXC 277 Cognitive Interaction Technology.    **
** The Excellence Cluster EXC 277 is a grant of the Deutsche       **
** Forschungsgemeinschaft (DFG) in the context of the German       **
** Excellence Initiative.                                          **
**                                                                 **
********************************************************************/

#pragma once

#include <ICLMath/DynVector.h>
#include <ICLMath/FixedVector.h>
#include <ICLUtils/Function.h>
#include <ICLUtils/Uncopyable.h>
#include <ICLUtils/Random.h>

namespace icl{
  namespace math{
   
    

    template<class DataPoint=std::vector<float>,
             class Model=std::vector<float> >
    class RansacFitter{
      public:
      typedef std::vector<DataPoint> DataSet;
      
      typedef utils::Function<Model,const DataSet&> ModelFitting;
      
      typedef utils::Function<icl64f,const Model&,const DataPoint&> PointError;
      
      private:
      int m_minPointsForModel;
      
      int m_iterations;
      
      icl64f m_maxModelDistance;
      
      int m_minClosePointsForGoodModel;
      
      ModelFitting m_fitting;
      
      PointError m_err;
      
      icl64f m_minErrorExit;
      
      public:
      struct Result{
        icl64f error;
        
        Model model;
        

        DataSet consensusSet;
  
        int iterationCount;
        
        bool found() const { return consensusSet.size(); }
      };
      
      private:
      Result m_result;
      
      static inline bool find_in(const std::vector<int> &v, int i, int n){
        return std::find(v.data(), v.data()+n, i) != v.data()+n;
      }
  
      void find_random_consensus_set(DataSet &currConsensusSet,
                                     const DataSet &allPoints,
                                     std::vector<int> &usedIndices){
        const int n = currConsensusSet.size();
        utils::URandI r(allPoints.size()-1);
        
        for(int i=0;i<n;++i){
          do { usedIndices[i] = r; } while ( find_in(usedIndices, usedIndices[i], i-1) );
          currConsensusSet[i] = allPoints[ usedIndices[i] ];
        }
      }
      
      public:
      RansacFitter(){}
      

      RansacFitter(int minPointsForModel, 
                   int iterations, 
                   ModelFitting fitting,
                   PointError err,
                   icl64f maxModelDistance,
                   int minClosePointsForGoodModel,
                   icl64f minErrorExit=0):
        m_minPointsForModel(minPointsForModel),
        m_iterations(iterations),
        m_maxModelDistance(maxModelDistance),
        m_minClosePointsForGoodModel(minClosePointsForGoodModel),
        m_fitting(fitting),m_err(err),
        m_minErrorExit(minErrorExit){
      }
      
      const Result &fit(const DataSet &allPoints){
        m_result.model = Model();
        m_result.consensusSet.clear();
        m_result.error = utils::Range64f::limits().maxVal;
        m_result.iterationCount = 0;
        std::vector<DataPoint> consensusSet(m_minPointsForModel);
        std::vector<int> usedIndices(m_minPointsForModel);
        
        int i = 0;
        for(i=0;i<m_iterations;++i){
          consensusSet.resize(m_minPointsForModel);
          find_random_consensus_set(consensusSet, allPoints, usedIndices);
          
          Model model = m_fitting(consensusSet);
          for(int j=0;j<(int)allPoints.size();++j){
            if(find_in(usedIndices, j, usedIndices.size())) continue;
            if(m_err(model, allPoints[j]) < m_maxModelDistance){
              consensusSet.push_back(allPoints[j]);
            }
          }
          
          if((int)consensusSet.size() >= m_minClosePointsForGoodModel){
            model = m_fitting(consensusSet);
            double error = 0;
            for(unsigned int j=0;j<consensusSet.size();++j){
              error += m_err(model, consensusSet[j]);
            }
            error /= consensusSet.size();
            
            if(error < m_result.error){
              m_result.error = error;
              m_result.model = model;
              m_result.consensusSet = consensusSet;
              if(m_result.error < m_minErrorExit){
                m_result.iterationCount = i;
                return m_result;
              }
            }
          }
        }
        m_result.iterationCount = i;
        return m_result;
      }
    };
  } // namespace math
}