vpImage.h

/****************************************************************************
 *
 * $Id: vpImage.h 4649 2014-02-07 14:57:11Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
 * 
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional 
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Image handling.
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/


#ifndef vpImage_H
#define vpImage_H

#include <visp/vpConfig.h>
#include <visp/vpDebug.h>
#include <visp/vpException.h>
#include <visp/vpImageException.h>
#include <visp/vpImagePoint.h>
#include <visp/vpRGBa.h>

#include <fstream>
#include <iostream>
#include <math.h>
#include <string.h>

class vpDisplay;

template<class Type>
class vpImage
{
  friend class vpImageConvert;

public:
  Type *bitmap ;  
  vpDisplay *display ;

  vpImage() ;
  vpImage(const vpImage<Type>&);
  vpImage(unsigned int height, unsigned int width) ;
  vpImage(unsigned int height, unsigned int width, Type value) ;
  virtual ~vpImage() ;
  void init(unsigned int height, unsigned int width) ;
  void init(unsigned int height, unsigned int width, Type value) ;
  void resize(const unsigned int h, const unsigned int w) ;
  void destroy() ;

  inline  unsigned int getHeight() const { return height; }
  inline  unsigned int getWidth() const { return width; }

  inline  unsigned int getRows() const { return height ; }

   inline  unsigned int getCols() const { return width ; }

   inline  unsigned int getSize() const { return width*height ; }


  // Return the maximum value within the bitmap
  Type getMaxValue() const ;
  // Return the minumum value within the bitmap
  Type getMinValue() const ;
  //Look for the minumum and the maximum value within the bitmap
  void getMinMaxValue(Type &min, Type &max) const;

  // Gets the value of a pixel at a location with bilinear interpolation.
  Type getValue(double i, double j) const;
  // Gets the value of a pixel at a location with bilinear interpolation.
  Type getValue(vpImagePoint &ip) const;

  inline unsigned int getNumberOfPixel() const{ return npixels; }

  //------------------------------------------------------------------
  //         Acces to the image


  inline Type *operator[]( const unsigned int i)   { return row[i];}
  inline Type *operator[]( const int i)   { return row[i];}

  inline const  Type *operator[](unsigned int i) const { return row[i];}
  inline const  Type *operator[](int i) const { return row[i];}

  inline  Type operator()(const unsigned int i, const  unsigned int j) const
  {
    return bitmap[i*width+j] ;
  }
  inline  void  operator()(const unsigned int i, const  unsigned int j,
               const Type &v)
  {
    bitmap[i*width+j] = v ;
  }
  inline  Type operator()(const vpImagePoint &ip) const
  {
    unsigned int i = (unsigned int) ip.get_i();
    unsigned int j = (unsigned int) ip.get_j();

    return bitmap[i*width+j] ;
  }
  inline  void  operator()(const vpImagePoint &ip,
               const Type &v)
  {
    unsigned int i = (unsigned int) ip.get_i();
    unsigned int j = (unsigned int) ip.get_j();

    bitmap[i*width+j] = v ;
  }

  vpImage<Type> operator-(const vpImage<Type> &B);

  vpImage<Type>&  operator=(const vpImage<Type> &I) ;

  vpImage<Type>& operator=(const Type &v);
  bool operator==(const vpImage<Type> &I);
  bool operator!=(const vpImage<Type> &I);

  void insert(const vpImage<Type> &src, const vpImagePoint topLeft);


  // Returns a new image that's half size of the current image
  void halfSizeImage(vpImage<Type> &res);

  // Returns a new image that's a quarter size of the current image
  void quarterSizeImage(vpImage<Type> &res);

  // Returns a new image that's double size of the current image
  void doubleSizeImage(vpImage<Type> &res);

  void sub(const vpImage<Type> &B, vpImage<Type> &C);
  void sub(const vpImage<Type> &A, const vpImage<Type> &B, vpImage<Type> &C);

#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

  vp_deprecated void sub(vpImage<Type>* im2, vpImage<Type>* dst);
  // Returns a new image that's half size of the current image
  vp_deprecated void halfSizeImage(vpImage<Type>* res);
  // Returns a new image that's a quarter size of the current image
  vp_deprecated void quarterSizeImage(vpImage<Type>* res);
  // Returns a new image that's double size of the current image
  vp_deprecated void doubleSizeImage(vpImage<Type>* res);
#endif
private:
  unsigned int npixels ; //<! number of pixel in the image
  unsigned int width ;   //<! number of columns
  unsigned int height ;   //<! number of rows
  Type **row ;    
  } ;


template<class Type>
void
vpImage<Type>::init(unsigned int h, unsigned int w, Type value)
{
  try
  {
    init(h,w) ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE(" ") ;
    throw ;
  }

  for (unsigned int i=0  ; i < npixels ;  i++)
    bitmap[i] = value ;
}


template<class Type>
void
vpImage<Type>::init(unsigned int h, unsigned int w)
{

  if (h != this->height) {
    if (row != NULL)  {
      vpDEBUG_TRACE(10,"Destruction row[]");
      delete [] row;
      row = NULL;
    }
  }

  if ((h != this->height) || (w != this->width))
  {
    if (bitmap != NULL) {
      vpDEBUG_TRACE(10,"Destruction bitmap[]") ;
      delete [] bitmap;
      bitmap = NULL;
    }
  }

  this->width = w ;
  this->height = h;

  npixels=width*height;

  if (bitmap == NULL)  bitmap = new  Type[npixels] ;

  //  vpERROR_TRACE("Allocate bitmap %p",bitmap) ;
  if (bitmap == NULL)
  {
        vpERROR_TRACE("cannot allocate bitmap ") ;
    throw(vpException(vpException::memoryAllocationError,
              "cannot allocate bitmap ")) ;
  }

  if (row == NULL)  row = new  Type*[height] ;
//  vpERROR_TRACE("Allocate row %p",row) ;
  if (row == NULL)
  {
    vpERROR_TRACE("cannot allocate row ") ;
    throw(vpException(vpException::memoryAllocationError,
              "cannot allocate row ")) ;
  }

  unsigned int i ;
  for ( i =0  ; i < height ; i++)
    row[i] = bitmap + i*width ;
}

template<class Type>
vpImage<Type>::vpImage(unsigned int h, unsigned int w)
  : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
{
  try
  {
    init(h,w,0) ;
  }
  catch(...)
  {
    throw ;
  }
}

template<class Type>
vpImage<Type>::vpImage (unsigned int h, unsigned int w, Type value)
  : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
{
  try
  {
    init(h,w,value) ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE(" ") ;
    throw ;
  }
}

template<class Type>
vpImage<Type>::vpImage()
  : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
{
}

template<class Type>
void
vpImage<Type>::resize(unsigned int h, unsigned int w)
{
  try
  {
    init(h, w) ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE(" ") ;
    throw ;
  }
}

template<class Type>
void
vpImage<Type>::destroy()
{
 //   vpERROR_TRACE("Deallocate ") ;


  if (bitmap!=NULL)
  {
  //  vpERROR_TRACE("Deallocate bitmap memory %p",bitmap) ;
//    vpDEBUG_TRACE(20,"Deallocate bitmap memory %p",bitmap) ;
    delete [] bitmap ;
    bitmap = NULL;
  }


  if (row!=NULL)
  {
 //   vpERROR_TRACE("Deallocate row memory %p",row) ;
//    vpDEBUG_TRACE(20,"Deallocate row memory %p",row) ;
    delete [] row ;
    row = NULL;
  }

}

template<class Type>
vpImage<Type>::~vpImage()
{
  destroy() ;
}



template<class Type>
vpImage<Type>::vpImage(const vpImage<Type>& I)
  : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
{
  try
  {
    resize(I.getHeight(),I.getWidth());
    memcpy(bitmap, I.bitmap, I.npixels*sizeof(Type)) ;
    for (unsigned int i =0  ; i < this->height ; i++) row[i] = bitmap + i*this->width ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE(" ") ;
    throw ;
  }
}

template<class Type>
Type vpImage<Type>::getMaxValue() const
{
  Type m = bitmap[0] ;
  for (unsigned int i=0 ; i < npixels ; i++)
  {
    if (bitmap[i]>m) m = bitmap[i] ;
  }
  return m ;
}

template<class Type>
Type vpImage<Type>::getMinValue() const
{
  Type m =  bitmap[0];
  for (unsigned int i=0 ; i < npixels ; i++)
    if (bitmap[i]<m) m = bitmap[i] ;
  return m ;
}


template<class Type>
void vpImage<Type>::getMinMaxValue(Type &min, Type &max) const
{
  min = max =  bitmap[0];
  for (unsigned int i=0 ; i < npixels ; i++)
  {
    if (bitmap[i]<min) min = bitmap[i] ;
    if (bitmap[i]>max) max = bitmap[i] ;
  }
}

template<class Type>
vpImage<Type> & vpImage<Type>::operator=(const vpImage<Type> &I)
{
    /* we first have to set the initial values of the image because resize function calls init function that test the actual size of the image */
  if(bitmap != NULL){
    delete[] bitmap;
    bitmap = NULL ;
  }
  
  if(row != NULL){
    delete[] row;
    row = NULL ;
  }
  this->width = I.width;
  this->height = I.height;
  this->npixels = I.npixels;
  try
  {
    if(I.npixels != 0)
    {
      if (bitmap == NULL){  
        bitmap = new  Type[npixels] ;
      }

      if (bitmap == NULL){
            vpERROR_TRACE("cannot allocate bitmap ") ;
        throw(vpException(vpException::memoryAllocationError,
                  "cannot allocate bitmap ")) ;
      }

      if (row == NULL){  
        row = new  Type*[height] ;
      }
      if (row == NULL){
        vpERROR_TRACE("cannot allocate row ") ;
        throw(vpException(vpException::memoryAllocationError,
                  "cannot allocate row ")) ;
      }      

      memcpy(bitmap, I.bitmap, I.npixels*sizeof(Type)) ;

      for (unsigned int i=0; i<this->height; i++){ 
        row[i] = bitmap + i*this->width;
      }
    }
  }
  catch(vpException me)
  {
    vpERROR_TRACE(" ") ;
    throw ;
  }
  return (* this);
}


template<class Type>
vpImage<Type>& vpImage<Type>::operator=(const Type &v)
{
  for (unsigned int i=0 ; i < npixels ; i++)
    bitmap[i] = v ;

  return *this;
}

template<class Type>
bool vpImage<Type>::operator==(const vpImage<Type> &I)
{
    if (this->width != I.getWidth())
        return false;
    if (this->height != I.getHeight())
        return false;
        
  for (unsigned int i=0 ; i < npixels ; i++) 
  {
    if (bitmap[i] != I.bitmap[i])
        return false;
  }
  return true ;
}
template<class Type>
bool vpImage<Type>::operator!=(const vpImage<Type> &I)
{
  if (this->width != I.getWidth())
    return true;
  if (this->height != I.getHeight())
    return true;

  for (unsigned int i=0 ; i < npixels ; i++)
  {
    if (bitmap[i] == I.bitmap[i])
      return false;
  }
  return true ;
}

template<class Type>
vpImage<Type> vpImage<Type>::operator-(const vpImage<Type> &B)
{
  vpImage<Type> C;
  sub(*this,B,C);
  return C;
}

template<class Type>
void vpImage<Type>::insert(const vpImage<Type> &src,
            const vpImagePoint topLeft)
{
  Type* srcBitmap;
  Type* destBitmap;
  
  int itl = (int)topLeft.get_i();
  int jtl = (int)topLeft.get_j();
  
  int dest_ibegin = 0;
  int dest_jbegin = 0;
  int src_ibegin = 0;
  int src_jbegin = 0;
  int dest_w = this->getWidth();
  int dest_h = this->getHeight();
  int src_w = src.getWidth();
  int src_h = src.getHeight();
  int wsize = src.getWidth();
  int hsize = src.getHeight();
  
  if (itl >= dest_h || jtl >= dest_w)
    return;
  
  if (itl < 0)
    src_ibegin = -itl;  
  else
    dest_ibegin = itl;
  
  if (jtl < 0)
    src_jbegin = -jtl;
  else
    dest_jbegin = jtl;
  
  if (src_w - src_jbegin > dest_w - dest_jbegin)
    wsize = dest_w - dest_jbegin;
  else
    wsize = src_w - src_jbegin;
  
  if (src_h - src_ibegin > dest_h - dest_ibegin)
    hsize = dest_h - dest_ibegin;
  else
    hsize = src_h - src_ibegin;
  
  for (int i = 0; i < hsize; i++)
  {
    srcBitmap = src.bitmap + ((src_ibegin+i)*src_w+src_jbegin);
    destBitmap = this->bitmap + ((dest_ibegin+i)*dest_w+dest_jbegin);
  
    memcpy(destBitmap,srcBitmap,wsize*sizeof(Type));
  }
}

template<class Type>
void
vpImage<Type>::halfSizeImage(vpImage<Type> &res)
{
  unsigned int h = height/2;
  unsigned int w = width/2;
  res.resize(h, w);
  for(unsigned int i = 0; i < h; i++)
    for(unsigned int j = 0; j < w; j++)
      res[i][j] = (*this)[i<<1][j<<1];
}

template<class Type>
void
vpImage<Type>::quarterSizeImage(vpImage<Type> &res)
{
  unsigned int h = height/4;
  unsigned int w = width/4;
  res.resize(h, w);
  for(unsigned int i = 0; i < h; i++)
    for(unsigned int j = 0; j < w; j++)
      res[i][j] = (*this)[i<<2][j<<2];
}

template<class Type>
void
vpImage<Type>::doubleSizeImage(vpImage<Type> &res)
{
  int h = height*2;
  int w = width*2;

  res.resize(h, w);

  for(int i = 0; i < h; i++)
    for(int j = 0; j < w; j++)
      res[i][j] = (*this)[i>>1][j>>1];

  /*
    A B C
    E F G
    H I J
    A C H J are pixels from original image
    B E G I are interpolated pixels
  */

  //interpolate pixels B and I
  for(int i = 0; i < h; i += 2)
    for(int j = 1; j < w - 1; j += 2)
      res[i][j] = (Type)(0.5 * ((*this)[i>>1][j>>1] 
                + (*this)[i>>1][(j>>1) + 1]));

  //interpolate pixels E and G
  for(int i = 1; i < h - 1; i += 2)
    for(int j = 0; j < w; j += 2)
      res[i][j] = (Type)(0.5 * ((*this)[i>>1][j>>1] 
                + (*this)[(i>>1)+1][j>>1]));

  //interpolate pixel F
  for(int i = 1; i < h - 1; i += 2)
    for(int j = 1; j < w - 1; j += 2)
      res[i][j] = (Type)(0.25 * ((*this)[i>>1][j>>1] 
                 + (*this)[i>>1][(j>>1)+1] 
                 + (*this)[(i>>1)+1][j>>1] 
                 + (*this)[(i>>1)+1][(j>>1)+1]));
}

template<class Type>
Type vpImage<Type>::getValue(double /* i */, double /* j */) const
{
  vpTRACE("Not implemented");
}

template<>
inline unsigned char vpImage<unsigned char>::getValue(double i, double j) const
{
  unsigned int iround, jround;
  double rfrac, cfrac;

  iround = (unsigned int)floor(i);
  jround = (unsigned int)floor(j);

  if (iround >= height || jround >= width) {
    vpERROR_TRACE("Pixel outside the image") ;
    throw(vpException(vpImageException::notInTheImage,
              "Pixel outside the image"));
  }

  if (i > height - 1)
    i = (double)(height - 1);

  if (j > width - 1)
    j = (double)(width - 1);

  double rratio = i - (double) iround;
  if(rratio < 0)
    rratio=-rratio;
  double cratio = j - (double) jround;
  if(cratio < 0) 
    cratio=-cratio;

  rfrac = 1.0f - rratio;
  cfrac = 1.0f - cratio;


  double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
             + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
  return (unsigned char)vpMath::round(value);
}


template<>
inline double vpImage<double>::getValue(double i, double j) const
{
  unsigned int iround, jround;
  double rfrac, cfrac;

  iround = (unsigned int)floor(i);
  jround = (unsigned int)floor(j);

  if (iround >= height || jround >= width) {
    vpERROR_TRACE("Pixel outside the image") ;
    throw(vpException(vpImageException::notInTheImage,
              "Pixel outside the image"));
  }

  if (i > height - 1)
    i = (double)(height - 1);

  if (j > width - 1)
    j = (double)(width - 1);

  double rratio = i - (double) iround;
  if(rratio < 0)
    rratio=-rratio;
  double cratio = j - (double) jround;
  if(cratio < 0) 
    cratio=-cratio;

  rfrac = 1.0f - rratio;
  cfrac = 1.0f - cratio;


  double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
             + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
  return value;
}

template<>
inline vpRGBa vpImage<vpRGBa>::getValue(double i, double j) const
{
  unsigned int iround, jround;
  double rfrac, cfrac;

  iround = (unsigned int)floor(i);
  jround = (unsigned int)floor(j);

  if (iround >= height || jround >= width) {
    vpERROR_TRACE("Pixel outside the image") ;
    throw(vpException(vpImageException::notInTheImage,
              "Pixel outside the image"));
  }

  if (i > height - 1)
    i = (double)(height - 1);

  if (j > width - 1)
    j = (double)(width - 1);

  double rratio = i - (double) iround;
  if(rratio < 0)
    rratio=-rratio;
  double cratio = j - (double) jround;
  if(cratio < 0) 
    cratio=-cratio;

  rfrac = 1.0f - rratio;
  cfrac = 1.0f - cratio;

  double valueR = ((double)row[iround][jround].R * rfrac + (double)row[iround+1][jround].R * rratio)*cfrac
             + ((double)row[iround][jround+1].R * rfrac + (double)row[iround+1][jround+1].R * rratio)*cratio;
  double valueG = ((double)row[iround][jround].G * rfrac + (double)row[iround+1][jround].G * rratio)*cfrac
             + ((double)row[iround][jround+1].G* rfrac + (double)row[iround+1][jround+1].G * rratio)*cratio;
  double valueB = ((double)row[iround][jround].B * rfrac + (double)row[iround+1][jround].B * rratio)*cfrac
             + ((double)row[iround][jround+1].B*rfrac + (double)row[iround+1][jround+1].B * rratio)*cratio;
  return vpRGBa((unsigned char)vpMath::round(valueR),(unsigned char)vpMath::round(valueG),(unsigned char)vpMath::round(valueB));
}

template<class Type>
Type vpImage<Type>::getValue(vpImagePoint & /* ip */) const
{
  vpTRACE("Not implemented");
}

template<>
inline unsigned char vpImage<unsigned char>::getValue(vpImagePoint &ip) const
{
  unsigned int iround, jround;
  double rfrac, cfrac;

  iround = (unsigned int)floor(ip.get_i());
  jround = (unsigned int)floor(ip.get_j());

  if (iround >= height || jround >= width) {
    vpERROR_TRACE("Pixel outside the image") ;
    throw(vpException(vpImageException::notInTheImage,
              "Pixel outside the image"));
  }

  if (ip.get_i() > height - 1)
    ip.set_i((double)(height - 1));

  if (ip.get_j() > width - 1)
    ip.set_j((double)(width - 1));

  double rratio = ip.get_i() - (double) iround;
  if(rratio < 0)
    rratio=-rratio;
  double cratio = ip.get_j() - (double) jround;
  if(cratio < 0) 
    cratio=-cratio;

  rfrac = 1.0f - rratio;
  cfrac = 1.0f - cratio;


  double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
             + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
  return (unsigned char)vpMath::round(value);
}


template<>
inline double vpImage<double>::getValue(vpImagePoint &ip) const
{
  unsigned int iround, jround;
  double rfrac, cfrac;

  iround = (unsigned int)floor(ip.get_i());
  jround = (unsigned int)floor(ip.get_j());

  if (iround >= height || jround >= width) {
    vpERROR_TRACE("Pixel outside the image") ;
    throw(vpException(vpImageException::notInTheImage,
              "Pixel outside the image"));
  }

  if (ip.get_i() > height - 1)
    ip.set_i((double)(height - 1));

  if (ip.get_j() > width - 1)
    ip.set_j((double)(width - 1));

  double rratio = ip.get_i() - (double) iround;
  if(rratio < 0)
    rratio=-rratio;
  double cratio = ip.get_j() - (double) jround;
  if(cratio < 0) 
    cratio=-cratio;

  rfrac = 1.0f - rratio;
  cfrac = 1.0f - cratio;


  double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
             + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
  return value;
}

template<>
inline vpRGBa vpImage<vpRGBa>::getValue(vpImagePoint &ip) const
{
  unsigned int iround, jround;
  double rfrac, cfrac;

  iround = (unsigned int)floor(ip.get_i());
  jround = (unsigned int)floor(ip.get_j());

  if (iround >= height || jround >= width) {
    vpERROR_TRACE("Pixel outside the image") ;
    throw(vpException(vpImageException::notInTheImage,
              "Pixel outside the image"));
  }

  if (ip.get_i() > height - 1)
    ip.set_i((double)(height - 1));

  if (ip.get_j() > width - 1)
    ip.set_j((double)(width - 1));

  double rratio = ip.get_i() - (double) iround;
  if(rratio < 0)
    rratio=-rratio;
  double cratio = ip.get_j() - (double) jround;
  if(cratio < 0) 
    cratio=-cratio;

  rfrac = 1.0f - rratio;
  cfrac = 1.0f - cratio;

  double valueR = ((double)row[iround][jround].R * rfrac + (double)row[iround+1][jround].R * rratio)*cfrac
             + ((double)row[iround][jround+1].R * rfrac + (double)row[iround+1][jround+1].R * rratio)*cratio;
  double valueG = ((double)row[iround][jround].G * rfrac + (double)row[iround+1][jround].G * rratio)*cfrac
             + ((double)row[iround][jround+1].G* rfrac + (double)row[iround+1][jround+1].G * rratio)*cratio;
  double valueB = ((double)row[iround][jround].B * rfrac + (double)row[iround+1][jround].B * rratio)*cfrac
             + ((double)row[iround][jround+1].B*rfrac + (double)row[iround+1][jround+1].B * rratio)*cratio;
  return vpRGBa((unsigned char)vpMath::round(valueR),(unsigned char)vpMath::round(valueG),(unsigned char)vpMath::round(valueB));
}

template<class Type>
void vpImage<Type>::sub(const vpImage<Type> &B, vpImage<Type> &C)
{

  try
  {
    if ((this->getHeight() != C.getHeight())
      || (this->getWidth() != C.getWidth()))
      C.resize(this->getHeight(), this->getWidth());
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    std::cout << me << std::endl ;
    throw ;
  }

  if ( (this->getWidth() != B.getWidth())||(this->getHeight() != B.getHeight()))
  {
    vpERROR_TRACE("\n\t\t vpImage mismatch in vpImage/vpImage substraction") ;
    throw(vpException(vpException::memoryAllocationError,
          "vpImage mismatch in vpImage/vpImage substraction ")) ;
  }

  for (unsigned int i=0;i<this->getWidth()*this->getHeight();i++)
  {
    *(C.bitmap + i) = *(bitmap + i) - *(B.bitmap + i) ;
  }
}

template<class Type>
void vpImage<Type>::sub(const vpImage<Type> &A, const vpImage<Type> &B,
                        vpImage<Type> &C)
{

  try
  {
    if ((A.getHeight() != C.getHeight())
      || (A.getWidth() != C.getWidth()))
      C.resize(A.getHeight(), A.getWidth());
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    std::cout << me << std::endl ;
    throw ;
  }

  if ( (A.getWidth() != B.getWidth())||(A.getHeight() != B.getHeight()))
  {
    vpERROR_TRACE("\n\t\t vpImage mismatch in vpImage/vpImage substraction") ;
    throw(vpException(vpException::memoryAllocationError,
                      "vpImage mismatch in vpImage/vpImage substraction ")) ;
  }

  for (unsigned int i=0;i<A.getWidth()*A.getHeight();i++)
  {
    *(C.bitmap + i) = *(A.bitmap + i) - *(B.bitmap + i) ;
  }
}
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

template<class Type>
void
vpImage<Type>::sub(vpImage<Type>* B, vpImage<Type>* C)
{
  if (B == NULL || C == NULL) {
    throw(vpException(vpException::memoryAllocationError,
                      "Images are not allocated in vpImage<>::sub()")) ;
  }
  if ( (this->getWidth() != B->getWidth())
    || (this->getHeight() != B->getHeight())
    || (this->getWidth() != C->getWidth())
    || (this->getHeight() != C->getHeight()))
  {
    vpERROR_TRACE("\n\t\t vpImage mismatch in vpImage/vpImage substraction") ;
    throw(vpException(vpException::memoryAllocationError,
          "vpImage mismatch in vpImage/vpImage substraction ")) ;
  }

  for(unsigned int i  = 0; i < height * width; i++)
    C->bitmap[i] = this->bitmap[i] - B->bitmap[i];
}

template<class Type>
void
vpImage<Type>::halfSizeImage(vpImage<Type>* res)
{
  unsigned int r = height/2;
  unsigned int c = width/2;
  if(res == NULL) {
    throw(vpException(vpException::memoryAllocationError,
                      "Images are not allocated in vpImage<>::sub()")) ;
  }
  if((res->getWidth() != c) || (res->getHeight()!= r))
    res->resize(r,c);
  for(unsigned int y = 0; y < r; y++)
    for(unsigned int x = 0; x < c; x++)
      (*res)[y][x] = (*this)[y*2][x*2];
}

template<class Type>
void
vpImage<Type>::quarterSizeImage(vpImage<Type>* res)
{
  unsigned int r = height/4;
  unsigned int c = width/4;
  if(res == NULL) {
    throw(vpException(vpException::memoryAllocationError,
                      "Images are not allocated in vpImage<>::sub()")) ;
  }
  if((res->getWidth() != c) || (res->getHeight()!= r))
    res->resize(r,c);
  for(unsigned int y = 0; y < r; y++)
    for(unsigned int x = 0; x < c; x++)
      (*res)[y][x] = (*this)[y*4][x*4];
}

template<class Type>
void
vpImage<Type>::doubleSizeImage(vpImage<Type>* res)
{
  int h = height*2;
  int w = width*2;

  if(res == NULL) {
    throw(vpException(vpException::memoryAllocationError,
                      "Images are not allocated in vpImage<>::sub()")) ;
  }
  if((res->getWidth() != w) || (res->getHeight()!= h))
    res->resize(h, w);

  for(int j = 0; j < h; j++)
    for(int i = 0; i < w; i++)
      (*res)[j][i] = (*this)[j/2][i/2];

  /*
    A B C
    E F G
    H I J
    A C H J are pixels from original image
    B E G I are interpolated pixels
  */

  //interpolate pixels B and I
  for(int j = 0; j < h; j += 2)
    for(int i = 1; i < w - 1; i += 2)
      (*res)[j][i] = (Type)(0.5 * ((*this)[j/2][i/2] + (*this)[j/2][i/2 + 1]));

  //interpolate pixels E and G
  for(int j = 1; j < h - 1; j += 2)
    for(int i = 0; i < w; i += 2)
      (*res)[j][i] = (Type)(0.5 * ((*this)[j/2][i/2] + (*this)[j/2+1][i/2]));

  //interpolate pixel F
  for(int j = 1; j < h - 1; j += 2)
    for(int i = 1; i < w - 1; i += 2)
      (*res)[j][i] = (Type)(0.25 * ((*this)[j/2][i/2] + (*this)[j/2][i/2+1] +
           (*this)[j/2+1][i/2] + (*this)[j/2+1][i/2+1]));
}

#endif // VISP_BUILD_DEPRECATED_FUNCTIONS

#endif


/*
 * Local variables:
 * c-basic-offset: 2
 * End:
 */