qdRotCensus.hpp

#ifndef QD_ROTCENSUS_HPP
#define QD_ROTCENSUS_HPP

#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <bitset>

#include <math.h>

#include "kernel/jafarMacro.hpp"
#include "jmath/jblas.hpp"
#include "image/Image.hpp"
#include "quasidense/qdImgCensus.hpp"
#include "boost/timer.hpp"

namespace jafar {

  namespace quasidense {

    struct rotDataCensus {
      
      //boost::numeric::ublas::matrix<bitset80> censusData;
      bitset80 *censusData;

      unsigned int width;
      unsigned int height;
      unsigned int widthstep;
      
      boost::numeric::ublas::matrix<bool> vMask;      
      
    private :

      unsigned int rExclu;

      unsigned int estimExcluRadius (double angle);
      
      void censusShiftsGenerator (long int *tab, unsigned int tabSize,
          unsigned int indCircQuadrant, unsigned int indRotAngle);
            
      
    public :

      rotDataCensus (jafar::image::Image &img, double angle, double cmThreshold=0.01) :
  width(img.width()),
  height(img.height()),
  widthstep(img.step()),
  vMask(img.height(),img.width())
      {
  JFR_PRECOND(angle>-M_PI && angle<=M_PI, "quadisense::rotDataCensus::rotDataCensus : Constructor waits for angle in ]-PI,PI]");
  JFR_PRECOND(img.depth() == IPL_DEPTH_8U , "quasidense::rotDataCensus::rotDataCensus : Constructor don't tolerate float images");
  
  unsigned int pos, posIma;
  unsigned int rExclu;
  unsigned int nbrow = img.height(), nbcol = img.width();;
  double vmax1, vmax2, cstnormal;
  double *diffv, *diffh;

  diffv = new double [nbrow*nbcol];
  diffh = new double [nbrow*nbcol];
    
//  width = img.width();
//  height = img.height();
//  widthstep = img.step(); 
  censusData = new bitset80[nbrow*widthstep];

  unsigned char *rawData = reinterpret_cast<unsigned char*>(img.data());
  unsigned char current_gl;
  cstnormal = 1.0/255.0;
  
  boost::timer chrono;

  // ******************************************************************
  //
  //                       PART 0 : ANGLE DECOMPOSITION
  //  
  // ******************************************************************
  
  if (angle<-13*M_PI/180)
    angle += 2.0*M_PI;
  
  unsigned int configBase, configLUT;
  double angleRed;

  if (angle<77*M_PI/180)
    configBase = 0;
  else if (angle<167*M_PI/180)
    configBase = 1;
  else if (angle<257*M_PI/180)
    configBase = 2;
  else 
    configBase = 3;
    
  angleRed = angle - configBase*M_PI_2;
  rExclu = 3;

  if (angleRed<13*M_PI/180)
    {
      rExclu = 2;
      configLUT = 0;
    }
  else if (angleRed<18*M_PI/180)
    {
      rExclu = 2;
      configLUT = 1;
    }
  else if (angleRed<25*M_PI/180)
    configLUT = 2;
  else if (angleRed<35*M_PI/180)
    configLUT = 3;
  else if (angleRed<40*M_PI/180)
    configLUT = 4;  
  else if (angleRed<50*M_PI/180)
    configLUT = 5;  
  else if (angleRed<55*M_PI/180)
    configLUT = 6;  
  else if (angleRed<66*M_PI/180)
    configLUT = 7;  
  else if (angleRed<73*M_PI/180)
    configLUT = 8;
  else if (angleRed<77*M_PI/180)
    {
      rExclu = 2;
      configLUT = 9;
    }          
  
  // ******************************************************************
  //
  //                       PART I : MASK GENERATION
  //  
  // ******************************************************************
  

        // ******************************************************************
  //  vMask initialization and optimized diffmap computation 
  //  4 steps : upper left corner / first row / first line / the rest
  //
  
  vMask(0,0) = false;
  diffv[0] = 0.0;
  diffh[0] = 0.0;
  
  pos = nbcol;
  for (unsigned int cptr=1; cptr<nbrow ; ++cptr,pos+=nbcol)
    {  
      vMask(cptr,0) = false;
      diffv[pos] = cstnormal*fabs((double)rawData[cptr*widthstep]-(double)rawData[(cptr-1)*widthstep]);
      diffh[pos] = 0.0;
    }
  
  for (unsigned int cptc=1; cptc<nbcol;cptc++)
    {  
      vMask(0,cptc) = false;
      diffv[cptc] = 0.0;
      diffh[cptc] = cstnormal*fabs((double)rawData[cptc]-(double)rawData[cptc-1]);
    }
  
  pos = nbcol;
  for (unsigned int cptr = 1; cptr<nbrow; ++cptr)   
    {
      pos++;
      posIma = cptr*widthstep;
      for (unsigned int cptc = 1; cptc<nbcol; ++cptc, ++pos)
        {
    vMask(cptr,cptc) = false;
    diffv[pos] = cstnormal*fabs((double)rawData[posIma+cptc]-(double)rawData[posIma+cptc-widthstep]);
    diffh[pos] = cstnormal*fabs((double)rawData[posIma+cptc]-(double)rawData[posIma+cptc-1]);
        }
    } 
  
  // *******************************************
        //  Mask Generation
  //  We exclude points too near of image sides
  //  In order to taking into account rotated 
  //  patch, exlcusion radius is doubled
  //

  for (unsigned int cptr=2*rExclu;cptr<(nbrow-2*rExclu);++cptr)
    {
      pos = cptr*nbcol+2*rExclu;
      for (unsigned int cptc=2*rExclu;cptc<(nbcol-2*rExclu);++pos,++cptc)
        {
    vmax1 = (diffv[pos]<diffh[pos])?diffh[pos]:diffv[pos];
    vmax2 = (diffv[pos+nbcol]<diffh[pos+1])?diffh[pos+1]:diffv[pos+nbcol];
    if ((vmax1>=cmThreshold)||(vmax2>=cmThreshold))
      vMask(cptr,cptc) = true;        
        }  
    }       

  // ******************************************************************
  //
  //                 PART II : CENSUS TRANSFORMATION
  //  
  // ******************************************************************

    
  // Variables for rank Transform
  unsigned int N, rCensus = 2;
  unsigned int testNumber;
  long int *shiftsTabular;

  N = (rCensus*2+1);
  testNumber = (N*N-1)/2;

  // Memory Allocation
  shiftsTabular = new long int [testNumber];  
  censusShiftsGenerator (shiftsTabular, testNumber, configBase, configLUT);
  
  //
  // Census Transform
  //
  // Process is optimized by using the symetry of the process
  // (comparison of pixels A and B permits to update bitstream
  // corresponding to A and B in the same time). To avoid
  // border effects, pixels located in a 2rExclu-width band
  // are marked as unused in vmask.
  //   
  
  unsigned int current_pos, tested_pos, irow, icol;
  
  for (pos=0;pos<widthstep*nbrow;++pos)
    censusData[pos].reset();
  
  for (irow = rExclu; irow<(nbrow-rExclu) ; ++irow)
    {
      pos = irow*widthstep;
      for ( icol = rExclu; icol<(nbcol-rExclu) ; ++icol)
        {
    current_pos = pos+icol;
    current_gl = rawData[current_pos];

    for (unsigned int cpt_test=0;cpt_test<testNumber;++cpt_test)
      {
        tested_pos = current_pos+shiftsTabular[cpt_test];
        if (current_gl!=rawData[tested_pos])
          if (current_gl<rawData[tested_pos])
      censusData[current_pos].set(cpt_test,1);
          else      
      censusData[tested_pos].set(N*N-1-cpt_test,1);
      }
        }
    }

  // 
  std::cout << " Census Transform accomplished in " << chrono.elapsed() << "sec." << std::endl;
      
  // ******************************************************************
  // 
  //                 PART III : SOME CLEANING
  //
  // ******************************************************************

  delete[] diffv;
  delete[] diffh;
  delete[] shiftsTabular; 

      }; // end of constructor

      ~rotDataCensus(void)
      {
  delete[] censusData;
      };
      
    }; // end of struct rotDataCensus
    

        
    class rotPairCensus {
      
      rotDataCensus img1;
      
      rotDataCensus img2;
            
    public :
      rotPairCensus (jafar::image::Image &img1_, jafar::image::Image &img2_, double angle_=0.0, double cmThreshold_=0.01) : 
  img1(img1_, 0.0, cmThreshold_),
  img2(img2_, angle_, cmThreshold_)
      {  };      
      
      bool isPointValidinIma1 (int u, int v);
      bool isPointValidinIma2 (int u, int v);

      bool isMatchValid (int u1, int v1, int u2, int v2);
      bool isMatchValid (int u1, int v1, int u2, int v2, unsigned int radius);
      
      double matchEval (int u1, int v1, int u2, int v2);
      double matchEval (int u1, int v1, int u2, int v2, unsigned int radius);

      void confirmedMatch (int u1, int v1, int u2, int v2)
      {
  img1.vMask(v1,u1) = false;
  img2.vMask(v2,u2) = false;
      };
      
    }; // end of class imgPairRank
          
             
  } // end namespace quasidense

}// end namespace jafar

#endif