trv_core.hpp

#ifndef TRV_CORE_HPP
#define TRV_CORE_HPP

#include "traversability/trv_homography.hpp"
#include "traversability/trv_homographyGeom.hpp"
#include "traversability/trv_homographyFine.hpp"
#include "traversability/trv_homographyMosaic.hpp"
#include "traversability/trv_util.hpp"
#include "traversability/trv_tracker.hpp"
#include "traversability/trv_densificator.hpp"

#include <image/Image.hpp>
#include "jmath/jblas.hpp"
#include "jmath/ublasExtra.hpp"
#include "kernel/jafarException.hpp"

// baustelle!!

namespace jafar {
  namespace traversability {
    
    class Core 
    {
      public:
        
        // image buffer
        int imBufferCnt;
        int imBufferSize;
        jafar::image::Image* imBuffer;
        
        // pose buffer
        int poseBufferCnt;
        int poseBufferSize;
        CameraPoseParameters* poseBuffer;
        
        // traversability buffer
        int trvBufferCnt;
        int trvBufferSize;
        jafar::image::Image* trvBuffer;
        
        // homography buffer (for mosaic update)
        // store always homographies HC0C1
        int homBufferCnt;
        int homBufferSize;
        jblas::mat33* homBuffer;
        
        // homography buffer (for geom precalculation)
        // store always homographies HC0C1
        int preBufferCnt;
        int preBufferSize;
        jblas::mat33* preBuffer;
        
        // pointers and indices to actual object
        int actIdx0;
        int actIdx1;
        jafar::image::Image* actIm0_ptr;
        jafar::image::Image* actIm1_ptr;
        jafar::image::Image* actMsk1_ptr;
        CameraPoseParameters* actPose0_ptr;
        CameraPoseParameters* actPose1_ptr;
        
        // intermediate images
        jafar::image::Image im0_1_fine;
        jafar::image::Image im0_1_geom;
        jafar::image::Image mask1_fine;
        jafar::image::Image mask1_geom;
        
        // homography constructing object
        HomographyGeom Hgeom;
        HomographyFine Hfine;
        HomographyMosaic Hmos;
        HomographyFine HmosUpdate;
        HomographyGeom HpreCalc;
        
        // specific homographies, g:geom and f:fine
        jblas::mat33 H_C0C1g;
        jblas::mat33 H_C1C0g;
        jblas::mat33 H_C0C1f;
        jblas::mat33 H_C1C0f;
        
        // homographies to apply on images
        jblas::mat33 H_C0C1;
        jblas::mat33 H_C1C0;
        
        Tracker* tracker;
        
        Densificator* densificator;
        
        // TODO make use of imageInfo object
        ImageInfo imageInfo;
        
        bool optWithGeomPrecalculation;
        
      public:
        Core ( TrackerType ttype, int bufferSize, int imWidth, int imHeight,
               int winHalfWidth, int winHalfHeight, bool withGeomPrecalc = false );
        ~Core ( void );
        
        // a bit clumpsy... TODO find better way
        void setupGeometry ( const PlaneParameters &plane, const CameraIntrinsicParameters &internals);
//        void setPose0 ( const CameraPoseParameters &camPose0 ) { Hgeom.setPoseCam0(camPose0); }
//        void setPose1 ( const CameraPoseParameters &camPose1 ) { Hgeom.setPoseCam1(camPose1); }

        TrackerType getTrackerType ( void ) const { return tracker->getTrackerType(); }
        
        // TODO: set idx to default value and then use poseBufferCounter...
        void setPose ( const int idx, const CameraPoseParameters &camPose ) {
          ++poseBufferCnt;
          
          poseBuffer[idx%poseBufferSize] = camPose;
        }
        void setImage ( const int idx, const jafar::image::Image &_im ) {
          ++imBufferCnt;
          imBuffer[idx%imBufferSize] = _im;
        }
        void init ( const int idx0, const int idx1 );
        
        
        jafar::image::Image getImage ( int idx ) const { return imBuffer[idx%imBufferSize]; }
        jafar::image::Image getTrv ( unsigned int idx=-1 ) const {
          if (trvBufferCnt==-1) JFR_WARNING("There is no traversability information available");
          return (idx==-1) ? trvBuffer[(trvBufferCnt)%trvBufferSize] : trvBuffer[idx%trvBufferSize]; }
        jafar::image::Image getImage0_1_fine ( void ) const { return im0_1_fine; }
        jafar::image::Image getImage0_1_geom ( void ) const { return im0_1_geom; }
        jafar::image::Image getMask1_fine ( void ) const { return mask1_fine; }
        
        // obsolete function
        MatchesContainer* getMatches ( void ) { return tracker->matches(); }
        
        // obsolete function, left in order to use the tracking engine to calc matches
        // between two images... (only for development purposes)
        void calcTracker ( jafar::image::Image &img0, jafar::image::Image &img1 );
        void calcTracker ( int step = 1 );
        
        void calcHgeom ( void );
        void calcHfine ( const jafar::image::Image &prevTravMask );
        void calcDensify ( void );
        void calcH_mosUpdate ( int idx0, int idx1 );
        void applyHomography ( const jblas::mat33 &H_C0C1,
                               const jafar::image::Image &img0,
                jafar::image::Image &img0_1 );
        
      private:
        void updateHmosBasic ( void );
      public:
        jblas::mat33 getH_C0Cn( void );
        
        
        /* //////////////////////////////////////////////////////////////////////////////
        this function is optimized.
        for images of size 512x384 it needs 7ms
        for images of size 640x480 it needs 11ms
        those values were measured with the method gettimeofday
        measurements taken in ruby have shown much higher values
        (up to ten times slower!)
        template <typename Type> void applyHomographyO( const jblas::mat33 &H_C0C1,
            const jafar::image::Image &img0,jafar::image::Image &img0_1,jafar::image::Image maskCFoV1 )
        {
          int widthIn     = img0.width();
          int heightIn    = img0.height();
          int widthOut    = img0_1.width();
          int heightOut   = img0_1.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];
          
          uint8_t const* im0ptr   = reinterpret_cast<uint8_t const*>(img0.data());
          uint8_t* im0_1ptr = reinterpret_cast<uint8_t*>(img0_1.data());
          uint8_t* msk1ptr  = reinterpret_cast<uint8_t*>(maskCFoV1.data());
            
          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  = (int)round((p0_x/p0_z));
              x0_v  = (int)round((p0_y/p0_z));
      
              if (x0_u >= 0 and x0_v >= 0 and x0_u < widthIn and x0_v < heightIn)
              {
                *(im0_1ptr++) = im0ptr[x0_v*widthIn+x0_u];
//                img0_1.setPixelValue<uint8_t>(img0.getPixelValue<uint8_t>(x0_u,x0_v),i,j);
                
                // change the if clause!!!
//                if (maskCFoV1)
                if (maskCFoV1.data() and x0_u>=5 and x0_v>=5 and x0_u < (widthIn-5) and x0_v < (heightIn-5))
                {
                  *(msk1ptr++) = static_cast<uint8_t>(255);
//                  maskCFoV1.setPixelValue<uint8_t>(255,i,j);
                }
                else
                {
                  *(msk1ptr++) = static_cast<uint8_t>(0);
                }
              }
              else
              {
                im0_1ptr++;
                *(msk1ptr++) = static_cast<uint8_t>(0);
//                img0_1.setPixelValue<uint8_t>(0,i,j);
              }
            }
          }
          
          delete [] h00u;
          delete [] h10u;
          delete [] h20u;
        }
      
    };
  } // namespace traversability
} // namespace jafar

#endif