univReprojection.hpp

#ifndef CAMERA_UNIVREPROJECTION_HPP
#define CAMERA_UNIVREPROJECTION_HPP

#include <cmath>
#include <iostream>

#include "kernel/jafarException.hpp"
#include "kernel/jafarDebug.hpp"

#include "image/Image.hpp"

#include "camera/cameraPinhole.hpp"
#include "camera/cameraBarreto.hpp"

#include "geom/t3dEuler.hpp"

namespace jafar {
  namespace camera {

    template<typename _channeltype> 
    void univReprojectionCylindric(CameraBarreto const& cameraBarreto,
          const jafar::image::Image& src_, jafar::image::Image& dst_, int interpolation_)
    {
      JFR_PRECOND(src_.width() == cameraBarreto.camera.width && src_.height() == cameraBarreto.camera.height,
      "reprojectionCylindric: source image size is invalid");
      JFR_PRECOND(src_.channels() == dst_.channels(),
      "reprojectionCylindric: images must have the same number of channel");       
      
      double phi, phiInt, phiExt, phiMin, phiMax, theta, thetaMin, thetaMax, phiStep, thetaStep;     
      double r=0.0;

      int nbPixel = 0,i,j;
      int dstNbCol = dst_.width(), dstNbRow = dst_.height();
      int ichannel, nbchannel = dst_.channels();
      
      
      jblas::vec3 direction, point3D;
      jblas::vec2 pixel, tmp;    

      _channeltype imaData; 

      /* Output image initialization */
      /* --------------------------- */
      //      cvSetZero(dst_);
      (cv::Mat)dst_ = cv::Mat::zeros(dst_.size(), dst_.type());
      
      /* Elevation angle bounds */
      /* ---------------------- */
      /* In first : elevation for inner circle */
      pixel(1) = cameraBarreto.mirrorCenterV;
      pixel(0) = cameraBarreto.mirrorCenterU+cameraBarreto.maskRadius;
      cameraBarreto.imageToCameraFrameDirection<jblas::vec2,jblas::vec3>(pixel, direction);
      tmp(0) = direction(0); tmp(1) = direction(1);
      r = ublas::norm_2(tmp);
      
      if (r<=1e-4)
  phiInt = (direction(2)>0.0)?0.5*M_PI:-0.5*M_PI;
      else
  phiInt = atan(direction(2)/r);

      /* In second : elevation for outer circle */
      pixel(1) = cameraBarreto.mirrorCenterV;
      pixel(0) = cameraBarreto.imageRadius + cameraBarreto.mirrorCenterU;
      cameraBarreto.imageToCameraFrameDirection<jblas::vec2,jblas::vec3>(pixel, direction);
      tmp(0) = direction(0); tmp(1) = direction(1);
      r = ublas::norm_2(tmp);
      
      phiExt = atan(direction(2)/r);     

      /* Angle discretization */
      /* -------------------- */
      thetaMax = M_PI/2.0;thetaMin = -3.0*M_PI/2.0;
      phiMin = (phiExt>phiInt)?phiInt:phiExt;
      phiMax = (phiExt>phiInt)?phiExt:phiInt;
      
      thetaStep = (thetaMax-thetaMin)/dstNbCol;
      phiStep   = (phiMax-phiMin)/dstNbRow;
      
      /* Loop on dst_ pixel */
      /* ------------------ */
      for (j=0, phi = phiMax ; j<dstNbRow ; j++, phi -= phiStep)  
  for (i=0, theta = thetaMin; i<dstNbCol ; i++, theta -= thetaStep, nbPixel++)
    {
      point3D(0) = cos(phi)*cos(theta);
      point3D(1) = cos(phi)*sin(theta);
      point3D(2) = sin(phi);
      
      if ( cameraBarreto.project<jblas::vec3,jblas::vec2>(point3D,pixel) )
        for (ichannel = 0; ichannel < nbchannel; ichannel++) 
    {
      /* Grey level in source image */
      if (interpolation_)
        imaData = (_channeltype)src_.getSubPixelValue<_channeltype>(pixel(0),pixel(1),ichannel);        
      else
        imaData = src_.getPixelValue<_channeltype>((int)pixel(0),(int)pixel(1),ichannel);       
      
      /* Put this value in destination iage */
      dst_.setPixelValue<_channeltype>(imaData,i,j,ichannel); 
    }  
      
    } // end of the loop on dst_ image     
      
    }; // reprojectionCylindric function


    bool pixelCyl2pixelPano (CameraBarreto const& cameraBarreto, 
           const jafar::image::Image& cylIma, 
           const jblas::vec2& pixelCyl,
           jblas::vec2& pixelPano)      
    {      
      JFR_PRECOND(pixelCyl(0)>=0 && pixelCyl(1)<cylIma.width() && pixelCyl(1)>=0 && pixelCyl(1)<cylIma.height(),      
      "pixelCyl2pixelPano : specified pixel is out of cylindrical image");

      double phi, phiInt, phiExt, phiMin, phiMax, theta, thetaMin, thetaMax, phiStep, thetaStep;     
      double r=0.0;
      int dstNbCol = cylIma.width(), dstNbRow = cylIma.height();
      
      jblas::vec2 pixel;
      jblas::vec3 direction, point3D;    
      
      /* Elevation angle bounds */
      /* ---------------------- */
      /* In first : elevation for inner circle */
      pixel(1) = cameraBarreto.mirrorCenterV;
      pixel(0) = cameraBarreto.mirrorCenterU+cameraBarreto.maskRadius;
      cameraBarreto.imageToCameraFrameDirection<jblas::vec2,jblas::vec3>(pixel, direction);
      pixel(0) = direction(0); pixel(1) = direction(1);
      r = ublas::norm_2(pixel);
      
      if (r<=1e-4)
  phiInt = (direction(2)>0.0)?0.5*M_PI:-0.5*M_PI;
      else
  phiInt = atan(direction(2)/r);

      /* In second : elevation for outer circle */
      pixel(1) = cameraBarreto.mirrorCenterV;
      pixel(0) = cameraBarreto.imageRadius + cameraBarreto.mirrorCenterU;
      cameraBarreto.imageToCameraFrameDirection<jblas::vec2,jblas::vec3>(pixel, direction);
      pixel(0) = direction(0); pixel(1) = direction(1);
      r = ublas::norm_2(pixel);
      
      phiExt = atan(direction(2)/r);     

      /* Angle discretization */
      /* -------------------- */
      thetaMax = M_PI/2.0;thetaMin = -3.0*M_PI/2.0;
      phiMin = (phiExt>phiInt)?phiInt:phiExt;
      phiMax = (phiExt>phiInt)?phiExt:phiInt;
      
      thetaStep = (thetaMax-thetaMin)/dstNbCol;
      phiStep   = (phiMax-phiMin)/dstNbRow;
      

      /* Pixel reprojection */
      /* ------------------ */
      theta  = thetaMin - pixelCyl(0)*thetaStep;
      phi = phiMax - pixelCyl(1)*phiStep;
      
      point3D(0) = cos(phi)*cos(theta);
      point3D(1) = cos(phi)*sin(theta);
      point3D(2) = sin(phi);
      
      return cameraBarreto.project<jblas::vec3,jblas::vec2>(point3D,pixelPano); 
      
    }; // pixelCyl2pixelPano

    template<typename _channeltype>
    void univReprojectionPerspective(CameraParabolicBarreto const& cameraBarreto,
              const jafar::image::Image& src_, 
              jafar::image::Image& dst_, 
             double elev_, double azim_, double roll_, double vOpen_, int interpolation_)
    {
      JFR_PRECOND(src_.width() == cameraBarreto.camera.width && src_.height() == cameraBarreto.camera.height,
      "reprojectionPerspective: confllict between source image size and calibration parameters.");
      JFR_PRECOND(src_.channels() == dst_.channels(),
      "reprojectionPerspective: images must have the same number of channel.");       

      double u0out, v0out, fu, fv, hOpen;
      int i=3, j, ichannel, nbchannel = dst_.channels();
      int dstNbCol = dst_.width(), dstNbRow = dst_.height();

      jblas::vec4 pth, ptest;
      jblas::vec3 direction, point3D, pt;
      jblas::vec2 pixel, pixelp, tmp;        
      jblas::mat44 matM;
      jblas::mat33 matR, tmatR, matR1, matR2;

      _channeltype imaData;

      jafar::geom::T3DEuler maT3DEuler;
      jafar::camera::CameraPinhole vCam;       
            
      /* Virtual camera initialization */
      /* ----------------------------- */
      hOpen = dstNbCol*vOpen_/dstNbRow;
      u0out = (double)(dstNbCol-1)/2.0;
      v0out = (double)(dstNbRow-1)/2.0;     
      fv = dstNbRow/(2.0*tan(vOpen_/2.0));  
      fu = dstNbCol/(2.0*tan(hOpen/2.0));    
      vCam.setParameters(dstNbCol, dstNbRow, u0out, v0out, fu, fv);

      /* Initialization T3D */
      /* ------------------ */      

      /* !  \internal Here we generate the rotation matrix describing
   how to pass from the mirror framework to the virtual
   perspective camera framework. We proceed in two steps.

   In the first step, we align mirror framework with this of the
   virtual perspective camera. Orientation of this camera is
   defined by two angles : elev_ and azim_. Be R1 this matrix

   In the second step,we applied a rotation around the optical axis
   (oZ). Be R2 the corresponding rotation matrix

   Our transform is formally R = R2*R1 and X|mirror =
   transpose(R)*X|cam. This transposition is unneccessary
   because of T3D class specifications.

       */
      
      maT3DEuler.setValue(0,0,0,roll_,0,0);
      matM = maT3DEuler.getM();
      for (i=0;i<3;i++)
  for (j=0;j<3;j++)
    matR2(i,j) = matM(i,j);         
        
      maT3DEuler.setValue(0,0,0,M_PI+azim_,0,-elev_ - M_PI/2.0);
      matM = maT3DEuler.getM();     
      for (i=0;i<3;i++)
  for (j=0;j<3;j++)
    matR1(i,j) = matM(i,j);

      matR = ublas::prod(matR1,matR2);
    
      /* Output image initialization */
      /* --------------------------- */
      //      cvSetZero(dst_);
      (cv::Mat)dst_ = cv::Mat::zeros(dst_.size(), dst_.type());
      /* Loop on dst_pixel */
      /* ----------------- */
      for (j=0;j<dstNbRow;j++)  
  {
    pixelp(1) = j;
    for (i=0;i<dstNbCol;i++)
      {       
        pixelp(0) = i;      
        vCam.imageToCameraFrame<jblas::vec2,jblas::vec3>(pixelp,pt);
        
        point3D = ublas::prod(matR,pt);

        if (cameraBarreto.project<jblas::vec3,jblas::vec2>(point3D,pixel) ) 
    for (ichannel = 0; ichannel < nbchannel; ichannel++) 
      {
        
        /* Grey level in source image */
        if (interpolation_)
          imaData = (_channeltype)src_.getSubPixelValue<_channeltype>(pixel(0),pixel(1),ichannel);        
        else
          imaData = src_.getPixelValue<_channeltype>((int)pixel(0),(int)pixel(1),ichannel);       
        
        /* Put this value in destination iage */
        dst_.setPixelValue<_channeltype>(imaData,i,j,ichannel); 

      }  // End of Channel loop

        
      } // End of Column loop

  } // End of Row loop

      
    }; // univReprojectionPerspective() function       
    
  } // namespace camera

} // namespace jafar

#endif // CAMERA_GENREPROJ_HPP