vpImageFilter.h

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 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 https://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, ...
 */
 
#ifndef _vpImageFilter_h_
#define _vpImageFilter_h_
 
#include <fstream>
#include <iostream>
#include <math.h>
#include <string.h>
 
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageException.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpRGBa.h>
 
#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC)
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#endif
 
class VISP_EXPORT vpImageFilter
{
public:
  static void canny(const vpImage<unsigned char> &I, vpImage<unsigned char> &Ic, unsigned int gaussianFilterSize,
                    float thresholdCanny, unsigned int apertureSobel);
 
  static void canny(const vpImage<unsigned char> &I, vpImage<unsigned char> &Ic, unsigned int gaussianFilterSize,
                    float lowerThresholdCanny, float higherThresholdCanny, unsigned int apertureSobel);
 
  template <class T> static double derivativeFilterX(const vpImage<T> &I, unsigned int r, unsigned int c)
  {
    return (2047.0 * (I[r][c + 1] - I[r][c - 1]) + 913.0 * (I[r][c + 2] - I[r][c - 2]) +
            112.0 * (I[r][c + 3] - I[r][c - 3])) / 8418.0;
  }
 
  template <class T> static double derivativeFilterY(const vpImage<T> &I, unsigned int r, unsigned int c)
  {
    return (2047.0 * (I[r + 1][c] - I[r - 1][c]) + 913.0 * (I[r + 2][c] - I[r - 2][c]) +
            112.0 * (I[r + 3][c] - I[r - 3][c])) / 8418.0;
  }
 
  template <class T, typename FilterType>
  static FilterType derivativeFilterX(const vpImage<T> &I, unsigned int r, unsigned int c, const FilterType *filter, unsigned int size)
  {
    unsigned int i;
    FilterType result;
 
    result = 0;
 
    for (i = 1; i <= (size - 1) / 2; i++) {
      result += filter[i] * (I[r][c + i] - I[r][c - i]);
    }
    return result;
  }
 
  template <class T, typename FilterType>
  static FilterType derivativeFilterY(const vpImage<T> &I, unsigned int r, unsigned int c, const FilterType *filter, unsigned int size)
  {
    unsigned int i;
    FilterType result;
 
    result = 0;
 
    for (i = 1; i <= (size - 1) / 2; i++) {
      result += filter[i] * (I[r + i][c] - I[r - i][c]);
    }
    return result;
  }
 
  template <typename FilterType>
  static void filter(const vpImage<unsigned char> &I, vpImage<FilterType> &If, const vpArray2D<FilterType> &M, bool convolve = false)
  {
    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++) {
          FilterType conv = 0;
 
          for (unsigned int a = 0; a < size_y; a++) {
            for (unsigned int b = 0; b < size_x; b++) {
              FilterType 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++) {
          FilterType corr = 0;
 
          for (unsigned int a = 0; a < size_y; a++) {
            for (unsigned int b = 0; b < size_x; b++) {
              FilterType val = I[i - half_size_y + a][j - half_size_x + b]; // Correlation
              corr += M[a][b] * val;
            }
          }
          If[i][j] = corr;
        }
      }
    }
  }
 
  template <typename FilterType>
  static void filter(const vpImage<FilterType> &I, vpImage<FilterType> &Iu, vpImage<FilterType> &Iv, const vpArray2D<FilterType> &M,
    bool convolve = false)
  {
    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++) {
          FilterType conv_u = 0;
          FilterType conv_v = 0;
 
          for (unsigned int a = 0; a < size; a++) {
            for (unsigned int b = 0; b < size; b++) {
              FilterType 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++) {
          FilterType conv_u = 0;
          FilterType conv_v = 0;
 
          for (unsigned int a = 0; a < size; a++) {
            for (unsigned int b = 0; b < size; b++) {
              FilterType 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;
        }
      }
    }
  }
 
  static void sepFilter(const vpImage<unsigned char> &I, vpImage<double> &If, const vpColVector &kernelH, const vpColVector &kernelV);
 
  template <typename FilterType>
  static void filter(const vpImage<unsigned char> &I, vpImage<FilterType> &GI, const FilterType *filter, unsigned int size)
  {
    vpImage<FilterType> GIx;
    filterX<FilterType>(I, GIx, filter, size);
    filterY<FilterType>(GIx, GI, filter, size);
    GIx.destroy();
  }
 
  template <typename FilterType>
  static void filter(const vpImage<FilterType> &I, vpImage<FilterType> &GI, const FilterType *filter, unsigned int size)
  {
    vpImage<FilterType> GIx;
    filterX<FilterType>(I, GIx, filter, size);
    filterY<FilterType>(GIx, GI, filter, size);
    GIx.destroy();
  }
 
  static inline unsigned char filterGaussXPyramidal(const vpImage<unsigned char> &I, unsigned int i, unsigned int j)
  {
    return (unsigned char)((1. * I[i][j - 2] + 4. * I[i][j - 1] + 6. * I[i][j] + 4. * I[i][j + 1] + 1. * I[i][j + 2]) / 16.);
  }
  static inline unsigned char filterGaussYPyramidal(const vpImage<unsigned char> &I, unsigned int i, unsigned int j)
  {
    return (unsigned char)((1. * I[i - 2][j] + 4. * I[i - 1][j] + 6. * I[i][j] + 4. * I[i + 1][j] + 1. * I[i + 2][j]) / 16.);
  }
 
  template <typename FilterType>
  static void filterX(const vpImage<unsigned char> &I, vpImage<FilterType> &dIx, const FilterType *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<FilterType>(I, i, j, filter, size);
      }
      for (unsigned int j = (size - 1) / 2; j < I.getWidth() - (size - 1) / 2; j++) {
        dIx[i][j] = vpImageFilter::filterX<FilterType>(I, i, j, filter, size);
      }
      for (unsigned int j = I.getWidth() - (size - 1) / 2; j < I.getWidth(); j++) {
        dIx[i][j] = vpImageFilter::filterXRightBorder<FilterType>(I, i, j, filter, size);
      }
    }
  }
 
  template<typename FilterType>
  static void filterX(const vpImage<FilterType> &I, vpImage<FilterType> &dIx, const FilterType *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<FilterType>(I, i, j, filter, size);
      }
      for (unsigned int j = (size - 1) / 2; j < I.getWidth() - (size - 1) / 2; j++) {
        dIx[i][j] = vpImageFilter::filterX<FilterType>(I, i, j, filter, size);
      }
      for (unsigned int j = I.getWidth() - (size - 1) / 2; j < I.getWidth(); j++) {
        dIx[i][j] = vpImageFilter::filterXRightBorder<FilterType>(I, i, j, filter, size);
      }
    }
  }
 
  static void filterX(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &dIx, const double *filter, unsigned int size);
  static void filterXR(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &dIx, const double *filter, unsigned int size);
  static void filterXG(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &dIx, const double *filter, unsigned int size);
  static void filterXB(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &dIx, const double *filter, unsigned int size);
 
  template<typename FilterType>
  static inline FilterType filterX(const vpImage<unsigned char> &I, unsigned int r, unsigned int c, const FilterType *filter, unsigned int size)
  {
    FilterType result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      result += filter[i] * (I[r][c + i] + I[r][c - i]);
    }
    return result + filter[0] * I[r][c];
  }
 
  static inline double filterXR(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c, const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      result += filter[i] * (I[r][c + i].R + I[r][c - i].R);
    }
    return result + filter[0] * I[r][c].R;
  }
 
  static inline double filterXG(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c, const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      result += filter[i] * (I[r][c + i].G + I[r][c - i].G);
    }
    return result + filter[0] * I[r][c].G;
  }
 
  static inline double filterXB(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c, const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      result += filter[i] * (I[r][c + i].B + I[r][c - i].B);
    }
    return result + filter[0] * I[r][c].B;
  }
 
  template <typename FilterType>
  static inline FilterType filterXLeftBorder(const vpImage<unsigned char> &I, unsigned int r, unsigned int c,
                                             const FilterType *filter, unsigned int size)
  {
    FilterType result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (c > i)
        result += filter[i] * (I[r][c + i] + I[r][c - i]);
      else
        result += filter[i] * (I[r][c + i] + I[r][i - c]);
    }
    return result + filter[0] * I[r][c];
  }
 
  static inline double filterXLeftBorderR(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c,
                                          const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (c > i)
        result += filter[i] * (I[r][c + i].R + I[r][c - i].R);
      else
        result += filter[i] * (I[r][c + i].R + I[r][i - c].R);
    }
    return result + filter[0] * I[r][c].R;
  }
 
  static inline double filterXLeftBorderG(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c,
                                          const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (c > i)
        result += filter[i] * (I[r][c + i].G + I[r][c - i].G);
      else
        result += filter[i] * (I[r][c + i].G + I[r][i - c].G);
    }
    return result + filter[0] * I[r][c].G;
  }
 
  static inline double filterXLeftBorderB(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c,
                                          const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (c > i)
        result += filter[i] * (I[r][c + i].B + I[r][c - i].B);
      else
        result += filter[i] * (I[r][c + i].B + I[r][i - c].B);
    }
    return result + filter[0] * I[r][c].B;
  }
 
  template <typename FilterType>
  static inline FilterType filterXRightBorder(const vpImage<unsigned char> &I, unsigned int r, unsigned int c,
                                              const FilterType *filter, unsigned int size)
  {
    FilterType result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (c + i < I.getWidth())
        result += filter[i] * (I[r][c + i] + I[r][c - i]);
      else
        result += filter[i] * (I[r][2 * I.getWidth() - c - i - 1] + I[r][c - i]);
    }
    return result + filter[0] * I[r][c];
  }
 
  static inline double filterXRightBorderR(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c,
                                           const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (c + i < I.getWidth())
        result += filter[i] * (I[r][c + i].R + I[r][c - i].R);
      else
        result += filter[i] * (I[r][2 * I.getWidth() - c - i - 1].R + I[r][c - i].R);
    }
    return result + filter[0] * I[r][c].R;
  }
 
  static inline double filterXRightBorderG(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c,
                                           const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (c + i < I.getWidth())
        result += filter[i] * (I[r][c + i].G + I[r][c - i].G);
      else
        result += filter[i] * (I[r][2 * I.getWidth() - c - i - 1].G + I[r][c - i].G);
    }
    return result + filter[0] * I[r][c].G;
  }
 
  static inline double filterXRightBorderB(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c,
                                           const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (c + i < I.getWidth())
        result += filter[i] * (I[r][c + i].B + I[r][c - i].B);
      else
        result += filter[i] * (I[r][2 * I.getWidth() - c - i - 1].B + I[r][c - i].B);
    }
    return result + filter[0] * I[r][c].B;
  }
 
  template <typename FilterType>
  static inline FilterType filterX(const vpImage<FilterType> &I, unsigned int r, unsigned int c,
                                   const FilterType *filter, unsigned int size)
  {
    FilterType result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      result += filter[i] * (I[r][c + i] + I[r][c - i]);
    }
    return result + filter[0] * I[r][c];
  }
 
  template <typename FilterType>
  static inline FilterType filterXLeftBorder(const vpImage<FilterType> &I, unsigned int r, unsigned int c,
                                             const FilterType *filter, unsigned int size)
  {
    FilterType result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (c > i)
        result += filter[i] * (I[r][c + i] + I[r][c - i]);
      else
        result += filter[i] * (I[r][c + i] + I[r][i - c]);
    }
    return result + filter[0] * I[r][c];
  }
 
  template <typename FilterType>
  static inline FilterType filterXRightBorder(const vpImage<FilterType> &I, unsigned int r, unsigned int c,
                                              const FilterType *filter, unsigned int size)
  {
    FilterType result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (c + i < I.getWidth())
        result += filter[i] * (I[r][c + i] + I[r][c - i]);
      else
        result += filter[i] * (I[r][2 * I.getWidth() - c - i - 1] + I[r][c - i]);
    }
    return result + filter[0] * I[r][c];
  }
 
  template <typename FilterType>
  static void filterY(const vpImage<unsigned char> &I, vpImage<FilterType> &dIy, const FilterType *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<FilterType>(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<FilterType>(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<FilterType>(I, i, j, filter, size);
      }
    }
  }
 
  static void filterY(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &dIx, const double *filter, unsigned int size);
  static void filterYR(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &dIx, const double *filter, unsigned int size);
  static void filterYG(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &dIx, const double *filter, unsigned int size);
  static void filterYB(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &dIx, const double *filter, unsigned int size);
 
  template<typename FilterType>
  static void filterY(const vpImage<FilterType> &I, vpImage<FilterType> &dIy, const FilterType *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<FilterType>(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<FilterType>(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<FilterType>(I, i, j, filter, size);
      }
    }
  }
 
  template<typename FilterType>
  static inline FilterType filterY(const vpImage<unsigned char> &I, unsigned int r, unsigned int c, const FilterType *filter, unsigned int size)
  {
    FilterType result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      result += filter[i] * (I[r + i][c] + I[r - i][c]);
    }
    return result + filter[0] * I[r][c];
  }
 
  static inline double filterYR(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c, const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      result += filter[i] * (I[r + i][c].R + I[r - i][c].R);
    }
    return result + filter[0] * I[r][c].R;
  }
  static inline double filterYG(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c, const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      result += filter[i] * (I[r + i][c].G + I[r - i][c].G);
    }
    return result + filter[0] * I[r][c].G;
  }
 
  static inline double filterYB(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c, const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      result += filter[i] * (I[r + i][c].B + I[r - i][c].B);
    }
    return result + filter[0] * I[r][c].B;
  }
 
  template<typename FilterType>
  static inline FilterType filterYTopBorder(const vpImage<unsigned char> &I, unsigned int r, unsigned int c,
                                            const FilterType *filter, unsigned int size)
  {
    FilterType result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (r > i)
        result += filter[i] * (I[r + i][c] + I[r - i][c]);
      else
        result += filter[i] * (I[r + i][c] + I[i - r][c]);
    }
    return result + filter[0] * I[r][c];
  }
 
  double static inline filterYTopBorderR(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c, const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (r > i)
        result += filter[i] * (I[r + i][c].R + I[r - i][c].R);
      else
        result += filter[i] * (I[r + i][c].R + I[i - r][c].R);
    }
    return result + filter[0] * I[r][c].R;
  }
 
  double static inline filterYTopBorderG(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c, const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (r > i)
        result += filter[i] * (I[r + i][c].G + I[r - i][c].G);
      else
        result += filter[i] * (I[r + i][c].G + I[i - r][c].G);
    }
    return result + filter[0] * I[r][c].G;
  }
 
  double static inline filterYTopBorderB(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c, const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (r > i)
        result += filter[i] * (I[r + i][c].B + I[r - i][c].B);
      else
        result += filter[i] * (I[r + i][c].B + I[i - r][c].B);
    }
    return result + filter[0] * I[r][c].B;
  }
 
  template<typename FilterType>
  static inline FilterType filterYBottomBorder(const vpImage<unsigned char> &I, unsigned int r, unsigned int c,
                                               const FilterType *filter, unsigned int size)
  {
    FilterType result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (r + i < I.getHeight())
        result += filter[i] * (I[r + i][c] + I[r - i][c]);
      else
        result += filter[i] * (I[2 * I.getHeight() - r - i - 1][c] + I[r - i][c]);
    }
    return result + filter[0] * I[r][c];
  }
 
  double static inline filterYBottomBorderR(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c,
                                            const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (r + i < I.getHeight())
        result += filter[i] * (I[r + i][c].R + I[r - i][c].R);
      else
        result += filter[i] * (I[2 * I.getHeight() - r - i - 1][c].R + I[r - i][c].R);
    }
    return result + filter[0] * I[r][c].R;
  }
 
  double static inline filterYBottomBorderG(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c,
                                            const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (r + i < I.getHeight())
        result += filter[i] * (I[r + i][c].G + I[r - i][c].G);
      else
        result += filter[i] * (I[2 * I.getHeight() - r - i - 1][c].G + I[r - i][c].G);
    }
    return result + filter[0] * I[r][c].G;
  }
 
  double static inline filterYBottomBorderB(const vpImage<vpRGBa> &I, unsigned int r, unsigned int c,
                                            const double *filter, unsigned int size)
  {
    double result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (r + i < I.getHeight())
        result += filter[i] * (I[r + i][c].B + I[r - i][c].B);
      else
        result += filter[i] * (I[2 * I.getHeight() - r - i - 1][c].B + I[r - i][c].B);
    }
    return result + filter[0] * I[r][c].B;
  }
 
  template<typename FilterType>
  static inline FilterType filterYTopBorder(const vpImage<double> &I, unsigned int r, unsigned int c,
                                            const FilterType *filter, unsigned int size)
  {
    FilterType result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (r > i)
        result += filter[i] * (I[r + i][c] + I[r - i][c]);
      else
        result += filter[i] * (I[r + i][c] + I[i - r][c]);
    }
    return result + filter[0] * I[r][c];
  }
 
  template<typename FilterType>
  static inline FilterType filterYBottomBorder(const vpImage<double> &I, unsigned int r, unsigned int c,
                                               const FilterType *filter, unsigned int size)
  {
    FilterType result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      if (r + i < I.getHeight())
        result += filter[i] * (I[r + i][c] + I[r - i][c]);
      else
        result += filter[i] * (I[2 * I.getHeight() - r - i - 1][c] + I[r - i][c]);
    }
    return result + filter[0] * I[r][c];
  }
 
  template<typename FilterType>
  static inline FilterType filterY(const vpImage<double> &I, unsigned int r, unsigned int c,
                                   const FilterType *filter, unsigned int size)
  {
    FilterType result;
 
    result = 0;
 
    for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
      result += filter[i] * (I[r + i][c] + I[r - i][c]);
    }
    return result + filter[0] * I[r][c];
  }
 
  template <typename FilterType>
  static void gaussianBlur(const vpImage<unsigned char> &I, vpImage<FilterType> &GI, unsigned int size = 7, FilterType sigma = 0., bool normalize = true)
  {
    FilterType *fg = new FilterType[(size + 1) / 2];
    vpImageFilter::getGaussianKernel<FilterType>(fg, size, sigma, normalize);
    vpImage<FilterType> GIx;
    vpImageFilter::filterX<FilterType>(I, GIx, fg, size);
    vpImageFilter::filterY<FilterType>(GIx, GI, fg, size);
    GIx.destroy();
    delete[] fg;
  }
 
  static void gaussianBlur(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &GI, unsigned int size = 7, double sigma = 0., bool normalize = true);
 
  template <typename FilterType>
  static void gaussianBlur(const vpImage<FilterType> &I, vpImage<FilterType> &GI, unsigned int size = 7, FilterType sigma = 0., bool normalize = true)
  {
    FilterType *fg = new FilterType[(size + 1) / 2];
    vpImageFilter::getGaussianKernel<FilterType>(fg, size, sigma, normalize);
    vpImage<FilterType> GIx;
    vpImageFilter::filterX<FilterType>(I, GIx, fg, size);
    vpImageFilter::filterY<FilterType>(GIx, GI, fg, size);
    GIx.destroy();
    delete[] fg;
  }
 
  template <class T> static double gaussianFilter(const vpImage<T> &fr, unsigned int r, unsigned int c)
  {
    return (15.0 * fr[r][c] + 12.0 * (fr[r - 1][c] + fr[r][c - 1] + fr[r + 1][c] + fr[r][c + 1]) +
            9.0 * (fr[r - 1][c - 1] + fr[r + 1][c - 1] + fr[r - 1][c + 1] + fr[r + 1][c + 1]) +
            5.0 * (fr[r - 2][c] + fr[r][c - 2] + fr[r + 2][c] + fr[r][c + 2]) +
            4.0 * (fr[r - 2][c + 1] + fr[r - 2][c - 1] + fr[r - 1][c - 2] + fr[r + 1][c - 2] + fr[r + 2][c - 1] +
                   fr[r + 2][c + 1] + fr[r - 1][c + 2] + fr[r + 1][c + 2]) +
            2.0 * (fr[r - 2][c - 2] + fr[r + 2][c - 2] + fr[r - 2][c + 2] + fr[r + 2][c + 2])) / 159.0;
  }
  // Gaussian pyramid operation
  static void getGaussPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char> &GI);
  static void getGaussXPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char> &GI);
  static void getGaussYPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char> &GI);
 
  template<typename FilterType>
  static void getGaussianKernel(FilterType *filter, unsigned int size, FilterType sigma = 0., bool normalize = true)
  {
    if (size % 2 != 1)
      throw(vpImageException(vpImageException::incorrectInitializationError, "Bad Gaussian filter size"));
 
    if (sigma <= 0)
      sigma = static_cast<FilterType>((size - 1) / 6.0);
 
    int middle = (int)(size - 1) / 2;
    FilterType sigma2 = static_cast<FilterType>(vpMath::sqr(sigma));
    FilterType coef1 = static_cast<FilterType>(1. / (sigma * sqrt(2. * M_PI)));
    FilterType _2_sigma2 = static_cast<FilterType>(2. * sigma2);
    for (int i = 0; i <= middle; i++) {
      filter[i] = coef1 * exp(-(i * i) / _2_sigma2);
    }
    if (normalize) {
      // renormalization
      FilterType 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;
      }
    }
  }
 
  template <typename FilterType>
  static void getGaussianDerivativeKernel(FilterType *filter, unsigned int size, FilterType sigma = 0., bool normalize = true)
  {
    if (size % 2 != 1)
      throw(vpImageException(vpImageException::incorrectInitializationError, "Bad Gaussian filter size"));
 
    if (sigma <= 0)
      sigma = static_cast<FilterType>((size - 1) / 6.0);
 
    int middle = (int)(size - 1) / 2;
    FilterType sigma2 = static_cast<FilterType>(vpMath::sqr(sigma));
    FilterType coef_1 = static_cast<FilterType>(1. / (sigma * sqrt(2. * M_PI)));
    FilterType coef_1_over_2 = coef_1 / static_cast<FilterType>(2.);
    FilterType _2_coef_1 = static_cast<FilterType>(2.) * coef_1;
    FilterType _2_sigma2 = static_cast<FilterType>(2. * sigma2);
    filter[0] = 0.;
    for (int i = 1; i <= middle; i++) {
      filter[i] = -coef_1_over_2 * (static_cast<FilterType>(exp(-((i + 1) * (i + 1)) / _2_sigma2)) - static_cast<FilterType>(exp(-((i - 1) * (i - 1)) / _2_sigma2)));
    }
 
    if (normalize) {
      FilterType sum = 0;
      for (int i = 1; i <= middle; i++) {
        sum += _2_coef_1 * static_cast<FilterType>(exp(-(i * i) / _2_sigma2));
      }
      sum += coef_1;
 
      for (int i = 1; i <= middle; i++) {
        filter[i] = filter[i] / sum;
      }
    }
  }
 
  // Gradient along X
  template<typename FilterType>
  static void getGradX(const vpImage<unsigned char> &I, vpImage<FilterType> &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;
      }
    }
  }
 
  template <typename ImageType, typename FilterType>
  static void getGradX(const vpImage<ImageType> &I, vpImage<FilterType> &dIx, const FilterType *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] = 0;
      }
      for (unsigned int j = (size - 1) / 2; j < I.getWidth() - (size - 1) / 2; j++) {
        dIx[i][j] = vpImageFilter::derivativeFilterX<ImageType, FilterType>(I, i, j, filter, size);
      }
      for (unsigned int j = I.getWidth() - (size - 1) / 2; j < I.getWidth(); j++) {
        dIx[i][j] = 0;
      }
    }
  }
 
  template <typename ImageType, typename FilterType>
  static void getGradXGauss2D(const vpImage<ImageType> &I, vpImage<FilterType> &dIx, const FilterType *gaussianKernel,
                              const FilterType *gaussianDerivativeKernel, unsigned int size)
  {
    vpImage<FilterType> GIy;
    vpImageFilter::filterY<FilterType>(I, GIy, gaussianKernel, size);
    vpImageFilter::getGradX<FilterType, FilterType>(GIy, dIx, gaussianDerivativeKernel, size);
  }
 
  // Gradient along Y
  template <typename FilterType>
  static void getGradY(const vpImage<unsigned char> &I, vpImage<FilterType> &dIy)
  {
    dIy.resize(I.getHeight(), I.getWidth());
    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;
      }
    }
  }
 
  template <typename ImageType, typename FilterType>
  static void getGradY(const vpImage<ImageType> &I, vpImage<FilterType> &dIy, const FilterType *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] = 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<ImageType, FilterType>(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;
      }
    }
  }
 
  template <typename ImageType, typename FilterType>
  static void getGradYGauss2D(const vpImage<ImageType> &I, vpImage<FilterType> &dIy, const FilterType *gaussianKernel,
                              const FilterType *gaussianDerivativeKernel, unsigned int size)
  {
    vpImage<FilterType> GIx;
    vpImageFilter::filterX<FilterType>(I, GIx, gaussianKernel, size);
    vpImageFilter::getGradY<FilterType, FilterType>(GIx, dIy, gaussianDerivativeKernel, size);
  }
 
  template <typename FilterType>
  inline static FilterType getSobelKernelX(FilterType *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!");
 
    vpArray2D<FilterType> SobelY(size * 2 + 1, size * 2 + 1);
    FilterType norm = getSobelKernelY<FilterType>(SobelY.data, size);
    memcpy(filter, SobelY.t().data, SobelY.getRows() * SobelY.getCols() * sizeof(FilterType));
    return norm;
  }
 
  template <typename FilterType>
  inline static FilterType getSobelKernelY(FilterType *filter, unsigned int size)
  {
    // Sobel kernel pre-computed for the usual size
    static const FilterType SobelY3x3[9] = { -1.0, -2.0, -1.0, 0.0, 0.0, 0.0, 1.0, 2.0, 1.0 };
    static const FilterType 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 FilterType 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 };
    const vpArray2D<FilterType> 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(FilterType));
      return 1 / 8.0;
    }
    if (kernel_size == 5) {
      memcpy(filter, SobelY5x5, kernel_size * kernel_size * sizeof(FilterType));
      return 1 / 16.0;
    }
    if (kernel_size == 7) {
      memcpy(filter, SobelY7x7, kernel_size * kernel_size * sizeof(FilterType));
      return 1 / 16.0;
    }
 
    vpArray2D<FilterType> sobelY(7, 7);
    memcpy(sobelY.data, SobelY7x7, sobelY.getRows() * sobelY.getCols() * sizeof(FilterType));
    for (unsigned int i = 4; i <= size; i++) {
      sobelY = vpArray2D<FilterType>::conv2(sobelY, smoothingKernel, "full");
    }
 
    memcpy(filter, sobelY.data, sobelY.getRows() * sobelY.getCols() * sizeof(FilterType));
 
    return 1 / 16.0;
  }
 
#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC)
  static float computeCannyThreshold(const cv::Mat &cv_I, const cv::Mat *p_cv_blur, float &lowerThresh);
  static float computeCannyThreshold(const vpImage<unsigned char> &I, float &lowerThresh);
  static float median(const cv::Mat &cv_I);
  static float median(const vpImage<unsigned char> &Isrc);
  static std::vector<float> median(const vpImage<vpRGBa> &Isrc);
#endif
};
 
#endif