vpImageFilter.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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:
 * Various image tools, convolution, ...
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/

#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpImageFilter.h>
#include <visp3/core/vpRGBa.h>
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
#include <opencv2/imgproc/imgproc.hpp>
#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020101)
#include <opencv2/imgproc/imgproc_c.h>
#elif defined(VISP_HAVE_OPENCV)
#include <cv.h>
#endif

void vpImageFilter::filter(const vpImage<unsigned char> &I, vpImage<double> &If, const vpMatrix &M, bool convolve)
{
  unsigned int size_y = M.getRows(), size_x = M.getCols();
  unsigned int half_size_y = size_y / 2, half_size_x = size_x / 2;

  If.resize(I.getHeight(), I.getWidth(), 0.0);

  if (convolve) {
    for (unsigned int i = half_size_y; i < I.getHeight() - half_size_y; i++) {
      for (unsigned int j = half_size_x; j < I.getWidth() - half_size_x; j++) {
        double conv = 0;

        for (unsigned int a = 0; a < size_y; a++) {
          for (unsigned int b = 0; b < size_x; b++) {
            double val = I[i + half_size_y - a][j + half_size_x - b]; // Convolution
            conv += M[a][b] * val;
          }
        }
        If[i][j] = conv;
      }
    }
  } else {
    for (unsigned int i = half_size_y; i < I.getHeight() - half_size_y; i++) {
      for (unsigned int j = half_size_x; j < I.getWidth() - half_size_x; j++) {
        double corr = 0;

        for (unsigned int a = 0; a < size_y; a++) {
          for (unsigned int b = 0; b < size_x; b++) {
            double val = I[i - half_size_y + a][j - half_size_x + b]; // Correlation
            corr += M[a][b] * val;
          }
        }
        If[i][j] = corr;
      }
    }
  }
}

void vpImageFilter::filter(const vpImage<double> &I, vpImage<double> &Iu, vpImage<double> &Iv, const vpMatrix &M,
                           bool convolve)
{
  unsigned int size = M.getRows();
  unsigned int half_size = size / 2;

  Iu.resize(I.getHeight(), I.getWidth(), 0.0);
  Iv.resize(I.getHeight(), I.getWidth(), 0.0);

  if (convolve) {
    for (unsigned int v = half_size; v < I.getHeight() - half_size; v++) {
      for (unsigned int u = half_size; u < I.getWidth() - half_size; u++) {
        double conv_u = 0;
        double conv_v = 0;

        for (unsigned int a = 0; a < size; a++) {
          for (unsigned int b = 0; b < size; b++) {
            double val = I[v + half_size - a][u + half_size - b]; // Convolution
            conv_u += M[a][b] * val;
            conv_v += M[b][a] * val;
          }
        }
        Iu[v][u] = conv_u;
        Iv[v][u] = conv_v;
      }
    }
  } else {
    for (unsigned int v = half_size; v < I.getHeight() - half_size; v++) {
      for (unsigned int u = half_size; u < I.getWidth() - half_size; u++) {
        double conv_u = 0;
        double conv_v = 0;

        for (unsigned int a = 0; a < size; a++) {
          for (unsigned int b = 0; b < size; b++) {
            double val = I[v - half_size + a][u - half_size + b]; // Correlation
            conv_u += M[a][b] * val;
            conv_v += M[b][a] * val;
          }
        }
        Iu[v][u] = conv_u;
        Iv[v][u] = conv_v;
      }
    }
  }
}

void vpImageFilter::sepFilter(const vpImage<unsigned char> &I, vpImage<double> &If, const vpColVector &kernelH,
                              const vpColVector &kernelV)
{
  unsigned int size = kernelH.size();
  unsigned int half_size = size / 2;

  If.resize(I.getHeight(), I.getWidth(), 0.0);
  vpImage<double> I_filter(I.getHeight(), I.getWidth(), 0.0);

  for (unsigned int i = 0; i < I.getHeight(); i++) {
    for (unsigned int j = half_size; j < I.getWidth() - half_size; j++) {
      double conv = 0.0;
      for (unsigned int a = 0; a < kernelH.size(); a++) {
        conv += kernelH[a] * I[i][j + half_size - a];
      }

      I_filter[i][j] = conv;
    }
  }

  for (unsigned int i = half_size; i < I.getHeight() - half_size; i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      double conv = 0.0;
      for (unsigned int a = 0; a < kernelV.size(); a++) {
        conv += kernelV[a] * I_filter[i + half_size - a][j];
      }

      If[i][j] = conv;
    }
  }
}

#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)

void vpImageFilter::canny(const vpImage<unsigned char> &Isrc, vpImage<unsigned char> &Ires,
                          unsigned int gaussianFilterSize, double thresholdCanny,
                          unsigned int apertureSobel)
{
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
  IplImage *img_ipl = NULL;
  vpImageConvert::convert(Isrc, img_ipl);
  IplImage *edges_ipl;
  edges_ipl = cvCreateImage(cvSize(img_ipl->width, img_ipl->height), img_ipl->depth, img_ipl->nChannels);

  cvSmooth(img_ipl, img_ipl, CV_GAUSSIAN, (int)gaussianFilterSize, (int)gaussianFilterSize, 0, 0);
  cvCanny(img_ipl, edges_ipl, thresholdCanny, thresholdCanny, (int)apertureSobel);

  vpImageConvert::convert(edges_ipl, Ires);
  cvReleaseImage(&img_ipl);
  cvReleaseImage(&edges_ipl);
#else
  cv::Mat img_cvmat, edges_cvmat;
  vpImageConvert::convert(Isrc, img_cvmat);
  cv::GaussianBlur(img_cvmat, img_cvmat, cv::Size((int)gaussianFilterSize, (int)gaussianFilterSize), 0, 0);
  cv::Canny(img_cvmat, edges_cvmat, thresholdCanny, thresholdCanny, (int)apertureSobel);
  vpImageConvert::convert(edges_cvmat, Ires);
#endif
}
#endif

void vpImageFilter::filter(const vpImage<unsigned char> &I, vpImage<double> &GI, const double *filter,
                           unsigned int size)
{
  vpImage<double> GIx;
  filterX(I, GIx, filter, size);
  filterY(GIx, GI, filter, size);
  GIx.destroy();
}

void vpImageFilter::filter(const vpImage<double> &I, vpImage<double> &GI, const double *filter, unsigned int size)
{
  vpImage<double> GIx;
  filterX(I, GIx, filter, size);
  filterY(GIx, GI, filter, size);
  GIx.destroy();
}

void vpImageFilter::filterX(const vpImage<unsigned char> &I, vpImage<double> &dIx, const double *filter,
                            unsigned int size)
{
  dIx.resize(I.getHeight(), I.getWidth());
  for (unsigned int i = 0; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < (size - 1) / 2; j++) {
      dIx[i][j] = vpImageFilter::filterXLeftBorder(I, i, j, filter, size);
      // dIx[i][j]=0;
    }
    for (unsigned int j = (size - 1) / 2; j < I.getWidth() - (size - 1) / 2; j++) {
      dIx[i][j] = vpImageFilter::filterX(I, i, j, filter, size);
    }
    for (unsigned int j = I.getWidth() - (size - 1) / 2; j < I.getWidth(); j++) {
      dIx[i][j] = vpImageFilter::filterXRightBorder(I, i, j, filter, size);
      // dIx[i][j]=0;
    }
  }
}
void vpImageFilter::filterX(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &dIx, const double *filter,
                            unsigned int size)
{
  dIx.resize(I.getHeight(), I.getWidth());
  for (unsigned int i = 0; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < (size - 1) / 2; j++) {
      dIx[i][j].R = static_cast<unsigned char>(vpImageFilter::filterXLeftBorderR(I, i, j, filter, size));
      dIx[i][j].G = static_cast<unsigned char>(vpImageFilter::filterXLeftBorderG(I, i, j, filter, size));
      dIx[i][j].B = static_cast<unsigned char>(vpImageFilter::filterXLeftBorderB(I, i, j, filter, size));
    }
    for (unsigned int j = (size - 1) / 2; j < I.getWidth() - (size - 1) / 2; j++) {
      dIx[i][j].R = static_cast<unsigned char>(vpImageFilter::filterXR(I, i, j, filter, size));
      dIx[i][j].G = static_cast<unsigned char>(vpImageFilter::filterXG(I, i, j, filter, size));
      dIx[i][j].B = static_cast<unsigned char>(vpImageFilter::filterXB(I, i, j, filter, size));
    }
    for (unsigned int j = I.getWidth() - (size - 1) / 2; j < I.getWidth(); j++) {
      dIx[i][j].R = static_cast<unsigned char>(vpImageFilter::filterXRightBorderR(I, i, j, filter, size));
      dIx[i][j].G = static_cast<unsigned char>(vpImageFilter::filterXRightBorderG(I, i, j, filter, size));
      dIx[i][j].B = static_cast<unsigned char>(vpImageFilter::filterXRightBorderB(I, i, j, filter, size));
    }
  }
}
void vpImageFilter::filterX(const vpImage<double> &I, vpImage<double> &dIx, const double *filter, unsigned int size)
{
  dIx.resize(I.getHeight(), I.getWidth());
  for (unsigned int i = 0; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < (size - 1) / 2; j++) {
      dIx[i][j] = vpImageFilter::filterXLeftBorder(I, i, j, filter, size);
      // dIx[i][j]=0;
    }
    for (unsigned int j = (size - 1) / 2; j < I.getWidth() - (size - 1) / 2; j++) {
      dIx[i][j] = vpImageFilter::filterX(I, i, j, filter, size);
    }
    for (unsigned int j = I.getWidth() - (size - 1) / 2; j < I.getWidth(); j++) {
      dIx[i][j] = vpImageFilter::filterXRightBorder(I, i, j, filter, size);
      // dIx[i][j]=0;
    }
  }
}
void vpImageFilter::filterY(const vpImage<unsigned char> &I, vpImage<double> &dIy, const double *filter,
                            unsigned int size)
{
  dIy.resize(I.getHeight(), I.getWidth());
  for (unsigned int i = 0; i < (size - 1) / 2; i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = vpImageFilter::filterYTopBorder(I, i, j, filter, size);
    }
  }
  for (unsigned int i = (size - 1) / 2; i < I.getHeight() - (size - 1) / 2; i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = vpImageFilter::filterY(I, i, j, filter, size);
    }
  }
  for (unsigned int i = I.getHeight() - (size - 1) / 2; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = vpImageFilter::filterYBottomBorder(I, i, j, filter, size);
    }
  }
}
void vpImageFilter::filterY(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &dIy, const double *filter,
                            unsigned int size)
{
  dIy.resize(I.getHeight(), I.getWidth());
  for (unsigned int i = 0; i < (size - 1) / 2; i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j].R = static_cast<unsigned char>(vpImageFilter::filterYTopBorderR(I, i, j, filter, size));
      dIy[i][j].G = static_cast<unsigned char>(vpImageFilter::filterYTopBorderG(I, i, j, filter, size));
      dIy[i][j].B = static_cast<unsigned char>(vpImageFilter::filterYTopBorderB(I, i, j, filter, size));
    }
  }
  for (unsigned int i = (size - 1) / 2; i < I.getHeight() - (size - 1) / 2; i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j].R = static_cast<unsigned char>(vpImageFilter::filterYR(I, i, j, filter, size));
      dIy[i][j].G = static_cast<unsigned char>(vpImageFilter::filterYG(I, i, j, filter, size));
      dIy[i][j].B = static_cast<unsigned char>(vpImageFilter::filterYB(I, i, j, filter, size));
    }
  }
  for (unsigned int i = I.getHeight() - (size - 1) / 2; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j].R = static_cast<unsigned char>(vpImageFilter::filterYBottomBorderR(I, i, j, filter, size));
      dIy[i][j].G = static_cast<unsigned char>(vpImageFilter::filterYBottomBorderG(I, i, j, filter, size));
      dIy[i][j].B = static_cast<unsigned char>(vpImageFilter::filterYBottomBorderB(I, i, j, filter, size));
    }
  }
}
void vpImageFilter::filterY(const vpImage<double> &I, vpImage<double> &dIy, const double *filter, unsigned int size)
{
  dIy.resize(I.getHeight(), I.getWidth());
  for (unsigned int i = 0; i < (size - 1) / 2; i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = vpImageFilter::filterYTopBorder(I, i, j, filter, size);
    }
  }
  for (unsigned int i = (size - 1) / 2; i < I.getHeight() - (size - 1) / 2; i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = vpImageFilter::filterY(I, i, j, filter, size);
    }
  }
  for (unsigned int i = I.getHeight() - (size - 1) / 2; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = vpImageFilter::filterYBottomBorder(I, i, j, filter, size);
    }
  }
}

void vpImageFilter::gaussianBlur(const vpImage<unsigned char> &I, vpImage<double> &GI, unsigned int size, double sigma,
                                 bool normalize)
{
  double *fg = new double[(size + 1) / 2];
  vpImageFilter::getGaussianKernel(fg, size, sigma, normalize);
  vpImage<double> GIx;
  vpImageFilter::filterX(I, GIx, fg, size);
  vpImageFilter::filterY(GIx, GI, fg, size);
  GIx.destroy();
  delete[] fg;
}

void vpImageFilter::gaussianBlur(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &GI, unsigned int size, double sigma,
                                 bool normalize)
{
  double *fg = new double[(size + 1) / 2];
  vpImageFilter::getGaussianKernel(fg, size, sigma, normalize);
  vpImage<vpRGBa> GIx;
  vpImageFilter::filterX(I, GIx, fg, size);
  vpImageFilter::filterY(GIx, GI, fg, size);
  GIx.destroy();
  delete[] fg;
}

void vpImageFilter::gaussianBlur(const vpImage<double> &I, vpImage<double> &GI, unsigned int size, double sigma,
                                 bool normalize)
{
  double *fg = new double[(size + 1) / 2];
  vpImageFilter::getGaussianKernel(fg, size, sigma, normalize);
  vpImage<double> GIx;
  vpImageFilter::filterX(I, GIx, fg, size);
  vpImageFilter::filterY(GIx, GI, fg, size);
  GIx.destroy();
  delete[] fg;
}

void vpImageFilter::getGaussianKernel(double *filter, unsigned int size, double sigma, bool normalize)
{
  if (size % 2 != 1)
    throw(vpImageException(vpImageException::incorrectInitializationError, "Bad Gaussian filter size"));

  if (sigma <= 0)
    sigma = (size - 1) / 6.0;

  int middle = (int)(size - 1) / 2;
  double sigma2 = vpMath::sqr(sigma);
  for (int i = 0; i <= middle; i++) {
    filter[i] = (1. / (sigma * sqrt(2. * M_PI))) * exp(-(i * i) / (2. * sigma2));
  }
  if (normalize) {
    // renormalization
    double sum = 0;
    for (int i = 1; i <= middle; i++) {
      sum += 2 * filter[i];
    }
    sum += filter[0];

    for (int i = 0; i <= middle; i++) {
      filter[i] = filter[i] / sum;
    }
  }
}

void vpImageFilter::getGaussianDerivativeKernel(double *filter, unsigned int size, double sigma, bool normalize)
{
  if (size % 2 != 1)
    throw(vpImageException(vpImageException::incorrectInitializationError, "Bad Gaussian filter size"));

  if (sigma <= 0)
    sigma = (size - 1) / 6.0;

  int middle = (int)(size - 1) / 2;
  double sigma2 = vpMath::sqr(sigma);
  filter[0] = 0.;
  for (int i = 1; i <= middle; i++) {
    filter[i] = -(1. / (sigma * sqrt(2. * M_PI))) *
                (exp(-((i + 1) * (i + 1)) / (2. * sigma2)) - exp(-((i - 1) * (i - 1)) / (2. * sigma2))) / 2.;
  }

  if (normalize) {
    double sum = 0;
    for (int i = 1; i <= middle; i++) {
      sum += 2. * (1. / (sigma * sqrt(2. * M_PI))) * exp(-(i * i) / (2. * sigma2));
    }
    sum += (1. / (sigma * sqrt(2. * M_PI)));

    for (int i = 1; i <= middle; i++) {
      filter[i] = filter[i] / sum;
    }
  }
}

void vpImageFilter::getGradX(const vpImage<unsigned char> &I, vpImage<double> &dIx)
{
  dIx.resize(I.getHeight(), I.getWidth());
  // dIx=0;
  for (unsigned int i = 0; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < 3; j++) {
      dIx[i][j] = 0;
    }
    for (unsigned int j = 3; j < I.getWidth() - 3; j++) {
      dIx[i][j] = vpImageFilter::derivativeFilterX(I, i, j);
    }
    for (unsigned int j = I.getWidth() - 3; j < I.getWidth(); j++) {
      dIx[i][j] = 0;
    }
  }
}

void vpImageFilter::getGradY(const vpImage<unsigned char> &I, vpImage<double> &dIy)
{
  dIy.resize(I.getHeight(), I.getWidth());
  // dIy=0;
  for (unsigned int i = 0; i < 3; i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = 0;
    }
  }
  for (unsigned int i = 3; i < I.getHeight() - 3; i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = vpImageFilter::derivativeFilterY(I, i, j);
    }
  }
  for (unsigned int i = I.getHeight() - 3; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = 0;
    }
  }
}

void vpImageFilter::getGradX(const vpImage<unsigned char> &I, vpImage<double> &dIx, const double *filter,
                             unsigned int size)
{
  dIx.resize(I.getHeight(), I.getWidth());
  //#pragma omp parallel for
  for (unsigned int i = 0; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < (size - 1) / 2; j++) {
      dIx[i][j] = 0;
    }
    for (unsigned int j = (size - 1) / 2; j < I.getWidth() - (size - 1) / 2; j++) {
      dIx[i][j] = vpImageFilter::derivativeFilterX(I, i, j, filter, size);
    }
    for (unsigned int j = I.getWidth() - (size - 1) / 2; j < I.getWidth(); j++) {
      dIx[i][j] = 0;
    }
  }
}
void vpImageFilter::getGradX(const vpImage<double> &I, vpImage<double> &dIx, const double *filter, unsigned int size)
{
  dIx.resize(I.getHeight(), I.getWidth());
  // dIx=0;
  for (unsigned int i = 0; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < (size - 1) / 2; j++) {
      dIx[i][j] = 0;
    }
    for (unsigned int j = (size - 1) / 2; j < I.getWidth() - (size - 1) / 2; j++) {
      dIx[i][j] = vpImageFilter::derivativeFilterX(I, i, j, filter, size);
    }
    for (unsigned int j = I.getWidth() - (size - 1) / 2; j < I.getWidth(); j++) {
      dIx[i][j] = 0;
    }
  }
}

void vpImageFilter::getGradY(const vpImage<unsigned char> &I, vpImage<double> &dIy, const double *filter,
                             unsigned int size)
{
  dIy.resize(I.getHeight(), I.getWidth());
  //#pragma omp parallel for
  for (unsigned int i = 0; i < (size - 1) / 2; i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = 0;
    }
  }
  //#pragma omp parallel for
  for (unsigned int i = (size - 1) / 2; i < I.getHeight() - (size - 1) / 2; i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = vpImageFilter::derivativeFilterY(I, i, j, filter, size);
    }
  }
  //#pragma omp parallel for
  for (unsigned int i = I.getHeight() - (size - 1) / 2; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = 0;
    }
  }
}

void vpImageFilter::getGradY(const vpImage<double> &I, vpImage<double> &dIy, const double *filter, unsigned int size)
{
  dIy.resize(I.getHeight(), I.getWidth());
  // dIy=0;
  for (unsigned int i = 0; i < (size - 1) / 2; i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = 0;
    }
  }
  for (unsigned int i = (size - 1) / 2; i < I.getHeight() - (size - 1) / 2; i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = vpImageFilter::derivativeFilterY(I, i, j, filter, size);
    }
  }
  for (unsigned int i = I.getHeight() - (size - 1) / 2; i < I.getHeight(); i++) {
    for (unsigned int j = 0; j < I.getWidth(); j++) {
      dIy[i][j] = 0;
    }
  }
}

void vpImageFilter::getGradXGauss2D(const vpImage<unsigned char> &I, vpImage<double> &dIx, const double *gaussianKernel,
                                    const double *gaussianDerivativeKernel, unsigned int size)
{
  vpImage<double> GIy;
  vpImageFilter::filterY(I, GIy, gaussianKernel, size);
  vpImageFilter::getGradX(GIy, dIx, gaussianDerivativeKernel, size);
}

void vpImageFilter::getGradYGauss2D(const vpImage<unsigned char> &I, vpImage<double> &dIy, const double *gaussianKernel,
                                    const double *gaussianDerivativeKernel, unsigned int size)
{
  vpImage<double> GIx;
  vpImageFilter::filterX(I, GIx, gaussianKernel, size);
  vpImageFilter::getGradY(GIx, dIy, gaussianDerivativeKernel, size);
}

// operation pour pyramide gaussienne
void vpImageFilter::getGaussPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char> &GI)
{
  vpImage<unsigned char> GIx;
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
  cv::Mat imgsrc, imgdest;
  vpImageConvert::convert(I, imgsrc);
  cv::pyrDown(imgsrc, imgdest, cv::Size((int)I.getWidth() / 2, (int)I.getHeight() / 2));
  vpImageConvert::convert(imgdest, GI);
#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
  cv::Mat imgsrc, imgdest;
  vpImageConvert::convert(I, imgsrc);
  cv::pyrDown(imgsrc, imgdest, cvSize((int)I.getWidth() / 2, (int)I.getHeight() / 2));
  vpImageConvert::convert(imgdest, GI);
#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
  IplImage *imgsrc = NULL;  // cvCreateImage(cvGetSize(imgign), IPL_DEPTH_8U, 1);
  IplImage *imgdest = NULL; // cvCreateImage(cvGetSize(imgign), IPL_DEPTH_8U, 1);
  imgsrc = cvCreateImage(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
  imgdest = cvCreateImage(cvSize((int)I.getWidth() / 2, (int)I.getHeight() / 2), IPL_DEPTH_8U, 1);
  vpImageConvert::convert(I, imgsrc);
  cvPyrDown(imgsrc, imgdest);
  vpImageConvert::convert(imgdest, GI);

  cvReleaseImage(&imgsrc);
  cvReleaseImage(&imgdest);
// vpImage<unsigned char> sGI;sGI=GI;

#else
  vpImageFilter::getGaussXPyramidal(I, GIx);
  vpImageFilter::getGaussYPyramidal(GIx, GI);
#endif
}

void vpImageFilter::getGaussXPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char> &GI)
{
#if 0
  GI.resize(I.getHeight(),(int)((I.getWidth()+1.)/2.)) ;
  for (unsigned int i=0 ; i < I.getHeight() ; i++)
  {
    GI[i][0]=I[i][0];
    for (unsigned int j=1 ; j < ((I.getWidth()+1.)/2.)-1 ; j++)
    {
      GI[i][j]=vpImageFilter::filterGaussXPyramidal(I,i,2*j);
    }
    GI[i][(int)((I.getWidth()+1.)/2.)-1]=I[i][2*((int)((I.getWidth()+1.)/2.)-1)];
  }
#else
  unsigned int w = I.getWidth() / 2;

  GI.resize(I.getHeight(), w);
  for (unsigned int i = 0; i < I.getHeight(); i++) {
    GI[i][0] = I[i][0];
    for (unsigned int j = 1; j < w - 1; j++) {
      GI[i][j] = vpImageFilter::filterGaussXPyramidal(I, i, 2 * j);
    }
    GI[i][w - 1] = I[i][2 * w - 1];
  }

#endif
}
void vpImageFilter::getGaussYPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char> &GI)
{

#ifdef ORIG
  GI.resize((int)((I.getHeight() + 1.) / 2.), I.getWidth());
  for (unsigned int j = 0; j < I.getWidth(); j++) {
    GI[0][j] = I[0][j];
    for (unsigned int i = 1; i < ((I.getHeight() + 1.) / 2.) - 1; i++) {
      GI[i][j] = vpImageFilter::filterGaussYPyramidal(I, 2 * i, j);
    }
    GI[(int)((I.getHeight() + 1.) / 2.) - 1][j] = I[2 * ((int)((I.getHeight() + 1.) / 2.) - 1)][j];
  }

#else
  unsigned int h = I.getHeight() / 2;

  GI.resize(h, I.getWidth());
  for (unsigned int j = 0; j < I.getWidth(); j++) {
    GI[0][j] = I[0][j];
    for (unsigned int i = 1; i < h - 1; i++) {
      GI[i][j] = vpImageFilter::filterGaussYPyramidal(I, 2 * i, j);
    }
    GI[h - 1][j] = I[2 * h - 1][j];
  }
#endif
}

double vpImageFilter::getSobelKernelX(double *filter, unsigned int size)
{
  if (size == 0)
    throw vpException(vpException::dimensionError, "Cannot get Sobel kernel of size 0!");
  if (size > 20)
    throw vpException(vpException::dimensionError, "Cannot get Sobel kernel of size > 20!");

  vpMatrix SobelY(size*2+1, size*2+1);
  double norm = getSobelKernelY(SobelY.data, size);
  memcpy(filter, SobelY.t().data, SobelY.getRows()*SobelY.getCols()*sizeof(double));
  return norm;
}

double vpImageFilter::getSobelKernelY(double *filter, unsigned int size)
{
  //Sobel kernel pre-computed for the usual size
  static const double SobelY3x3[9] = {-1.0, -2.0, -1.0,
                                      0.0, 0.0, 0.0,
                                      1.0, 2.0, 1.0};
  static const double SobelY5x5[25] = {-1.0, -4.0, -6.0, -4.0, -1.0,
                                       -2.0, -8.0, -12.0, -8.0, -2.0,
                                       0.0, 0.0, 0.0, 0.0, 0.0,
                                       2.0, 8.0, 12.0, 8.0, 2.0,
                                       1.0, 4.0, 6.0, 4.0, 1.0};
  static const double SobelY7x7[49] = {-1, -6, -15, -20, -15, -6, -1,
                                       -4, -24, -60, -80, -60, -24, -4,
                                       -5, -30, -75, -100, -75, -30, -5,
                                        0, 0, 0, 0, 0, 0, 0,
                                        5, 30, 75, 100, 75, 30, 5,
                                        4, 24, 60, 80, 60, 24, 4,
                                        1, 6, 15, 20, 15, 6, 1};
  static const vpMatrix smoothingKernel(3,3);
  smoothingKernel[0][0] = 1.0;  smoothingKernel[0][1] = 2.0;  smoothingKernel[0][2] = 1.0;
  smoothingKernel[1][0] = 2.0;  smoothingKernel[1][1] = 4.0;  smoothingKernel[1][2] = 2.0;
  smoothingKernel[2][0] = 1.0;  smoothingKernel[2][1] = 2.0;  smoothingKernel[2][2] = 1.0;

  if (size == 0)
    throw vpException(vpException::dimensionError, "Cannot get Sobel kernel of size 0!");
  if (size > 20)
    throw vpException(vpException::dimensionError, "Cannot get Sobel kernel of size > 20!");

  const unsigned int kernel_size = size*2+1;
  if (kernel_size == 3) {
    memcpy(filter, SobelY3x3, kernel_size*kernel_size*sizeof(double));
    return 1/8.0;
  }
  if (kernel_size == 5) {
    memcpy(filter, SobelY5x5, kernel_size*kernel_size*sizeof(double));
    return 1/16.0;
  }
  if (kernel_size == 7) {
    memcpy(filter, SobelY7x7, kernel_size*kernel_size*sizeof(double));
    return 1/16.0;
  }

  vpMatrix sobelY(7,7);
  memcpy(sobelY.data, SobelY7x7, sobelY.getRows()*sobelY.getCols()*sizeof(double));
  for (unsigned int i = 4; i <= size; i++) {
    sobelY = vpMatrix::conv2(sobelY, smoothingKernel, "full");
  }

  memcpy(filter, sobelY.data, sobelY.getRows()*sobelY.getCols()*sizeof(double));

  return 1/16.0;
}