trv_homographyOld.hpp

#ifndef TRV_HOMOGRAPHYOLD_HPP
#define TRV_HOMOGRAPHYOLD_HPP

#include "kernel/jafarException.hpp"
#include "jmath/jblas.hpp"
#include "jmath/ublasExtra.hpp"
#include "traversability/trv_rotations.hpp"
#include "traversability/trv_util.hpp"


#include "../libsLourakis/homest-1.2/homest.h"

#include <image/Image.hpp>


namespace jafar {
  namespace traversability {
    
    class HomographyOld {
      
      private:
        CameraPoseParameters mPoseC0;
        CameraPoseParameters mPoseC1;
        CameraIntrinsicParameters mInternals;
        PlaneParameters mPlane;
        ImageInfo mImageInfo;
        
        CameraDeltaParameters mDeltaPoseC0C1;
        CameraDeltaParameters mDeltaPoseC0Cn;
        jblas::mat33 mH_C0C1_geom;
        jblas::mat33 mH_C0C1_fine;
        jblas::mat33 mH_C0Cn_geom;
        jblas::mat33 mH_C0Cn_fine;
        
        jblas::mat33 mDEBUG;
        
        jafar::image::Image mImg0;
        jafar::image::Image mImg1;
        jafar::image::Image mCFoVMask;
        
        // matches, interface with gfm: class MatchesContainer
        MatchesContainer* mMatches;
        
        // projection statistics
        jblas::vec mProjError;
        double mMeanProjError;
        double mStdProjError;
        
        // normalization of matches
//        jblas::mat33 mT_norm_ref;
//        jblas::mat33 mT_norm_match;
        
      public:
        HomographyOld ( void );
        ~HomographyOld ( void );
        
        void setImageInfo ( const ImageInfo &imageInfo );
        void setPlane     ( const PlaneParameters &plane );
        void setInternals ( const CameraIntrinsicParameters &internals );
        void setPoseCam0  ( const CameraPoseParameters &camPose0 );
        void setPoseCam1  ( const CameraPoseParameters &camPose1 );
        void setImageCam0 ( jafar::image::Image &img0 );
        void setImageCam1 ( jafar::image::Image &img1 );
        void setDeltaPose ( void );
        void setDeltaPose ( const CameraPoseParameters &camPose0,
                            const CameraPoseParameters &camPose1 );
        void setH_C0Cn_geom ( void );
        void setH_C0Cn_fine ( void );
        void setH_CnC0_DEBUG ( void );
        
        void setMatches ( MatchesContainer* matches ) { mMatches = matches; }
        MatchesContainer* getMatches ( void ) { return mMatches; }
        
        void getPoseCam0  ( CameraPoseParameters &camPose0 );
        void getPoseCam1  ( CameraPoseParameters &camPose1 );
        void getDeltaPose ( CameraDeltaParameters &delPars );
        void getImageCam0 ( jafar::image::Image &img0 );
        void getImageCam1 ( jafar::image::Image &img1 );
        
        jblas::mat33 getH_C0C1_geom ( void );
        jblas::mat33 getH_C0C1_fine ( void );
        jblas::mat33 getH_C0Cn_geom ( void );
        jblas::mat33 getH_C0Cn_fine ( void );
        
        jblas::mat33 getH_C1C0_geom ( void );
        jblas::mat33 getH_C1C0_fine ( void );
        jblas::mat33 getH_CnC0_geom ( void );
        jblas::mat33 getH_CnC0_fine ( void );
        
        double getMeanProjError ( void ) { return mMeanProjError; }
        double getStdProjError ( void ) { return mStdProjError; }
        
        CameraPoseParameters getPoseCam0 ( void );
        CameraPoseParameters getPoseCam1 ( void );
        CameraDeltaParameters getDeltaPose ( void );
        jafar::image::Image getImageCam0 ( void );
        jafar::image::Image getImageCam1 ( void );
        
        void printGeometricInfo ( void );
        
        void initMask ( void );
        void initMosaic ( int mosaicWidth, int mosaicHeight, int startU = 0, int startV = 0 );
        void calcHomographyGeom ( void );
        void calcHomographyGeom ( const CameraPoseParameters &camPose0,
                                  const CameraPoseParameters &camPose1,
                                  const CameraIntrinsicParameters &internals,
                                  const PlaneParameters &plane );
        void estimateHomography ( MatchesContainer* mc );
        void estimateHomographyR ( MatchesContainer* mc );
        void estimateHomographyWithGeom ( MatchesContainer* mc );
        
//        void homest(double (*pts0)[2], double (*pts1)[2], int nmatches, double inlPcent, double H01[NUM_HPARAMS],
//               int normalize, int NLrefine, int *nbOutliers, int verbose);

//        void estimateHomography ( const jafar::gfm::MatchingResult &rm );
//        void estimateHomographyWithGeom ( const jafar::gfm::MatchingResult &rm );
        
        void applyHomography ( jafar::image::Image &imgOut );
        void applyHomography ( const jblas::mat33 &H_C0C1,
                               const jafar::image::Image &imgIn,
                               jafar::image::Image &imgOut );
        void addToTraversabilityMap ( const jblas::mat33 &H_CnC0,
            const jafar::image::Image &imgTrvIn, const jafar::image::Image &imgIn,
      const jafar::image::Image &maskIn,jafar::image::Image &mosaicCntOut, jafar::image::Image &mosaicTrvOut );
        template <typename Type> void applyHomographyO( const jblas::mat33 &H_C0C1,
                                  const jafar::image::Image &imgIn,
                                  jafar::image::Image &imgOut,
                  jafar::image::Image maskCFoV ) {
          int widthIn     = imgIn.width();
          int heightIn    = imgIn.height();
          int widthOut    = imgOut.width();
          int heightOut   = imgOut.height();  
      
          Type p0_x;
          Type p0_y;
          Type p0_z;
          int x0_u; int x0_v;
      
          Type h01v1;
          Type h11v1;
          Type h21v1;
          
          Type* h00u = new Type[widthOut];
          Type* h10u = new Type[widthOut];
          Type* h20u = new Type[widthOut];
      
//          static Type h00u[512];
//          static Type h10u[512]; 
//          static Type h20u [512];
      
//      static unsigned char img[512*384];
//      unsigned char* imgC = img;
      
          for (int j=0;j<heightOut;j++)
          {
            h01v1 = static_cast<Type>(H_C0C1(0,1))*j+static_cast<Type>(H_C0C1(0,2));
            h11v1 = static_cast<Type>(H_C0C1(1,1))*j+static_cast<Type>(H_C0C1(1,2));
            h21v1 = static_cast<Type>(H_C0C1(2,1))*j+static_cast<Type>(H_C0C1(2,2));
        
            for (int i=0;i<widthOut;i++)
            {
              if (j==0)
              {
                h00u[i] = static_cast<Type>(H_C0C1(0,0))*i;
                h10u[i] = static_cast<Type>(H_C0C1(1,0))*i;
                h20u[i] = static_cast<Type>(H_C0C1(2,0))*i;
              }
          
              p0_x = h00u[i]+h01v1;
              p0_y = h10u[i]+h11v1;
              p0_z = h20u[i]+h21v1;

              x0_u  = round((p0_x/p0_z));
              x0_v  = round((p0_y/p0_z));

              if (x0_u >= 0 and x0_v >= 0 and x0_u < widthIn and x0_v < heightIn)
              {
                imgOut.setPixelValue<uint8_t>(imgIn.getPixelValue<uint8_t>(x0_u,x0_v),i,j);
                if (maskCFoV)
                {
                  maskCFoV.setPixelValue<uint8_t>(255,i,j);
                }
//            *imgC = imgIn.getPixelValue<uint8_t>(x0_u,x0_v);
//            imgC++;
              }
              else
              {
//            *imgC = 0;
//            imgC++;
                imgOut.setPixelValue<uint8_t>(0,i,j);
              }
            }
          }
      
      delete [] h00u;
      delete [] h10u;
      delete [] h20u;
        }
        void addToMosaic ( const jblas::mat33 &H_CnC0,
                           const jafar::image::Image &imgIn,
                           jafar::image::Image &mosaicOut );
        void addCornersToMosaic ( const jblas::mat33 &H_C0Cn,
                                  const jafar::image::Image &imgIn,
                                  jafar::image::Image &mosaicOut );
        
        void calcHomographyDecomposition ( const jblas::mat33 &H_C1C0, const jblas::mat33 K,
                                           jblas::mat33 &R_C1C0, jblas::vec3 &t_C1, jblas::vec3 &n_C0 );
        
        // projection statistics
        void calcProjErrorStatsOld ( const jblas::mat33 &H_C0C1, const jafar::gfm::MatchingResult &rm );
        void calcProjErrorStats ( const jblas::mat33 &H_C0C1, const MatchesContainer &mc );
        
      private:
        void calcA_i ( const double x0u, const double x0v, const double x1u, const double x1v, jblas::mat &A_i, const double weight = 1.0 );
//        void calcA_i ( const fdetect::InterestFeature* x0, const fdetect::InterestFeature* x1, jblas::mat &A_i );
        void calcJ_i( const fdetect::InterestFeature* x0, const fdetect::InterestFeature* x1, jblas::mat &J_i );
        void updatePlaneParameters ( void );
        void calcBoundingRect ( const jblas::mat33 &H_COutCIn,
            const jafar::image::Image &imgIn, cv::Rect &boundingRect );
        void shrinkBoundingRectToImage ( const jafar::image::Image &img, cv::Rect &boundingRect );
        
//        template <class Vec> void printVec ( const Vec &vec );
//        template <class Mat> void printMat ( const Mat &mat );
        
//        void calcMatchesCentroid ( const jafar::gfm::MatchingResult &rm,
//                                   jblas::vec2 &centroid_ref, jblas::vec2 &centroid_match );
//        void calcMatchesMeanDist ( const jafar::gfm::MatchingResult &rm, const jblas::vec2 &centroid, double &meanDist );
        
    };
    
    
  } // namespace traversability
} // namespace jafar

#endif