qdOmniMask.hpp

#ifndef QD_OMNIMASK_HPP
#define QD_OMNIMASK_HPP

#include <string>
#include <vector>

#include <math.h>

#include "kernel/jafarMacro.hpp"
#include "jmath/jblas.hpp"
#include "image/Image.hpp"
#include "camera/cameraBarreto.hpp"

#include "boost/timer.hpp"

namespace jafar {

  namespace quasidense {

    class omniMask {
      
      // Mask image
      boost::numeric::ublas::matrix<bool> maskIma;
      
      // dimension
      unsigned int nbcol;
      unsigned int nbrow;
      
    public : 

      omniMask (jafar::camera::CameraBarreto &omniCam, unsigned int nbcol_, unsigned int nbrow_, float winOverAngle) :
  maskIma(nbrow_, nbcol_),
  nbcol(nbcol_),
  nbrow(nbrow_)
      {
  unsigned int cpt1, cpt2, margin=20; 
  int cpt3;
  double elev, azim, elevTest, azimTest, elevMin, elevMax;

  boost::timer chrono;

  chrono.restart();

  // Initialize maskIma
  for (cpt1=0;cpt1<nbrow;++cpt1)
    for (cpt2=0;cpt2<nbcol;++cpt2)
      maskIma(cpt1,cpt2) = false;
  
  
  jblas::vec2 onePix, onePixTest;
  jblas::vec3 pt3D, pt3DTest;
  
  // Convert omniCam.imageRadius in max elev angle
  onePix[0] = omniCam.mirrorCenterU;
  onePix[1] = omniCam.mirrorCenterV + omniCam.imageRadius;
  
  omniCam.imageToCameraFrame<jblas::vec2,jblas::vec3>(onePix,pt3D);
  double n = ublas::norm_2(pt3D);
  
  // elevation angle is measured with respect to the 0z axis (parabola is oriented
  // towards negative sense ofthe  Oz axis)
  // => elevation angle of the outer circle <elevation angle of the inner circle
  //
  elev = M_PI_2 - asin(pt3D[2]/n);
  elevMax = elev-winOverAngle;      

  // Convert omniCam.maskRadius in min elev angle
  onePix[0] = omniCam.mirrorCenterU + omniCam.maskRadius;
  onePix[1] = omniCam.mirrorCenterV;
  
  omniCam.imageToCameraFrame<jblas::vec2,jblas::vec3>(onePix,pt3D);
  n = ublas::norm_2(pt3D);  
  elev = M_PI_2 - asin(pt3D[2]/n);
  elevMin = elev+winOverAngle;  
  
  // Prune pixel which elevation is not in [elevMax, elevMin]
  for (cpt1=0;cpt1<nbrow;++cpt1)
    {
      onePix[1] = (double)cpt1;
      for (cpt2=0;cpt2<nbcol;++cpt2)
        {
    onePix[0] = (double)cpt2;
    omniCam.imageToCameraFrame<jblas::vec2,jblas::vec3>(onePix,pt3D);
    n = ublas::norm_2(pt3D);
    elev = M_PI_2 - asin(pt3D[2]/n);    
    if (elev<=elevMin && elev>=elevMax)
      maskIma(cpt1,cpt2) = true;
        }
    }
      

  // Detect circles going out image       
  //
  if (omniCam.mirrorCenterU-omniCam.imageRadius<0)

    // Circle goes out on the left of image 
    {
      std::cout << "Circle goes out on the left of image " << std::endl; 
      
      for (cpt1=0;cpt1<nbrow;++cpt1)
        {
    onePix[1] = (double)cpt1;
    for (cpt2=0;cpt2<margin;cpt2++)
      {       
        onePix[0] = (double)cpt2;
        
        if (maskIma(cpt1,cpt2)==false)
          continue;
        
        omniCam.imageToCameraFrame<jblas::vec2,jblas::vec3>(onePix,pt3D);
        n = ublas::norm_2(pt3D);

        elev = M_PI_2 - asin(pt3D[2]/n);
        azim = atan2(pt3D[1],pt3D[0]);        
        elevTest = elev-winOverAngle;

        for (cpt3=-1;cpt3<=1;++cpt3)
          {
      azimTest = azim+cpt3*winOverAngle;
      
      // angle => point on sphere
      pt3DTest[0] = sin(elevTest)*cos(azimTest);
      pt3DTest[1] = sin(elevTest)*sin(azimTest);
      pt3DTest[2] = cos(elevTest);
      
      // point on sphere to pixel
      omniCam.project<jblas::vec3,jblas::vec2>(pt3DTest,onePixTest);
      
      // test on pixel
//      if (onePixTest[0]<0 || onePixTest[0]>nbcol || onePixTest[1]<0 || onePixTest[1]>nbrow)
      if (onePixTest[0]<0 || onePixTest[0]>=(nbcol-1) || onePixTest[1]<0 || onePixTest[1]>=(nbrow-1))
        {
          maskIma(cpt1,cpt2) = false;
          continue;
        }
          }
      }
        }
    }
        
  if (omniCam.mirrorCenterU+omniCam.imageRadius>(nbcol-1))
    
    // Circle goes out on the right of image 
    {
      std::cout << " Circle goes out on the right of image " << std::endl;
      
      for (cpt1=0;cpt1<nbrow;++cpt1)
        {
    onePix[1] = (double)cpt1;
    for (cpt2=nbcol-margin;cpt2<nbcol;++cpt2)
      {       
        onePix[0] = (double)cpt2;
        
        if (maskIma(cpt1,cpt2)==false)
          continue;

        omniCam.imageToCameraFrame<jblas::vec2,jblas::vec3>(onePix,pt3D);
        n = ublas::norm_2(pt3D);

        elev = M_PI_2 - asin(pt3D[2]/n);
        azim = atan2(pt3D[1],pt3D[0]);        
        elevTest = elev-winOverAngle;

        for (cpt3=-1;cpt3<=1;++cpt3)
          {
      azimTest = azim+cpt3*winOverAngle;
      
      // angle => point on sphere
      pt3DTest[0] = sin(elevTest)*cos(azimTest);
      pt3DTest[1] = sin(elevTest)*sin(azimTest);
      pt3DTest[2] = cos(elevTest);
      
      // point on sphere to pixel
      omniCam.project<jblas::vec3,jblas::vec2>(pt3DTest,onePixTest);
      
      // test on pixel
//      if (onePixTest[0]<0 || onePixTest[0]>nbcol || onePixTest[1]<0 || onePixTest[1]>nbrow)
      if (onePixTest[0]<0 || onePixTest[0]>=(nbcol-1) || onePixTest[1]<0 || onePixTest[1]>=(nbrow-1))
        {
          maskIma(cpt1,cpt2) = false;
          continue;
        }
          }
      }
        }
    }

  if (omniCam.mirrorCenterV+omniCam.imageRadius>(nbrow-1))
    // Circle goes out on the bottom of image 
    {
      std::cout << " Circle goes out on the bottom of image " << std::endl;

      for (cpt1=nbrow-margin;cpt1<nbrow;++cpt1)
        {        
    
    onePix[1] = (double)cpt1;
    for (cpt2=0;cpt2<nbcol;++cpt2)
      {       
        onePix[0] = cpt2;
        
        if (maskIma(cpt1,cpt2)==false)
          continue;

        omniCam.imageToCameraFrame<jblas::vec2,jblas::vec3>(onePix,pt3D);
        n = ublas::norm_2(pt3D);

        elev = M_PI_2 - asin(pt3D[2]/n);
        azim = atan2(pt3D[1],pt3D[0]);        
        elevTest = elev-winOverAngle;

        for (cpt3=-1;cpt3<2;++cpt3)
          {
      azimTest = azim+cpt3*winOverAngle;
      
      // angle => point on sphere
      pt3DTest[0] = sin(elevTest)*cos(azimTest);
      pt3DTest[1] = sin(elevTest)*sin(azimTest);
      pt3DTest[2] = cos(elevTest);
      
      // point on sphere to pixel
      omniCam.project<jblas::vec3,jblas::vec2>(pt3DTest,onePixTest);
      
      // test on pixel
//      if (onePixTest[0]<0 || onePixTest[0]>nbcol || onePixTest[1]<0 || onePixTest[1]>nbrow)
      if (onePixTest[0]<0 || onePixTest[0]>=(nbcol-1) || onePixTest[1]<0 || onePixTest[1]>=(nbrow-1))
        {
          maskIma(cpt1,cpt2) = false;
          continue;
        }
          }
      }
        }
    }

  if (omniCam.mirrorCenterV-omniCam.imageRadius<0)
    // Circle goes out on the top of image 
    {
      std::cout << " Circle goes out on the top of image " << std::endl;

      for (cpt1=0;cpt1<margin;++cpt1)
        {
    
    onePix[1] = (double)cpt1;
    for (cpt2=0;cpt2<nbcol;++cpt2)
      {       
        onePix[0] = cpt2;
        
        if (maskIma(cpt1,cpt2)==false)
          continue;

        omniCam.imageToCameraFrame<jblas::vec2,jblas::vec3>(onePix,pt3D);
        n = ublas::norm_2(pt3D);
        
        elev = M_PI_2 - asin(pt3D[2]/n);
        azim = atan2(pt3D[1],pt3D[0]);;       
        elevTest = elev-winOverAngle;

        for (cpt3=-1;cpt3<2;++cpt3)
          {
      azimTest = azim+cpt3*winOverAngle;
      
      // angle => point on sphere
      pt3DTest[0] = sin(elevTest)*cos(azimTest);
      pt3DTest[1] = sin(elevTest)*sin(azimTest);
      pt3DTest[2] = cos(elevTest);
      
      // point on sphere to pixel
      omniCam.project<jblas::vec3,jblas::vec2>(pt3DTest,onePixTest);
      
      // test on pixel
      if (onePixTest[0]<0 || onePixTest[0]>=(nbcol-1) || onePixTest[1]<0 || onePixTest[1]>=(nbrow-1))
        {
          maskIma(cpt1,cpt2) = false;
          continue;
        }
          }
      }
        }
    } 

  std::cout << " Mask computation 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 (maskIma(cptr,cptc))
        {
    exportIma.setPixelValue<unsigned char>(255,cptc,cptr,0);
    ++nbpixelvalid;
        }
      else
        exportIma.setPixelValue<unsigned char>(0,cptc,cptr,0);  
  exportIma.save("/home/msanfour/mask.tiff","maskCorrel");

  
      } // end construction definition

      bool isPixelValid (unsigned int icol, unsigned irow)
      {
  return maskIma(irow,icol);
      }
      
    }; // end omniMask class

  } // end namespace quasidense

} // end namespace jafar


#endif