vpImage.h

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 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://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * 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 <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpImageException.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpRGBa.h>
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
#include <visp3/core/vpThread.h>
#endif

#include <fstream>
#include <iomanip> // std::setw
#include <iostream>
#include <math.h>
#include <string.h>

class vpDisplay;

// Ref: http://en.cppreference.com/w/cpp/language/friend#Template_friends
template <class Type> class vpImage; // forward declare to make function declaration possible

// declarations
template <class Type> std::ostream &operator<<(std::ostream &, const vpImage<Type> &);

std::ostream &operator<<(std::ostream &, const vpImage<unsigned char> &);
std::ostream &operator<<(std::ostream &, const vpImage<char> &);
std::ostream &operator<<(std::ostream &, const vpImage<float> &);
std::ostream &operator<<(std::ostream &, const vpImage<double> &);

template <class Type> void swap(vpImage<Type> &first, vpImage<Type> &second);

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

public:
  Type *bitmap; 
  vpDisplay *display;

  vpImage();
  vpImage(const vpImage<Type> &);
#ifdef VISP_HAVE_CPP11_COMPATIBILITY
  vpImage(vpImage<Type> &&);
#endif
  vpImage(unsigned int height, unsigned int width);
  vpImage(unsigned int height, unsigned int width, Type value);
  vpImage(Type *const array, const unsigned int height, const unsigned int width, const bool copyData = false);
  virtual ~vpImage();


  // destructor
  void destroy();

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

  inline unsigned int getCols() const { return width; }
  inline unsigned int getHeight() const { return 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;

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

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

  // 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 getWidth() const { return width; }

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

  void init(unsigned int height, unsigned int width);
  void init(unsigned int height, unsigned int width, Type value);
  void init(Type *const array, const unsigned int height, const unsigned int width, const bool copyData = false);
  void insert(const vpImage<Type> &src, const vpImagePoint &topLeft);

  //------------------------------------------------------------------
  //         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=(vpImage<Type> other);

  vpImage<Type> &operator=(const Type &v);
  bool operator==(const vpImage<Type> &I);
  bool operator!=(const vpImage<Type> &I);
  friend std::ostream &operator<<<>(std::ostream &s, const vpImage<Type> &I);
  friend std::ostream &operator<<(std::ostream &s, const vpImage<unsigned char> &I);
  friend std::ostream &operator<<(std::ostream &s, const vpImage<char> &I);
  friend std::ostream &operator<<(std::ostream &s, const vpImage<float> &I);
  friend std::ostream &operator<<(std::ostream &s, const vpImage<double> &I);

  // Perform a look-up table transformation
  void performLut(const Type (&lut)[256], const unsigned int nbThreads = 1);

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

  // set the size of the image without initializing it.
  void resize(const unsigned int h, const unsigned int w);
  // set the size of the image and initialize it.
  void resize(const unsigned int h, const unsigned int w, const Type &val);

  void sub(const vpImage<Type> &B, vpImage<Type> &C);
  void sub(const vpImage<Type> &A, const vpImage<Type> &B, vpImage<Type> &C);
  void subsample(unsigned int v_scale, unsigned int h_scale, vpImage<Type> &sampled) const;

  friend void swap<>(vpImage<Type> &first, vpImage<Type> &second);


private:
  unsigned int npixels; 
  unsigned int width;   
  unsigned int height;  
  Type **row;           
};

template <class Type> std::ostream &operator<<(std::ostream &s, const vpImage<Type> &I)
{
  if (I.bitmap == NULL) {
    return s;
  }

  for (unsigned int i = 0; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < I.getWidth() - 1; j++) {
      s << I[i][j] << " ";
    }

    // We don't add "  " after the last column element
    s << I[i][I.getWidth() - 1];

    // We don't add a \n character at the end of the last row line
    if (i < I.getHeight() - 1) {
      s << std::endl;
    }
  }

  return s;
}

inline std::ostream &operator<<(std::ostream &s, const vpImage<unsigned char> &I)
{
  if (I.bitmap == NULL) {
    return s;
  }

  std::ios_base::fmtflags original_flags = s.flags();

  for (unsigned int i = 0; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < I.getWidth() - 1; j++) {
      s << std::setw(3) << static_cast<unsigned>(I[i][j]) << " ";
    }

    // We don't add "  " after the last column element
    s << std::setw(3) << static_cast<unsigned>(I[i][I.getWidth() - 1]);

    // We don't add a \n character at the end of the last row line
    if (i < I.getHeight() - 1) {
      s << std::endl;
    }
  }

  s.flags(original_flags); // restore s to standard state
  return s;
}

inline std::ostream &operator<<(std::ostream &s, const vpImage<char> &I)
{
  if (I.bitmap == NULL) {
    return s;
  }

  std::ios_base::fmtflags original_flags = s.flags();

  for (unsigned int i = 0; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < I.getWidth() - 1; j++) {
      s << std::setw(4) << static_cast<int>(I[i][j]) << " ";
    }

    // We don't add "  " after the last column element
    s << std::setw(4) << static_cast<int>(I[i][I.getWidth() - 1]);

    // We don't add a \n character at the end of the last row line
    if (i < I.getHeight() - 1) {
      s << std::endl;
    }
  }

  s.flags(original_flags); // restore s to standard state
  return s;
}

inline std::ostream &operator<<(std::ostream &s, const vpImage<float> &I)
{
  if (I.bitmap == NULL) {
    return s;
  }

  std::ios_base::fmtflags original_flags = s.flags();
  s.precision(9); // http://en.cppreference.com/w/cpp/types/numeric_limits/max_digits10

  for (unsigned int i = 0; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < I.getWidth() - 1; j++) {
      s << I[i][j] << " ";
    }

    // We don't add "  " after the last column element
    s << I[i][I.getWidth() - 1];

    // We don't add a \n character at the end of the last row line
    if (i < I.getHeight() - 1) {
      s << std::endl;
    }
  }

  s.flags(original_flags); // restore s to standard state
  return s;
}

inline std::ostream &operator<<(std::ostream &s, const vpImage<double> &I)
{
  if (I.bitmap == NULL) {
    return s;
  }

  std::ios_base::fmtflags original_flags = s.flags();
  s.precision(17); // http://en.cppreference.com/w/cpp/types/numeric_limits/max_digits10

  for (unsigned int i = 0; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < I.getWidth() - 1; j++) {
      s << I[i][j] << " ";
    }

    // We don't add "  " after the last column element
    s << I[i][I.getWidth() - 1];

    // We don't add a \n character at the end of the last row line
    if (i < I.getHeight() - 1) {
      s << std::endl;
    }
  }

  s.flags(original_flags); // restore s to standard state
  return s;
}

#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
namespace
{
struct ImageLut_Param_t {
  unsigned int m_start_index;
  unsigned int m_end_index;

  unsigned char m_lut[256];
  unsigned char *m_bitmap;

  ImageLut_Param_t() : m_start_index(0), m_end_index(0), m_lut(), m_bitmap(NULL) {}

  ImageLut_Param_t(const unsigned int start_index, const unsigned int end_index, unsigned char *bitmap)
    : m_start_index(start_index), m_end_index(end_index), m_lut(), m_bitmap(bitmap)
  {
  }
};

vpThread::Return performLutThread(vpThread::Args args)
{
  ImageLut_Param_t *imageLut_param = static_cast<ImageLut_Param_t *>(args);
  unsigned int start_index = imageLut_param->m_start_index;
  unsigned int end_index = imageLut_param->m_end_index;

  unsigned char *bitmap = imageLut_param->m_bitmap;

  unsigned char *ptrStart = bitmap + start_index;
  unsigned char *ptrEnd = bitmap + end_index;
  unsigned char *ptrCurrent = ptrStart;

  //    while(ptrCurrent != ptrEnd) {
  //      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
  //      ++ptrCurrent;
  //    }

  if (end_index - start_index >= 8) {
    // Unroll loop version
    for (; ptrCurrent <= ptrEnd - 8;) {
      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
      ++ptrCurrent;

      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
      ++ptrCurrent;

      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
      ++ptrCurrent;

      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
      ++ptrCurrent;

      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
      ++ptrCurrent;

      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
      ++ptrCurrent;

      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
      ++ptrCurrent;

      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
      ++ptrCurrent;
    }
  }

  for (; ptrCurrent != ptrEnd; ++ptrCurrent) {
    *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
  }

  return 0;
}

struct ImageLutRGBa_Param_t {
  unsigned int m_start_index;
  unsigned int m_end_index;

  vpRGBa m_lut[256];
  unsigned char *m_bitmap;

  ImageLutRGBa_Param_t() : m_start_index(0), m_end_index(0), m_lut(), m_bitmap(NULL) {}

  ImageLutRGBa_Param_t(const unsigned int start_index, const unsigned int end_index, unsigned char *bitmap)
    : m_start_index(start_index), m_end_index(end_index), m_lut(), m_bitmap(bitmap)
  {
  }
};

vpThread::Return performLutRGBaThread(vpThread::Args args)
{
  ImageLutRGBa_Param_t *imageLut_param = static_cast<ImageLutRGBa_Param_t *>(args);
  unsigned int start_index = imageLut_param->m_start_index;
  unsigned int end_index = imageLut_param->m_end_index;

  unsigned char *bitmap = imageLut_param->m_bitmap;

  unsigned char *ptrStart = bitmap + start_index * 4;
  unsigned char *ptrEnd = bitmap + end_index * 4;
  unsigned char *ptrCurrent = ptrStart;

  if (end_index - start_index >= 4 * 2) {
    // Unroll loop version
    for (; ptrCurrent <= ptrEnd - 4 * 2;) {
      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].R;
      ptrCurrent++;
      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].G;
      ptrCurrent++;
      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].B;
      ptrCurrent++;
      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].A;
      ptrCurrent++;

      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].R;
      ptrCurrent++;
      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].G;
      ptrCurrent++;
      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].B;
      ptrCurrent++;
      *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].A;
      ptrCurrent++;
    }
  }

  while (ptrCurrent != ptrEnd) {
    *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].R;
    ptrCurrent++;

    *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].G;
    ptrCurrent++;

    *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].B;
    ptrCurrent++;

    *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].A;
    ptrCurrent++;
  }

  return 0;
}
}
#endif

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

  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];

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

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

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

template <class Type>
void vpImage<Type>::init(Type *const array, const unsigned int h, const unsigned int w, const bool copyData)
{
  if (h != this->height) {
    if (row != NULL) {
      delete[] row;
      row = NULL;
    }
  }

  // Delete bitmap if copyData==false, otherwise only if the dimension differs
  if ((copyData && ((h != this->height) || (w != this->width))) || !copyData) {
    if (bitmap != NULL) {
      delete[] bitmap;
      bitmap = NULL;
    }
  }

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

  npixels = width * height;

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

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

    // Copy the image data
    memcpy(bitmap, array, (size_t)(npixels * sizeof(Type)));
  } else {
    // Copy the address of the array in the bitmap
    bitmap = array;
  }

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

  for (unsigned int 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)
{
  init(h, w, 0);
}

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)
{
  init(h, w, value);
}

template <class Type>
vpImage<Type>::vpImage(Type *const array, const unsigned int h, const unsigned int w, const bool copyData)
  : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
{
  init(array, h, w, copyData);
}

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) { init(h, w); }

template <class Type> void vpImage<Type>::resize(unsigned int h, unsigned int w, const Type &val) { init(h, w, val); }

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)
{
  resize(I.getHeight(), I.getWidth());
  memcpy(bitmap, I.bitmap, I.npixels * sizeof(Type));
}

#ifdef VISP_HAVE_CPP11_COMPATIBILITY

template <class Type>
vpImage<Type>::vpImage(vpImage<Type> &&I)
  : bitmap(I.bitmap), display(I.display), npixels(I.npixels), width(I.width), height(I.height), row(I.row)
{
  I.bitmap = NULL;
  I.display = NULL;
  I.npixels = 0;
  I.width = 0;
  I.height = 0;
  I.row = NULL;
}
#endif

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=(vpImage<Type> other)
{
  swap(*this, other);
  // Swap back display pointer if it was not null
  // vpImage<unsigned char> I2(480, 640);
  // vpDisplayX d(I2);
  // I2 = I1; //copy only the data
  if (other.display != NULL)
    display = other.display;

  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;

  //  printf("wxh: %dx%d bitmap: %p I.bitmap %p\n", width, height, bitmap,
  //  I.bitmap);
  for (unsigned int i = 0; i < npixels; i++) {
    if (bitmap[i] != I.bitmap[i]) {
      //      std::cout << "differ for pixel " << i << " (" << i%this->height
      //      << ", " << i - i%this->height << ")" << std::endl;
      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 true;
  //  }
  //  return false;
  return !(*this == I);
}

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)
{
  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 = (int)this->getWidth();
  int dest_h = (int)this->getHeight();
  int src_w = (int)src.getWidth();
  int src_h = (int)src.getHeight();
  int wsize = (int)src.getWidth();
  int hsize = (int)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++) {
    Type *srcBitmap = src.bitmap + ((src_ibegin + i) * src_w + src_jbegin);
    Type *destBitmap = this->bitmap + ((dest_ibegin + i) * dest_w + dest_jbegin);

    memcpy(destBitmap, srcBitmap, (size_t)wsize * sizeof(Type));
  }
}

template <class Type> void vpImage<Type>::halfSizeImage(vpImage<Type> &res) const
{
  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>::subsample(unsigned int v_scale, unsigned int h_scale, vpImage<Type> &sampled) const
{
  unsigned int h = height / v_scale;
  unsigned int w = width / h_scale;
  sampled.resize(h, w);
  for (unsigned int i = 0; i < h; i++)
    for (unsigned int j = 0; j < w; j++)
      sampled[i][j] = (*this)[i * v_scale][j * h_scale];
}

template <class Type> void vpImage<Type>::quarterSizeImage(vpImage<Type> &res) const
{
  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
{
  unsigned int iround, jround;
  double rfrac, cfrac;

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

  if (iround >= height || jround >= width) {
    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 (Type)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) {
    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) {
    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> inline Type vpImage<Type>::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) {
    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 (Type)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) {
    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) {
    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) {
    std::cout << me << std::endl;
    throw;
  }

  if ((this->getWidth() != B.getWidth()) || (this->getHeight() != B.getHeight())) {
    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) {
    std::cout << me << std::endl;
    throw;
  }

  if ((A.getWidth() != B.getWidth()) || (A.getHeight() != B.getHeight())) {
    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);
  }
}

template <class Type> void vpImage<Type>::performLut(const Type (&)[256], const unsigned int)
{
  std::cerr << "Not implemented !" << std::endl;
}

template <>
inline void vpImage<unsigned char>::performLut(const unsigned char (&lut)[256], const unsigned int nbThreads)
{
  unsigned int size = getWidth() * getHeight();
  unsigned char *ptrStart = (unsigned char *)bitmap;
  unsigned char *ptrEnd = ptrStart + size;
  unsigned char *ptrCurrent = ptrStart;

  bool use_single_thread = (nbThreads == 0 || nbThreads == 1);
#if !defined(VISP_HAVE_PTHREAD) && !defined(_WIN32)
  use_single_thread = true;
#endif

  if (!use_single_thread && getSize() <= nbThreads) {
    use_single_thread = true;
  }

  if (use_single_thread) {
    // Single thread

    while (ptrCurrent != ptrEnd) {
      *ptrCurrent = lut[*ptrCurrent];
      ++ptrCurrent;
    }
  } else {
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
    // Multi-threads

    std::vector<vpThread *> threadpool;
    std::vector<ImageLut_Param_t *> imageLutParams;

    unsigned int image_size = getSize();
    unsigned int step = image_size / nbThreads;
    unsigned int last_step = image_size - step * (nbThreads - 1);

    for (unsigned int index = 0; index < nbThreads; index++) {
      unsigned int start_index = index * step;
      unsigned int end_index = (index + 1) * step;

      if (index == nbThreads - 1) {
        end_index = start_index + last_step;
      }

      ImageLut_Param_t *imageLut_param = new ImageLut_Param_t(start_index, end_index, bitmap);
      memcpy(imageLut_param->m_lut, lut, 256 * sizeof(unsigned char));

      imageLutParams.push_back(imageLut_param);

      // Start the threads
      vpThread *imageLut_thread = new vpThread((vpThread::Fn)performLutThread, (vpThread::Args)imageLut_param);
      threadpool.push_back(imageLut_thread);
    }

    for (size_t cpt = 0; cpt < threadpool.size(); cpt++) {
      // Wait until thread ends up
      threadpool[cpt]->join();
    }

    // Delete
    for (size_t cpt = 0; cpt < threadpool.size(); cpt++) {
      delete threadpool[cpt];
    }

    for (size_t cpt = 0; cpt < imageLutParams.size(); cpt++) {
      delete imageLutParams[cpt];
    }
#endif
  }
}

template <> inline void vpImage<vpRGBa>::performLut(const vpRGBa (&lut)[256], const unsigned int nbThreads)
{
  unsigned int size = getWidth() * getHeight();
  unsigned char *ptrStart = (unsigned char *)bitmap;
  unsigned char *ptrEnd = ptrStart + size * 4;
  unsigned char *ptrCurrent = ptrStart;

  bool use_single_thread = (nbThreads == 0 || nbThreads == 1);
#if !defined(VISP_HAVE_PTHREAD) && !defined(_WIN32)
  use_single_thread = true;
#endif

  if (!use_single_thread && getSize() <= nbThreads) {
    use_single_thread = true;
  }

  if (use_single_thread) {
    // Single thread
    while (ptrCurrent != ptrEnd) {
      *ptrCurrent = lut[*ptrCurrent].R;
      ++ptrCurrent;

      *ptrCurrent = lut[*ptrCurrent].G;
      ++ptrCurrent;

      *ptrCurrent = lut[*ptrCurrent].B;
      ++ptrCurrent;

      *ptrCurrent = lut[*ptrCurrent].A;
      ++ptrCurrent;
    }
  } else {
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
    // Multi-threads
    std::vector<vpThread *> threadpool;
    std::vector<ImageLutRGBa_Param_t *> imageLutParams;

    unsigned int image_size = getSize();
    unsigned int step = image_size / nbThreads;
    unsigned int last_step = image_size - step * (nbThreads - 1);

    for (unsigned int index = 0; index < nbThreads; index++) {
      unsigned int start_index = index * step;
      unsigned int end_index = (index + 1) * step;

      if (index == nbThreads - 1) {
        end_index = start_index + last_step;
      }

      ImageLutRGBa_Param_t *imageLut_param = new ImageLutRGBa_Param_t(start_index, end_index, (unsigned char *)bitmap);
      memcpy(imageLut_param->m_lut, lut, 256 * sizeof(vpRGBa));

      imageLutParams.push_back(imageLut_param);

      // Start the threads
      vpThread *imageLut_thread = new vpThread((vpThread::Fn)performLutRGBaThread, (vpThread::Args)imageLut_param);
      threadpool.push_back(imageLut_thread);
    }

    for (size_t cpt = 0; cpt < threadpool.size(); cpt++) {
      // Wait until thread ends up
      threadpool[cpt]->join();
    }

    // Delete
    for (size_t cpt = 0; cpt < threadpool.size(); cpt++) {
      delete threadpool[cpt];
    }

    for (size_t cpt = 0; cpt < imageLutParams.size(); cpt++) {
      delete imageLutParams[cpt];
    }
#endif
  }
}

template <class Type> void swap(vpImage<Type> &first, vpImage<Type> &second)
{
  using std::swap;
  swap(first.bitmap, second.bitmap);
  swap(first.display, second.display);
  swap(first.npixels, second.npixels);
  swap(first.width, second.width);
  swap(first.height, second.height);
  swap(first.row, second.row);
}

#endif