hough.hpp

/* $Id$ */

#ifndef SAMS_HOUGH_HPP_
#define SAMS_HOUGH_HPP_

#include "jafarConfig.h"

#ifdef HAVE_BOOST_SANDBOX
#ifdef HAVE_LAPACK


#include <cmath>
#include <string>
#include <utility>
#include <iostream>
#include <vector>
#include <ext/hash_map>
#include "kernel/keyValueFile.hpp"
#include "jmath/jblas.hpp"
#include "sams/model.hpp"
#include "sams/frame.hpp"
#include "sams/matching.hpp"
#include "sams/hypothesis.hpp"


namespace jafar {
  namespace sams {

    class Hough {
      public:

      class Params : public jafar::kernel::KeyValueFileLoad {
        public:
        int xBins;
        int yBins;
        int oBins;
        int sBins;
        double thd;

        Params(double xBins_=8, int yBins_=8, int oBins_=8, int sBins_=8,
               double thd_=1.5) :
          xBins(xBins_), yBins(yBins_), oBins(oBins_), sBins(sBins_), thd(thd_) {};

        protected:

        void loadKeyValueFile(jafar::kernel::KeyValueFile const & keyValueFile) {
          JFR_TRACE_BEGIN;
          keyValueFile.getItem("xBins", xBins);
          keyValueFile.getItem("yBins", yBins);
          keyValueFile.getItem("oBins", oBins);
          keyValueFile.getItem("sBins", sBins);
          keyValueFile.getItem("thd",thd);
          JFR_TRACE_END("Hough::Params::loadKeyValueFile");
        };

      }; // class Params

      Hough() {};
      Hough(Params params_) : params(params_) {};
      Hough(std::string const & fileName_) {
        params.load(fileName_);
      };

      std::vector<jafar::sams::Hypothesis> process(jafar::sams::Model const & md_, 
                                                   jafar::sams::Frame const & fr_, 
                                                   std::vector<jafar::sams::Match> const & matches_);        

      jblas::mat computeRANSACSimilarityTransform(std::vector<jafar::sams::Match> & matches_, 
                                                  jafar::sams::Hypothesis const & hc_,
                                                  double err_=-1.0,
                                                  bool useHoughTrans_=true);

      private:
      Params params;
      int nViews;

      struct hough_bin {
        double wght;
        std::vector<int> match_idx;

        hough_bin() : wght(0.0), match_idx() {};
      };

      typedef std::vector<hough_bin> acc_type;
      std::vector<hough_bin> acc;

      inline int make_linearIdx(std::vector<int> const & vIdx_, std::vector<int> const & vDims_) const {
        int key = vIdx_[0];
        int prod = 1;
        for(int i=1; i < vIdx_.size(); i++) {
          prod *= vDims_[i-1];
          key += prod*(vIdx_[i]);
        }
        return key;           
      };

      inline void make_MultipleIdx(int key_, std::vector<int>& vIdx_, std::vector<int> const & vDims_) const {
        int idx = key_;
        for(int i=vIdx_.size()-1; i >= 0; i--) {
          if (i > 0) {
            int div = 1;
            for(int j=0; j < i; j++)
              div *= vDims_[j];
            vIdx_[i] = idx/div;
            idx -= div*vIdx_[i];
          } else
            vIdx_[i] = idx;
        }
      }; 

      void addValue(int vId, double dx, double dy, double dt, double ds, int mIdx);

      std::vector<jafar::sams::Hypothesis> analyse(jafar::sams::Model const & md_, std::vector<jafar::sams::Match> const & matches_);

      friend std::ostream& operator <<(std::ostream& s, const Hough& hough_);
    };

    std::ostream& operator <<(std::ostream& s, const Hough& hough_);

  } // namespace sams
} // namespace jafar
#endif 
#endif
#endif /*SAMS_HOUGH_HPP_*/