qdAlgo.hpp

#ifndef QD_ALGO_HPP
#define QD_ALGO_HPP

#include <set>
#include <list>
#include <string>
#include <iomanip>

#include "quasidense/qdMatches.hpp"

#include "quasidense/qdImgRank.hpp"
#include "quasidense/qdImgZncc.hpp"
#include "quasidense/qdImgCensus.hpp"

#include "boost/timer.hpp"

namespace jafar {

  namespace quasidense {

    void initShifts (int rLocal, pixel* &localShiftIma1, pixel** &localShiftIma2, unsigned int* &nbShift);
    void initShiftsWRot (double angle, pixel* &localShiftIma1, pixel** &localShiftIma2, unsigned int* &nbShift);


    template <typename imgPair>
    void matchesPropagation (imgPair &imagesPair, jafar::image::Image &imaRef, 
           std::string filematch, double deltaYaw, int rLocal, double initThres, 
           double propaThres, std::string directory, std::string prefix)
    {
      JFR_PRECOND(deltaYaw>-M_PI && deltaYaw<=M_PI, "quadisense::qdAlgo::matchesPropagation. deltaYaw must be in ]-PI,PI]");

      int cpt, u1, v1, u2, v2;
      unsigned int searchAeraSize, *nbShifts;
      pixel *localShiftIma1, **localShiftIma2;
      double *disparitymap;
      boost::timer t1;
      
      std::ostringstream outFilenameStream;  

      //
      // disparity map constrat enhancement
      // 
      double power=0.75, minnzv = 30, vnorm, maxnzv=255.0;
      vnorm = pow(maxnzv,power);
      
      t1.restart();
      
      //
      // Pre computation of local matching hypotheses
      // 
      
      if (fabs(deltaYaw)<M_PI/18.0)
  {
    initShifts(rLocal, localShiftIma1, localShiftIma2, nbShifts);
    searchAeraSize = (2*rLocal+1)*(2*rLocal+1);
  }
      else
  {
    std::cout << " Rotation angle > Threshold : Propagation process adapted to rotation configuration " << std::endl;
    std::cout << " (3x3 in ref image to 5x5 in insp image)" << std::endl;   
    initShiftsWRot(deltaYaw,localShiftIma1,localShiftIma2,nbShifts);              
    searchAeraSize = 8;
  }
      
      // 
      // Images reading and imgpair allocation
      //
      unsigned int nbrow = imaRef.height(), nbcol = imaRef.width();  
            
      jafar::image::Image exportIma(nbcol,nbrow,IPL_DEPTH_8U,JfrImage_CS_GRAY);
      jafar::image::Image warpedIma(nbcol,nbrow,IPL_DEPTH_8U,JfrImage_CS_GRAY);
      
      disparitymap = new double[nbrow*nbcol];
      for (v1=0,cpt=0;v1<(int)nbrow;++v1)
  for (u1=0;u1<(int)nbcol;++u1,++cpt)
    {
      disparitymap[cpt] = -1.0;
      exportIma.setPixelValue<unsigned char>(0,u1,v1,0);
      warpedIma.setPixelValue<unsigned char>(0,u1,v1,0);    
    }
      
      outFilenameStream << nbrow*nbcol;
      unsigned int nbDigits = outFilenameStream.str().size();
      outFilenameStream.str("");
      
      //
      // Declaration of the set of Seeds and the list of valid matches
      // 
      std::set<pixelmatch*,greaterSimilScore> Seeds;
      std::set<pixelmatch*,greaterSimilScore> Local;
      std::list<pixelmatch*> ValidMatches;
      
      // 
      // Load initial seeds, add to Seeds and update vMask
      // Encore un nouveau format de fichier mais cette fois ci je vais faire 
      // plaisir à Simon : il s'agira d'un fichier texte !
      // 
      // Chaque ligne non vide = quadruplets d'entier + un double  dans l'ordre suivant :
      // icol1, ilig1, icol2, ilig2, score
      // 
      unsigned int nbInitialSeeds = 0, nbInitialValidSeeds = 0;
      std::ifstream matchesfile(filematch.c_str(),std::ios::in);
      pixelmatch *onePM;
      double similScore;
            
      if (matchesfile.is_open()) 
  {
    std::string oneline;
    while(!matchesfile.eof())
      {
        getline(matchesfile,oneline);
        
        if (oneline.size() == 0 || oneline.at(0) == '#')
    continue;
        ++nbInitialSeeds;   
        
        std::istringstream ss(oneline);
        ss >> u1 >> v1 >> u2 >> v2 >> similScore;      
        
        if (similScore >= initThres)
    {
      onePM = new pixelmatch(u1,v1,u2,v2,similScore);
      Seeds.insert(onePM);
      imagesPair.confirmedMatch(u1,v1,u2,v2);
      ++nbInitialValidSeeds;
    }
      }
    matchesfile.close();
  }
      std::cout << " Elapsed time for data formating : " << t1.elapsed() << std::endl;
      std::cout << " # ValidInitial Seeds : " << nbInitialValidSeeds << std::endl;
      
      
      //
      // Propagation algorithm
      //
      pixel pixTestIma1, pixTestIma2;  
      std::set<pixelmatch*,greaterSimilScore>::iterator bestSeed = Seeds.begin();
      std::set<pixelmatch*,greaterSimilScore>::iterator itOnLocal;
      std::list<pixelmatch*>::iterator vmLastEntry;
      std::list<pixelmatch*>::iterator itOnPmList;// = ValidMatches.begin();  
      unsigned int nbiter=0;
      double dispa, dispamax = -(double)nbrow*(double)nbcol;
      std::ofstream outfile, outfile2;
  
      outFilenameStream.str("");
      outFilenameStream << directory << "/" << prefix << "_matchinglist.txt";  
      
      outfile.open(outFilenameStream.str().c_str(),std::ios::out);
      if (outfile.is_open())
  {
    t1.restart();
    while (!Seeds.empty())
      {      
        // Transfer of the best match from Seeds to ValidMatches 
        ValidMatches.push_back(*bestSeed);   
        vmLastEntry = --(ValidMatches.end());
        Seeds.erase(bestSeed);      
        
        // Local set generation 
        pixTestIma1 = (*vmLastEntry)->pix1;
        pixTestIma2 = (*vmLastEntry)->pix2;      
        for (unsigned int cpt1 = 0; cpt1<searchAeraSize ; ++cpt1)
    {   
      u1 = pixTestIma1.u + localShiftIma1[cpt1].u;
      v1 = pixTestIma1.v + localShiftIma1[cpt1].v;
      
      if (imagesPair.isPointValidinIma1(u1,v1))
        {
          for (unsigned int cpt2=0; cpt2<nbShifts[cpt1] ; ++cpt2)
      {
        u2 = pixTestIma2.u+localShiftIma2[cpt1][cpt2].u;
        v2 = pixTestIma2.v+localShiftIma2[cpt1][cpt2].v;
        
        if (imagesPair.isPointValidinIma2(u2,v2))
          {
            similScore = imagesPair.matchEval(u1,v1,u2,v2);
            if (similScore>propaThres)
        {
          onePM = new pixelmatch(u1,v1,u2,v2,similScore);       
          Local.insert(onePM);
        }
          }
      }
        }
    }

//        std::cout << "Local exploration .... OK" << std::endl;
        
        // Transfer from Local to Seeds and update 
        if (!Local.empty())
    {
      itOnLocal = Local.begin();    
      while (itOnLocal != Local.end())
        {       
          u1 = ((*itOnLocal)->pix1).u;
          v1 = ((*itOnLocal)->pix1).v;
          u2 = ((*itOnLocal)->pix2).u;
          v2 = ((*itOnLocal)->pix2).v;
      
          if (imagesPair.isMatchValid(u1,v1,u2,v2))
      {
        Seeds.insert(*itOnLocal);
        imagesPair.confirmedMatch(u1,v1,u2,v2);
        Local.erase(itOnLocal);
      }            
          else
      {
        free(*itOnLocal);
        Local.erase(itOnLocal);
      }
          ++itOnLocal;
        }
    }

//        std::cout << " Global list updated successfully ! " << std::endl;
        
        bestSeed = Seeds.begin();
        ++nbiter;
        dispa = (*vmLastEntry)->disparity();      
        dispamax = (dispamax>dispa)?dispamax:dispa;       
        
//        std::cout << " nbIter = " << nbiter << std::endl;
//        std::cout << " dispaMax = " << dispamax << std::endl;
    
        // Save the disparity map at regular interval
        if (nbiter%10000==0)
    {
      if (nbiter==10000)
        itOnPmList = ValidMatches.begin();
      else
        ++itOnPmList;
      
      while (itOnPmList != ValidMatches.end())
        {
          dispa = (*itOnPmList)->disparity();
          u1 = ((*itOnPmList)->pix1).u;
          v1 = ((*itOnPmList)->pix1).v;
          disparitymap[v1*nbcol+u1] = dispa;
          
          dispa = maxnzv*dispa/dispamax;
          dispa = minnzv + (maxnzv-minnzv)*pow(dispa,power)/vnorm;
          exportIma.setPixelValue<unsigned char>((unsigned char)dispa,u1,v1,0);
          
          u2 = ((*itOnPmList)->pix2).u;
          v2 = ((*itOnPmList)->pix2).v;
          warpedIma.setPixelValue<unsigned char>(imaRef.getPixelValue<unsigned char>(u1,v1),u2,v2,0);
          
          outfile << u1 << " " << v1 << " " << u2 << " " << v2 << std::endl;
          
          ++itOnPmList;
        }
      --itOnPmList;
      
      outFilenameStream.str("");
      outFilenameStream << directory << "/" << prefix << "dispaMap_after" << std::setfill('0') << std::setw(nbDigits) << nbiter << ".tif";    
      exportIma.save(outFilenameStream.str().c_str(),"disparityMap");               
    }           
      }      
    
    //
    // Export Final Disparity Map 
    // 
        
    ++ itOnPmList;
    
    while (itOnPmList != ValidMatches.end())
      {
        dispa = (*itOnPmList)->disparity();
        u1 = ((*itOnPmList)->pix1).u;
        v1 = ((*itOnPmList)->pix1).v;
        disparitymap[v1*nbcol+u1] = dispa;
        
        dispa = maxnzv*dispa/dispamax;
        dispa = minnzv + (maxnzv-minnzv)*pow(dispa,power)/vnorm;
        exportIma.setPixelValue<unsigned char>((unsigned char)dispa,u1,v1,0);
        
        u2 = ((*itOnPmList)->pix2).u;
        v2 = ((*itOnPmList)->pix2).v;
        warpedIma.setPixelValue<unsigned char>(imaRef.getPixelValue<unsigned char>(u1,v1),u2,v2,0);
        
        outfile << u1 << " " << v1 << " " << u2 << " " << v2 << std::endl;
    
        ++itOnPmList;
      }       
        
    outFilenameStream.str("");
    outFilenameStream << directory << "/" << prefix << "dispaMap_atConvergence_after" << std::setfill('0') << std::setw(nbDigits) << nbiter << ".map";
    outfile2.open(outFilenameStream.str().c_str(),std::ios::binary | std::ios::out);
    if (outfile.is_open())      
      {
        outfile2.write((char*)&nbcol,sizeof(unsigned int));
        outfile2.write((char*)&nbrow,sizeof(unsigned int));
        outfile2.write((char*)disparitymap,nbrow*nbcol*sizeof(double));
        outfile2.close();
      }
    

    outFilenameStream.str("");
    outFilenameStream << directory << "/" << prefix << "dispaMap_atConvergence_after" << std::setfill('0') << std::setw(nbDigits) << nbiter << ".tif";
    exportIma.save(outFilenameStream.str().c_str(),"disparityMap");
    
    outFilenameStream.str("");
    outFilenameStream << directory << "/" << prefix << "Ima1Warped_atConvergence_after" << std::setfill('0') << std::setw(nbDigits) << nbiter << ".tif";
    warpedIma.save(outFilenameStream.str().c_str(),"ImaWarped");
    
    outfile.close();
    std::cout << " " << nbiter-1 << " iterations in  " << t1.elapsed() <<" sec" << std::endl;

  } // end if on outfile.is_open();
      

      //
      // Before quit, free explicitly allocated memory
      //
      
      Seeds.clear();  
      itOnPmList = ValidMatches.begin();
      while (itOnPmList != ValidMatches.end())
  {
    delete *itOnPmList;
    ++itOnPmList;
  }      
      std::cout << " Clear ValidMatches  .... OK!" << std::endl;

      delete[] nbShifts;
      std::cout << " Dynamic variable \"nbshifts\" unallocated " << std::endl;

      for (cpt=0;cpt<(int)searchAeraSize;++cpt)
  delete[] localShiftIma2[cpt];
      std::cout << " Dynamic variable \"localShiftIma2\" unallocated " << std::endl; 

      delete[] disparitymap;
      std::cout << " Dynamic variable \"disparitymap\" unallocated " << std::endl;
    };

  } // end of namespace quasidense
  
} // end of namespace jafar

#endif