trv_util.hpp

#ifndef TRV_UTIL_HPP
#define TRV_UTIL_HPP

#include <image/Image.hpp>
#include "kernel/jafarException.hpp"
#include "jmath/jblas.hpp"
#include "jmath/ublasExtra.hpp"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/bindings/lapack/driver/gesdd.hpp>


// tools for representation of rotations (to be moved to jmath!)
#include "traversability/trv_rotations.hpp" 

#include "fdetect/DetectionResult.hpp"
#include "gfm/MatchingResult.hpp"
#include "fdetect/InterestFeature.hpp"

# include "klt/klt.hpp"


namespace jafar
{
  namespace traversability
  {
    struct CameraPoseParameters
    {
      // constructor
      CameraPoseParameters( double _x = 0, double _y = 0, double _z = 0,
                            double _psi = 0, double _theta = 0, double _phi = 0 ) :
          x(_x), y(_y), z(_z), psi(_psi), theta(_theta), phi(_phi) {}
      
      // camera pose and position parameters
      double x;       
      double y;       
      double z;       
      double psi;     
      double theta;   
      double phi;     
    };
    

    struct CameraIntrinsicParameters
    {
      // constructor
      CameraIntrinsicParameters( double _au = 0, double _av = 0,
                                 double _u0 = 0, double _v0 = 0 ) :
          alpha_u(_au), alpha_v(_av), u0(_u0), v0(_v0) {
                    double karr[9] = { alpha_u, 0.0, u0 , 0.0, alpha_v, v0, 0.0, 0.0, 1.0 };
                    jafar::jmath::ublasExtra::setMatrixValue(K,karr,3,3);
                    jmath::ublasExtra::inv(K,Kinv);
          }
      
      // intrinsic camera parameters
      double alpha_u;       
      double alpha_v;       
      double u0;            
      double v0;            
      jblas::mat33 K;       
      jblas::mat33 Kinv;    
    };
    

    struct PlaneParameters
    {
      PlaneParameters( double _d = 1.0, double _nx = 0.0, double _ny = 0.0, double _nz = 1.0) :
          d(_d), nx(_nx), ny(_ny), nz(_nz) {
                  double narr[3] = {nx,ny,nz};
                  jafar::jmath::ublasExtra::setVectorValue(n_W,narr,3);
          }
      double d;     
      double nx;    
      double ny;
      double nz;
      jblas::vec3 n_W;
    };
    

    struct CameraParameters
    {
      CameraParameters(CameraPoseParameters _pose = CameraPoseParameters(),
                       CameraIntrinsicParameters _internals = CameraIntrinsicParameters(),
                       PlaneParameters _plane = PlaneParameters(1.0)) : 
          pose(_pose), internals(_internals), plane(_plane) {}
      
      // position and pose of considered camera
      CameraPoseParameters pose;
      
      // internal parameters
      CameraIntrinsicParameters internals;
      
      // environment variable (defining homography)
      PlaneParameters plane;
      
      // depending variables
      // jblas::mat33 R_C0W;  //!< rotation matrix camera0 -> world
      // jblas::mat33 R_C1C0; //!< rotation matrix camer0 -> camera1
      // jblas::mat33 H_C0W;    //!< homography from world to camera0 frame
      // jblas::mat33 H_C1C0; //!< homography from camera0 -> camera1
    };
    

    struct CameraDeltaParameters
    {
      CameraDeltaParameters(CameraParameters _cam0 = CameraParameters(),
                            CameraParameters _cam1 = CameraParameters()) :
          cam0(_cam0), cam1(_cam1) {}
      
      // parameters of cams 0 and 1
      CameraParameters cam0;
      CameraParameters cam1;
      
      // depending variables, to be set by the method setDeltaParameters
      jblas::vec3 rC0_W;
      jblas::vec3 rC1_W;
      jblas::vec3 tC1C0_W;
      
      jblas::vec4 qC0C1_b;
      jblas::vec4 qC0C1;
      jblas::vec4 qWC0;
      jblas::vec4 qWC1;
      
      jblas::mat33 R_C0C1;
      jblas::mat33 R_C1C0;
      jblas::mat33 R_WC0;
      jblas::mat33 R_C0W;
      jblas::mat33 R_C1W;
      jblas::mat33 R_WC1;
    };
    

    struct FileInfoDataSet
    {
      FileInfoDataSet( ) :
          pathPos(""), pathImg(""), stemImage(""), stemPositionData(""), fileType(""),
                  nOfDigits(0), startIdx(0), stopIdx(0) {}
      FileInfoDataSet( std::string _pathPos, std::string _pathImg,
                        std::string _stemImage, std::string _stemPositionData, std::string _fileType,
                          int _nOfDigits, int _startIdx, int _stopIdx ) :
          pathPos(_pathPos), pathImg(_pathImg), stemImage(_stemImage), stemPositionData(_stemPositionData),
               fileType(_fileType), nOfDigits(_nOfDigits), startIdx(_startIdx), stopIdx(_stopIdx) {}
      
      // path to the repository
      std::string pathPos;
      std::string pathImg;
      std::string stemImage;
      std::string stemPositionData;
      std::string fileType;
      int nOfDigits;
      int startIdx;
      int stopIdx;
    };
    

    struct ImageInfo
    {
      ImageInfo (int _width = 512, int _height = 384, int _depth = CV_8U,
                        JfrImage_TypeColorSpace _colorSpace = JfrImage_CS_GRAY ) :
          width(_width), height(_height), depth(_depth), colorSpace(_colorSpace) {}
      int width;
      int height;
      int depth;
      JfrImage_TypeColorSpace colorSpace;
    };
    

    class Quaternion
    {
      private:
        jblas::vec4 mq;
        
      public:
        Quaternion();
        Quaternion( jblas::mat33 R );
        Quaternion( jblas::vec3 axis, double phi );
        Quaternion( double psi, double theta, double phi ); 
        Quaternion( double q0, double q1, double q2, double q3 );
        
        // transformations
//        void mat2quat( jblas::mat33 );
//        jblas::mat33 quat2mat( void );
    };
    

    struct MatchesInfo
    {
      MatchesInfo ( jblas::vec2 _ctr_ref = jblas::zero_vec(2),
                    jblas::vec2 _ctr_match = jblas::zero_vec(2),
                        double _md_ref = 0, double _md_match = 0, int _n=0 ):
          centroid_ref(_ctr_ref), centroid_match(_ctr_match),
          meanDist_ref(_md_ref), meanDist_match(_md_match), nOfMatches(_n) {};
      jblas::vec2 centroid_ref;
      jblas::vec2 centroid_match;
      double meanDist_ref, meanDist_match;
      int nOfMatches;
    };
    

    class MatchesContainer
    {
      private:
        jblas::mat mPoints_ref;
        jblas::mat mPoints_match;
        jblas::veci mIdentifier;
        jblas::veci mPartition;
        jblas::vec mWeight; 
        MatchesInfo mInfo;
        bool mIsNormalized;
        bool mIsStaticallyNormalized;
      public:
        MatchesContainer ();
        MatchesContainer ( const jafar::gfm::MatchingResult &rm, int width=-1, int height=-1 );
        MatchesContainer ( MatchesContainer* mcBuffer[], int idx0, int idx1, int bufferSize,
                           int width=-1, int height=-1 );

        MatchesContainer ( const KLT_FeatureTable &ft, int idx0, int idx1, int width=-1, int height=-1 );
        MatchesContainer ( jblas::mat &points_ref, jblas::mat &points_mat, int width=-1, int height=-1 );
        
        MatchesContainer ( const MatchesContainer &mc );

        ~MatchesContainer ( void ) {};
      public:
        void printInfo ( void );
        void normalize ( void );
        void denormalize ( void );
        jblas::mat33 denormalizeHom ( jblas::mat33 H01 );
        jblas::mat* points_ref( void ) { return &mPoints_ref; }
        jblas::mat* points_match( void ) { return &mPoints_match; }
        double point_ref_u( int idx ) const { return mPoints_ref(0,idx); }
        double point_ref_v( int idx ) const { return mPoints_ref(1,idx); }
        double point_match_u( int idx ) const { return mPoints_match(0,idx); }
        double point_match_v( int idx ) const { return mPoints_match(1,idx); }
        int nOfMatches( void ) const { return mInfo.nOfMatches; }
        int id ( int idx ) const { return mIdentifier(idx); }
        bool isNormalized ( void ) const { return mIsNormalized; }
        
        
        void setWeight ( const double weight, const int idx ) { mWeight(idx) = weight; }
        void setPartition ( const int partition, const int idx ) { mPartition(idx) = partition; }
        double getWeight ( const int idx ) const { return mWeight(idx); }
        int getPartition ( const int idx ) { return mPartition(idx); }
        jblas::vec* weights ( void ) { return &mWeight; }
        
      private:
        void calcInfo ( int width=-1, int height=-1 );
        int countTrackedFeatures( KLT_FeatureTable ft, int idx0, int idx1);
    };
    
    
    
    // some toools
    inline int mod ( int a, int b ) { return ( (a%b) >= 0) ? a%b : b+a%b; } 
    template <class Vec> void printVec ( const Vec &vec )
    {
      using namespace std;
      
      int n = vec.size();
      
      for (int i=0;i<n;i++)
      {
        cout << vec(i) << "\t";
      }
      cout << endl;
    }
    
    template <class Mat> void printMat ( const Mat &mat )
    {
      using namespace std;
      
      int m = mat.size1();
      int n = mat.size2();
      
      for (int i=0;i<m;i++)
      {
        for (int j=0;j<n;j++)
        {
          cout << mat(i,j) << "\t";
        }
        cout << endl;
      }
    }
    
    
    
    
    
    
    
    // OLD
    
    class HomMat
    {
      private:
        jblas::mat33 mH;
        
      public:
        HomMat();
        HomMat( jblas::mat33 H );
        void set ();                              
        void set ( jblas::mat33 H );              
        void set ( const double *H );             
        void get ( jblas::mat33 &H );             
        jblas::mat33 get ( void );                
        jblas::mat33 getInv ( void );             
        void set_ij ( double val, int idx_i, int idx_j ); 
        double get_ij ( int idx_i, int idx_j );   
        void clear ( void );                      
        void print ( void );                      
    };
    
    class Ellipse
    {
      private:
        jblas::mat *mPoints;
        jafar::image::Image *mMask;
        
      private:
        void setLine( jblas::mat *points, int counter, int line, int x1, int x2, int width, int height );
        void calcIntersections();
        void calcEllipseConditionForWin( jblas::mat *ellPoints, jblas::vec2 center, int winX, int winY,
                                         double A, double B, double C, double k2 );
        void calcEllipseConditionForWinLbL ( jblas::mat *ellPoints, jblas::vec2 center, int winX, int winY,  
                                             double A, double B, double C, double F );
        
        
//        TODO:
//        + ellipse fertig. zuerst baustelle fertig -> ellipse.set(center,cov).
//              ACHTUNG: meine formeln richtig nur für senkrecht stehende achsen!
//        + aufräumen in HomTest::run. KEINE funtktionsoptimierung
//        + testfunktionen in der klasse testRuns hinterlegen!!! alles aus dem run dort reinkopieren
//        - calcHom. 4punkt berechnung muss verdammt nochmal funktionieren
//        + homography estimation mit richtigem H ausprobieren (vorher funktionierte es)
//        + ein bisschen doku
//        - weiter mit herleitung der nötigen formeln ... (was ich gestern mit Bertrand besprochen habe)
//        - testRun1 fertig machen (mit ellipsen rund um pünktli)
//        - MAILS!
        
//        TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO
//        - store the ellipse in a better suitable structure (or static array with fixed size)
//        - check that calcEllipseConditionForWin and calcEllipseConditionForWinLbL draw exactely the same ellipses
//        - the upper left corner of a pixel (i,j) really belongs to the point [i,j]. in order to 
//          evaluate the ellipse condition correctly, one has to take this into accout!!!
//        - check subpixel precision of ellipse. what if vec center is given as double???
//        - write destructor
//        TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO
        
      public:
        Ellipse( void );
        void set( const jblas::vec2 &center, const jblas::vec2 &ax1, const double &sigma1,
                  const jblas::vec2 &ax2, const double &sigma2 );
        void set1( const jblas::vec2 &center, const jblas::mat22 &Cov, bool DEBUG_FLAG = false );
        jblas::mat* get( void );
        void setMask( const jblas::mat *points, const int &width, const int &height );
        jafar::image::Image* getMask( void );
        void draw( jafar::image::Image *img );
    };
  }
}

#endif