reprojection.hpp

/* $Id$ */

#ifndef CAMERA_REPROJECTION_HPP
#define CAMERA_REPROJECTION_HPP

#include <cmath>
#include <iostream>

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

#include "image/Image.hpp"

#include "camera/cameraBarreto.hpp"

namespace jafar {
  namespace camera {

    template<typename _channeltype> 
    void reprojectionCylindric(CameraParabolicBarreto 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()==1 && dst_.channels()==1,
      "reprojectionCylindric: images must be mono channel");  

      // h in pixel
      double h = 2*cameraBarreto.p * cameraBarreto.imageRadius / (cameraBarreto.mirrorDiameter/2);
      

      double radiusmin = cameraBarreto.maskRadius;
      double radiusmax = cameraBarreto.imageRadius;

      int dstNbCol = dst_.width(), dstNbRow = dst_.height();
      int nbPixel = 0, i, j; 
      
      double theta = 0.0, thetaMin = 0.0, thetaMax = 2*M_PI, thetaStep;
      double phi = 0.0, phiMin, phiMax, phiStep;
      double x, y, radius = 0.0;
      
      _channeltype imaData; 

      /* Bounds on the angles */
      phiMin = atan2 (radiusmin*radiusmin-h*h,2*radiusmin*h);
      phiMax = atan2 (radiusmax*radiusmax-h*h,2*radiusmax*h);

      /* Angular discretisation*/
      thetaStep = (thetaMax-thetaMin)/dstNbCol;
      phiStep   = (phiMax-phiMin)/dstNbRow;
      
      /* Loop on dst_ pixel */
      for (j=0, phi = phiMax ; j<dstNbRow ; j++, phi -= phiStep)
  {      
    radius = h*(1+sin(phi))/cos(phi);
      
    for (i=0, theta = thetaMin; i<dstNbCol ; i++, theta += thetaStep, nbPixel++)
      {
        x = radius*cos(theta) + cameraBarreto.camera.u0;
        y = radius*sin(theta) + cameraBarreto.camera.v0;
            
        /* Grey level in source image */
        if (interpolation_)
          imaData = (_channeltype)src_.getSubPixelValue<_channeltype>(x,y);       
        else
          imaData = src_.getPixelValue<_channeltype>((int)x,(int)y);        
        
        /* Put this value in destination iage */
        dst_.setPixelValue<_channeltype>(imaData,i,j); 
        
      } // end of the loop over theta
  }  // end loop over phi
      
    }; // reprojectionCylindric function



    template<typename _channeltype> 
    void reprojectionPlane(CameraParabolicBarreto const& cameraBarreto,
         const jafar::image::Image& src_, 
         jafar::image::Image& dst_, 
         double planedist_, double areawidth_, int interpolation_)
    {
      JFR_PRECOND(src_.width() == cameraBarreto.camera.width && src_.height() == cameraBarreto.camera.height,
      "reprojectionPlane: source image size is invalid");
      JFR_PRECOND(src_.channels()==1 && dst_.channels()==1,
      "reprojectionCylindric: images must be mono channel");  

      // h in pixel
      double h = 2*cameraBarreto.p * cameraBarreto.imageRadius / (cameraBarreto.mirrorDiameter/2);

      int dstNbCol = dst_.width(), dstNbRow = dst_.height();  
      int i, j;
      
      double x, xMin, xMax, xStep;
      double y, yMin, yMax, yStep;
      double xMirror, xImage, yImage, yMirror;
      double phi, theta;
      double radiusOnPlane, radiusOnMirror;
          
      _channeltype imaData;
      
      // Bounds of the plane
      xMin = yMin = -areawidth_;
      xMax = yMax = areawidth_;
      
      // Plane discretisation
      xStep = 2*areawidth_/dstNbCol;
      yStep = 2*areawidth_/dstNbRow;
      
      // Loop on dst_ pixel 
      for (j=0, y = yMin; y<yMax; y+=yStep, j++)    
  for (i=0, x = xMin; x<xMax; x+=xStep, i++)
    { 
      radiusOnPlane = sqrt(x*x+y*y);
      
      // Angular coordinates
      phi = -atan2(planedist_,radiusOnPlane);
      theta = atan2(y,x);
      
      // Coordinates on mirror 
      radiusOnMirror = h*(1+sin(phi))/cos(phi); 
      xMirror = radiusOnMirror*cos(theta);
      yMirror = radiusOnMirror*sin(theta);
      
      // Coordinates on image plane
      xImage = cameraBarreto.camera.u0 + xMirror;
      yImage = cameraBarreto.camera.v0 + yMirror;
      
      // Interpolation and copy
      if (interpolation_)       
        imaData = (_channeltype)src_.getSubPixelValue<_channeltype>(xImage,yImage);
      else
        imaData = src_.getPixelValue<_channeltype>((int)xImage,(int)yImage);
      
      dst_.setPixelValue<_channeltype>(imaData,i,j);  
      
    } // end of the pixel loop
      
    }; // reprojectionPlane() funtion

    template<typename _channeltype>
    void reprojectionPerspective(CameraParabolicBarreto const& cameraBarreto,
         const jafar::image::Image& src_, 
         jafar::image::Image& dst_, 
         double elev_, double azim_, double vOpen_, int interpolation_)
    {
      JFR_PRECOND(src_.width() == cameraBarreto.camera.width && src_.height() == cameraBarreto.camera.height,
      "reprojectionPerspective: source image size is invalid");
      JFR_PRECOND(src_.channels()==1 && dst_.channels()==1,
      "reprojectionCylindric: images must be mono channel");  

      // h in pixel
      double h = 2*cameraBarreto.p * cameraBarreto.imageRadius / (cameraBarreto.mirrorDiameter/2);
      
      int dstNbCol = dst_.width(), dstNbRow = dst_.height();
      int i,j;

      double focal, u0, v0;
      double ur,vr;
      double phi, dPhi, theta, dTheta;
      double xImage, yImage, radius;
      
      _channeltype imaData;
      
      // Intrinsic parameters
      u0 = (double)(dstNbCol-1)/2;
      v0 = (double)(dstNbRow-1)/2;     
      focal = dstNbRow/(2*tan(vOpen_/2));  

      // Loop on dst_pixel
      for (j=0;j<dstNbRow;j++)
  {
    vr = -(double)j+v0;
    dPhi = atan2(vr,focal);
    phi = elev_+dPhi;
    radius = h*(1+sin(phi))/cos(phi); 

    for (i=0;i<dstNbCol;i++)
      {
        ur = (double)i-u0;
        dTheta = atan2(ur,focal);
        theta = dTheta + azim_;

        // Coordinates on image plane
        xImage = cameraBarreto.camera.u0 + radius*cos(theta);
        yImage = cameraBarreto.camera.v0 + radius*sin(theta);
                      
        // Interpolation and copy
        if (interpolation_)       
          imaData = (_channeltype)src_.getSubPixelValue<_channeltype>(xImage,yImage);
        else
          imaData = src_.getPixelValue<_channeltype>((int)xImage,(int)yImage);
        
        dst_.setPixelValue<_channeltype>(imaData,i,j);  
                    
      }   // end loop on rows
  }   // end loop on lines  
    }; // reprojectionPerspective() function

  } // namespace camera
} // namespace jafar

#endif // CAMERA_REPROJECTION_HPP