qdOmniCensus.hpp

#ifndef QD_OMNICENSUS_HPP
#define QD_OMNICENSUS_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/qdOmniMask.hpp"
#include "quasidense/qdImgCensus.hpp"

#include "boost/timer.hpp"

namespace jafar {

  namespace quasidense {

    
    struct omniDataCensus {
      
      jafar::quasidense::bitset80 *censusData;

      unsigned int width;

      unsigned int height;

      unsigned int widthstep;
      
      boost::numeric::ublas::matrix<bool> vMask;      
      
      omniDataCensus (jafar::image::Image &img , jafar::camera::CameraBarreto &omniCam,
          jafar::quasidense::omniMask &geomMask, double oasCensus=0.0079, 
          unsigned int rCensus=2, double hrdThreshold=0.01) :
  width(img.width()),
  height(img.height()),
  widthstep(img.step()),
  vMask(img.height(),img.width())
      {
  JFR_PRECOND(img.depth() == IPL_DEPTH_8U , "quasidense::omniDataCensus::omniDataCensus : Constructor don't tolerate float images");
  JFR_PRECOND( rCensus<=4, "quasidense::ingDataCensus::imgDataCensus : 4 is the maximal radius tolarated by Census Transform");
  
  boost::timer chrono;

  unsigned int pos, posIma;
  unsigned int nbrow = img.height(), nbcol = img.width();
  double vmax1, vmax2, vmaxS1, vmaxS2, cstnormal;
  double *diffu, *diffl, *difful, *diffur;

  diffu = new double [nbrow*nbcol];
  diffl = new double [nbrow*nbcol];
  difful = new double [nbrow*nbcol];
  diffur = new double [nbrow*nbcol];
    
  censusData = new bitset80[nbrow*widthstep];

  unsigned char *rawData = reinterpret_cast<unsigned char*>(img.data());
  cstnormal = 1.0/255.0;
  

  // ******************************************************************
  //
  //                       PART I : MASK GENERATION
  //  
  // ******************************************************************

  // ******************************************************************

        // ******************************************************************
  //  vMask initialization and optimized diffmap computation 
  //  5 steps : first row/last row/first column/last column/the rest
  //  
  
  chrono.restart();
  
  // First row
  for (unsigned int cptc=0, cptr=0; cptc<nbcol; ++cptc)
    {
      vMask(cptr,cptc) = false;
      diffu[cptc] = 0.0;
      diffl[cptc] = 0.0;
      difful[cptc] = 0.0;
      diffur[cptc] = 0.0;
    }

  // Last row
  pos = nbcol*(nbrow-1);
  posIma = (nbrow-1)*widthstep;
  for (unsigned int cptc=1, cptr=nbrow-1; cptc<=(nbcol-2); ++cptc)
    {
      vMask(cptr,cptc) = false;

      diffl[pos+cptc] = 0.0;
      diffu[pos+cptc] = cstnormal*fabs( (double)rawData[posIma+cptc] - 
                (double)rawData[posIma-widthstep+cptc] );

      difful[pos+cptc] = cstnormal*fabs( (double)rawData[posIma+cptc] - 
                 (double)rawData[posIma-widthstep+cptc-1] );     

      diffur[pos+cptc] = cstnormal*fabs( (double)rawData[posIma+cptc] - 
                 (double)rawData[posIma-widthstep+cptc+1] );
    }

  // First Column
  for (unsigned int cptc=0, cptr=1; cptr<nbrow; ++cptr)
    {
      vMask(cptr,cptc) = false;

      diffl[cptr*nbcol] = 0.0;
      diffu[cptr*nbcol] = cstnormal*fabs( (double)rawData[cptr*widthstep+cptc] - 
            (double)rawData[(cptr-1)*widthstep+cptc] );

      difful[cptr*nbcol] = 0.0;
      diffur[cptr*nbcol] = cstnormal*fabs( (double)rawData[cptr*widthstep+cptc] - 
             (double)rawData[(cptr-1)*widthstep+cptc+1] );
    } 

  // Last Column  
  for (unsigned int cptc=nbcol-1, cptr=1; cptr<nbrow; ++cptr)
    {
      vMask(cptr,cptc) = false;

      diffl[(cptr+1)*nbcol-1] = cstnormal*fabs( (double)rawData[cptr*widthstep+cptc] - 
                  (double)rawData[cptr*widthstep+cptc-1] );
      diffu[(cptr+1)*nbcol-1] = cstnormal*fabs( (double)rawData[cptr*widthstep+cptc] - 
                  (double)rawData[(cptr-1)*widthstep+cptc] );
      
      diffur[(cptr+1)*nbcol-1] = 0.0;
      difful[(cptr+1)*nbcol-1] = cstnormal*fabs( (double)rawData[cptr*widthstep+cptc] - 
                   (double)rawData[(cptr-1)*widthstep+cptc-1] );
    } 
  
  // The rest
  for (unsigned int cptr=1;cptr<=(nbrow-2);++cptr)
    {
      pos = cptr*nbcol;
      posIma = cptr*widthstep;

      for (unsigned int cptc=1;cptc<=(nbcol-2);++cptc)
        {
    vMask(cptr,cptc) = false;
    diffl[pos+cptc] = cstnormal*fabs( (double)rawData[posIma+cptc] - 
              (double)rawData[posIma+cptc-1] );
    diffu[pos+cptc] = cstnormal*fabs( (double)rawData[posIma+cptc] - 
              (double)rawData[posIma-widthstep+cptc] );
      
    diffur[pos+cptc] = cstnormal*fabs( (double)rawData[posIma+cptc] - 
               (double)rawData[posIma-widthstep+cptc+1] );
    difful[pos+cptc] = cstnormal*fabs( (double)rawData[posIma+cptc] - 
               (double)rawData[posIma-widthstep+cptc-1] );
        }
    } 

  std::cout << " First step of Homogeneous area detection accomplished in " << chrono.elapsed() << "sec." << std::endl;
 

  // *******************************************
        //  Mask Generation
  //  We exclude points too near of image sides
  //

  chrono.restart();
  
  for (unsigned int cptr=1; cptr<=(nbrow-2); ++cptr)
    {
      pos = cptr*nbcol;
      for (unsigned int cptc=1; cptc<=(nbcol-2); ++pos, ++cptc)
        if (geomMask.isPixelValid(cptc,cptr))
    {     
      vmaxS1 = (diffl[pos]>diffu[pos])?diffl[pos]:diffu[pos];
      vmaxS2 = (difful[pos]>diffur[pos])?difful[pos]:diffur[pos];
      vmax1 = (vmaxS1>vmaxS2)?vmaxS1:vmaxS2;
      
      vmaxS1 = (diffl[pos+1]>diffu[pos+nbcol])?diffl[pos+1]:diffu[pos+nbcol];
      vmaxS2 = (difful[pos+nbcol+1]>diffur[pos+nbcol-1])?difful[pos+nbcol+1]:diffur[pos+nbcol-1];
      vmax2 = (vmaxS1>vmaxS2)?vmaxS1:vmaxS2;
        
      if ((vmax1>=hrdThreshold)||(vmax2>=hrdThreshold))
        vMask(cptr,cptc) = true; 
    }
    }

  std::cout << " Second step of Homogeneous area detection accomplished in " << chrono.elapsed() << "sec." << std::endl;

//  jafar::image::Image exportIma(nbcol,nbrow,IPL_DEPTH_8U,JfrImage_CS_GRAY);
  
//  unsigned int nbpixelvalid = 0;
  
//  for (unsigned int cptr = 0; cptr<nbrow; ++cptr)
//    for (unsigned int cptc = 0; cptc<nbcol; ++cptc)     
//      if (vMask(cptr,cptc))
//        {
//    exportIma.setPixelValue<unsigned char>(255,cptc,cptr,0);
//    ++nbpixelvalid;
//        }
//      else
//        exportIma.setPixelValue<unsigned char>(0,cptc,cptr,0);  
//  exportIma.save("/home/msanfour/testMasque.tiff","disparityMap");

//  std::cout << "Nombre de pixels valides : " << nbpixelvalid << " / " << width*height << std::endl;
  
  // ******************************************************************
  //
  //                 PART II : CENSUS TRANSFORMATION
  //  
  // ******************************************************************
  
  jblas::vec2 targetPix, onePixTest;
  jblas::vec3 pt3D, pt3DTest;
  
  unsigned char currentGL;
  unsigned int currentPOS;

  int winWcpt=0, winHcpt=0;

  double elev, elev0, azim, azim0, norm3D, targetGL;
  
  chrono.restart();

  for (unsigned int cptr=1; cptr<=(nbrow-2); ++cptr)
    {

      targetPix[1] = cptr;
      currentPOS = cptr*widthstep;

      for (unsigned int cptc=1; cptc<=(nbcol-2); ++cptc)
        {
    
    targetPix[0] = cptc;
    if (vMask(cptr,cptc))
      {
        // get grey level of the current pixel
        currentGL = rawData[currentPOS+cptc];
        
        // pixel to angular position on equivalent sphere
        omniCam.imageToCameraFrame<jblas::vec2,jblas::vec3>(targetPix,pt3D);
        norm3D = ublas::norm_2(pt3D);
        elev0 = M_PI_2 - asin(pt3D[2]/norm3D);
        azim0 = atan2(pt3D[1],pt3D[0]);       

        censusData[currentPOS+cptc].reset();

 //         if (popo==0)
//          {
//      std::cout << "initial bitstream = " << censusData[currentPOS+cptc] << std::endl;
//      std::cout << "CurrentGL = " << (unsigned int)currentGL << std::endl;
//          }

        // Boucle Principale
        for (pos = 0, winWcpt=-rCensus; winWcpt<=(int)rCensus; ++winWcpt)
          {
      azim = azim0 + winWcpt*oasCensus/(double)rCensus;
      for (winHcpt=-rCensus; winHcpt<=(int)rCensus; ++winHcpt, ++pos)
        {
          if ( (winWcpt==0) && (winHcpt==0) )
            {
        --pos;
        continue;
            }
          elev = elev0 + winHcpt*oasCensus/(double)rCensus;
          
          // angular position to position on the equivalent sphere
          pt3DTest[0] = sin(elev)*cos(azim);
          pt3DTest[1] = sin(elev)*sin(azim);
          pt3DTest[2] = cos(elev);
      
          // point on sphere to pixel
          omniCam.project<jblas::vec3,jblas::vec2>(pt3DTest,onePixTest);

          // get radiometry on onePixTest
          targetGL = img.getSubPixelValue<unsigned char>(onePixTest[0],onePixTest[1]);          
      
//          if (popo==0)
//            std::cout << "targetGL = " << targetGL << std::endl;
          
          if ((double)currentGL>=targetGL)
            censusData[currentPOS+cptc].set(pos,1);

        } // end loop on winHcpt
      
          } // end loop on winWcpt    
        
//        if (popo==0)
//          {
//      std::cout << "final bitstream = " << censusData[currentPOS+cptc] << std::endl;
//      popo = 1;
//          }
        
      } // end test on vMask

        } // end of the loop on image columns
        
    } // end of the loop on image rows

        std::cout << " Census Transform accomplished in " << chrono.elapsed() << "sec." << std::endl;
    

      
  // ******************************************************************
  // 
  //                 PART III : SOME CLEANING
  //
  // ******************************************************************

  delete[] diffl;
  delete[] diffu;
  delete[] difful;
  delete[] diffur;  

      }; // end of constructor

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

    class omniPairCensus {

      omniDataCensus img1;
      
      omniDataCensus img2;
      
      unsigned int rCensus;
      
      unsigned int winWidth;
      
    public :
      
      omniPairCensus (jafar::image::Image &img1_, jafar::image::Image &img2_,
          jafar::camera::CameraBarreto &omnicam_,
          jafar::quasidense::omniMask &geomMask_,
          double oasCensus_=0.0079,
          unsigned int rCensus_=2,
          double hrdThreshold_=0.01) :
          
  img1(img1_, omnicam_, geomMask_, oasCensus_, rCensus_, hrdThreshold_),
  img2(img2_, omnicam_, geomMask_, oasCensus_, rCensus_, hrdThreshold_),
  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 of namespace quasidense

} // end of namespace jafar
    
#endif