trv_homographyFine.hpp

#ifndef TRV_HOMOGRAPHYFINE_HPP
#define TRV_HOMOGRAPHYFINE_HPP

#include "traversability/trv_util.hpp"
#include "traversability/trv_homography.hpp"
#include "traversability/trv_homographyGeom.hpp"
#include "traversability/traversabilityException.hpp"

#include "kernel/jafarException.hpp"
#include "jmath/jblas.hpp"
#include "jmath/ublasExtra.hpp"
#include "jmath/randomIntTmplt.hpp"     // random int for ransac estimation
#include "jmath/linearSolvers.hpp"  // LU-decomposition

#include "../libsLourakis/homest-1.2/homest.h"        // lourakis' robust homography estimator


namespace jafar {
  namespace traversability {
    
    class HomographyFine : public Homography
    {
      private:
        // matches, interface with gfm: class MatchesContainer
        MatchesContainer* mMatches;
        
        // projection statistics
        jblas::vec mProjError;
        double mMeanProjError;
        double mStdProjError;
      
      public:
        HomographyFine ( void );
        HomographyFine ( const HomographyGeom &Hgeom );
        ~HomographyFine ( void );
        void printInfo ( void ) const;
        
        int setMatches ( MatchesContainer* matches ) {
          if (matches->nOfMatches()==0)
          {
            return 1;
          }
          // call copy constructor to perform deep copy of matchesContainer.
          // if only the reference is passed, the member mMatches will be 
          // changed as soon as the member mMatches of tracker is changed...
          mMatches = new MatchesContainer(*matches);
          // instead of
          // mMatches = matches;
          return 0;
        }
        MatchesContainer getMatches ( void ) { return *mMatches; }
        
        void estimate ( void );
        void estimateR ( void );
        void estimateRANSAC ( void );
        
        // is antiquated (obsolete): TODO: remove this function, but ensure that this is done in core
        void estimateWithGeom ( const jblas::mat33& H_C0C1_geom );
        
      public:
        // projection statistics
        double getMeanProjError ( void ) { return mMeanProjError; }
        double getStdProjError ( void ) { return mStdProjError; }
        void calcProjErrorStats ( const jblas::mat33 H01, double &mean, double &stdev );
        void sortOutMatches ( const jafar::image::Image &mask );
        
      private:
        template <class Mat> void calcA_i ( const double x0u, const double x0v, const double x1u, const double x1v, Mat &A_i, const double weight = 1.0 );
        void calcH4p ( const MatchesContainer *mc, const jblas::veci4 &idxVec, jblas::mat33 &H_C0C1 );
        void pointSelectionCriteria ( const int idx, const MatchesContainer* matches, jblas::veci4 &idxVec, int &featCnt, const double minDistSq, const double minCrossP );
    
    };  // class HomographyFine
  } // namespace traversability
} // namespace jafar

#endif