qdImgCensus.hpp

#ifndef QD_IMGCENSUS_HPP
#define QD_IMGCENSUS_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 "boost/timer.hpp"

namespace jafar {

  namespace quasidense {

    
    // Census on 9x9 window / one bit by pixel
    typedef std::bitset<80> bitset80;
    
    
    struct imgDataCensus {
      
      //boost::numeric::ublas::matrix<bitset80> censusData;
      bitset80 *censusData;

      unsigned int width;
      unsigned int height;
      unsigned int widthstep;
      
      boost::numeric::ublas::matrix<bool> vMask;      
      
      imgDataCensus (jafar::image::Image &img , unsigned int rCensus=2, unsigned int rExclu=2, double cmThreshold=0.01) :
  vMask(img.height(),img.width())
      {
  JFR_PRECOND(img.depth() == IPL_DEPTH_8U , "quasidense::imgDataCensus::imgDataCensus : Constructor don't tolerate float images");
  JFR_PRECOND( rCensus<=4, "quasidense::ingDataCensus::imgDataCensus : 4 is the maximal radius tolarated by Census Transform");
  
  unsigned int pos, posIma;
  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 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
  //

  for (unsigned int cptr=rExclu;cptr<(nbrow-rExclu);++cptr)
    {
      pos = cptr*nbcol+rExclu;
      for (unsigned int cptc=rExclu;cptc<(nbcol-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 R = rCensus, N = (rCensus*2+1);
  unsigned int maxMissingTestNumber = R*(R+1);
  unsigned int effectiveMTN;
  unsigned int testNumber = (N*N-1)/2;

  unsigned int *bitIndexTabular, *missingBitIndexTabular;
  long int *shiftsTabular, *missingShiftsTabular;
  
  // Memory Allocation
  bitIndexTabular = new unsigned int [N*N];
  shiftsTabular = new long int [testNumber];
  missingBitIndexTabular = new unsigned int [maxMissingTestNumber];
  missingShiftsTabular = new long int [maxMissingTestNumber];
  
  //
  // STEP #1 - Some pre-caclulations
  // 
  
  // bitIndexFilling filling
  unsigned int R_int=0, perim = 0, halfperim = 0, firstindex = 0, index = 0;
  unsigned int icol = 0, irow = 0;
  
  for (unsigned int iorbit=0;iorbit<R;iorbit++)
    {
      R_int = R-iorbit;
      perim = 8*R_int;
      halfperim = 4*R_int;
      
      index = firstindex;
      
      for (icol=iorbit;icol<(N-iorbit);++icol)
        {
    bitIndexTabular[iorbit*N+icol] = index;
    bitIndexTabular[(2*R-iorbit)*N+2*R-icol] = index + halfperim;
    index++;
        }      
      --icol;
      
      for (irow=iorbit+1;irow<(N-iorbit-1);irow++)
        {
    bitIndexTabular[irow*N+icol] = index;
    bitIndexTabular[(2*R-irow)*N+2*R-icol] = index + halfperim;
    index++;
        }
      
      firstindex += perim;
      
    }

  bitIndexTabular[testNumber] = 0;
  
  // shiftsTabularFilling      
  for (pos = 0, irow = 0; irow<R ; ++irow)
    for (icol = 0; icol<N; ++icol, ++pos)     
      shiftsTabular[pos] = (irow-R)*widthstep+(icol-R);     
  
  for (icol = 0; icol<R; ++icol, ++pos)
    shiftsTabular[pos] = icol-R;
  
  
  //
  // STEP #2 - Census Transform
  //   
  
  unsigned int current_pos, tested_pos;
  
  for (pos=0;pos<widthstep*nbrow;++pos)
    censusData[pos].reset();
  
  // 1st Block
  for (irow = R; irow<nbrow ; ++irow)
    {
      pos = irow*widthstep;
      for ( icol = R; icol<(nbcol-R) ; ++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(bitIndexTabular[cpt_test],1);
          else
      censusData[tested_pos].set(bitIndexTabular[N*N-1-cpt_test],1);
      }
        }
    }


  // R-first effective columns 
  for (icol=R;icol<2*R;icol++)
    {      
      // Find missing tests
      effectiveMTN = R*(2*R-icol);
      for (unsigned int ccol = 0, pos = 0; ccol<(2*R-icol); ++ccol)
        for(irow = 1; irow<=R;++irow,++pos)
    {
      missingShiftsTabular[pos] = irow*widthstep-R+ccol;
      missingBitIndexTabular[pos] = bitIndexTabular[(irow+R)*N+ccol];
    }
      
      // Make tests
      for (irow=R;irow<(nbrow-R);++irow)
        {
    current_pos = irow*widthstep+icol;
    current_gl = rawData[current_pos];
    for (unsigned int cpt_test=0;cpt_test<effectiveMTN;++cpt_test)
      {
        tested_pos = current_pos+missingShiftsTabular[cpt_test];
        if (current_gl<rawData[tested_pos])
          censusData[current_pos].set(missingBitIndexTabular[cpt_test],1);
      }
        }
    }
  
  // R-last effective colums
  
  for (icol=(nbcol-2*R);icol<(nbcol-R);++icol)
    {
      // Find missing tests
      effectiveMTN = (R+1)*(icol-nbcol+2*R+1);
      
      for (unsigned int ccol = R, pos = 0; ccol>(R-icol+nbcol-2*R-1); --ccol)
        for(irow = 0; irow<=R;++irow,++pos)
    {
      missingShiftsTabular[pos] = irow*widthstep+ccol;
      missingBitIndexTabular[pos] = bitIndexTabular[(irow+R)*N+ccol+R];
    }
      
      // Make tests
      for (irow=R;irow<(nbrow-R);++irow)
        {
    current_pos = irow*widthstep+icol;
    current_gl = rawData[current_pos];
    for (unsigned int cpt_test=0;cpt_test<effectiveMTN;++cpt_test)
      {
        tested_pos = current_pos+missingShiftsTabular[cpt_test];
        if (current_gl<rawData[tested_pos])
          censusData[current_pos].set(missingBitIndexTabular[cpt_test],1);      
      }
        }
    }
        std::cout << " Census Transform accomplished in " << chrono.elapsed() << "sec." << std::endl;
  // ******************************************************************
  // 
  //                 PART III : SOME CLEANING
  //
  // ******************************************************************

  delete[] diffv;
  delete[] diffh;
  delete[] bitIndexTabular;
  delete[] missingBitIndexTabular;
  delete[] shiftsTabular; 
  delete[] missingShiftsTabular;

      }; // end of constructor

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

        
    class imgPairCensus {

      imgDataCensus img1;
      
      imgDataCensus img2;
      
      unsigned int rCensus;
      
      unsigned int winwidth;
      
    public :
      imgPairCensus (jafar::image::Image &img1_, jafar::image::Image &img2_, unsigned int rCensus_=2, 
         unsigned int rExclu_=2, double cmThreshold_=0.01) : 
  img1(img1_, rCensus_, rExclu_, cmThreshold_),
  img2(img2_, rCensus_, rExclu_, cmThreshold_),
  rCensus(rCensus_),        
  winwidth(2*rCensus_+1)       
      {  };      
      
      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