testParticleFilter.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 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:
 * Test Particle Filter functionalities.
 */
 
#include <visp3/core/vpConfig.h>
 
#if defined(VISP_HAVE_CATCH2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
#include <limits>
#include <vector>
 
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpParticleFilter.h>
#include <visp3/core/vpUniRand.h>
 
#ifdef VISP_HAVE_DISPLAY
#include <visp3/core/vpImage.h>
#include <visp3/gui/vpDisplayFactory.h>
#endif
 
#define CATCH_CONFIG_RUNNER
#include <catch.hpp>
 
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
 
namespace
{
bool opt_display = false; 
 
class vpParabolaModel
{
public:
  inline vpParabolaModel(const unsigned int &degree, const unsigned int &height, const unsigned int &width)
    : m_degree(degree)
    , m_height(height)
    , m_width(width)
    , m_coeffs(degree + 1, 0.)
  { }
 
  inline vpParabolaModel(const vpColVector &coeffs, const unsigned int &height, const unsigned int &width)
    : m_degree(coeffs.size() - 1)
    , m_height(height)
    , m_width(width)
    , m_coeffs(coeffs)
  { }
 
  inline vpParabolaModel(const vpMatrix &coeffs, const unsigned int &height, const unsigned int &width)
    : m_degree(coeffs.getRows() - 1)
    , m_height(height)
    , m_width(width)
    , m_coeffs(coeffs.getCol(0))
  { }
 
  inline vpParabolaModel(const vpParabolaModel &other)
  {
    *this = other;
  }
 
  inline double eval(const double &u) const
  {
    double normalizedU = u / m_width;
    double v = 0.;
    for (unsigned int i = 0; i <= m_degree; ++i) {
      v += m_coeffs[i] * std::pow(normalizedU, i);
    }
    v *= m_height;
    return v;
  }
 
  inline  vpColVector toVpColVector() const
  {
    return m_coeffs;
  }
 
  inline vpParabolaModel &operator=(const vpParabolaModel &other)
  {
    m_degree = other.m_degree;
    m_height = other.m_height;
    m_width = other.m_width;
    m_coeffs = other.m_coeffs;
    return *this;
  }
 
  static void fillSystem(const unsigned int &degree, const unsigned int &height, const unsigned int&width, const std::vector< vpImagePoint> &pts, vpMatrix &A, vpMatrix &b)
  {
    const unsigned int nbPts = static_cast<unsigned int>(pts.size());
    const unsigned int nbCoeffs = degree + 1;
    std::vector<vpImagePoint> normalizedPts;
    for (const auto &pt: pts) {
      normalizedPts.push_back(vpImagePoint(pt.get_i() / height, pt.get_j() / width));
    }
    A.resize(nbPts, nbCoeffs, 1.);
    b.resize(nbPts, 1);
    for (unsigned int i = 0; i < nbPts; ++i) {
      double u = normalizedPts[i].get_u();
      double v = normalizedPts[i].get_v();
      for (unsigned int j = 0; j < nbCoeffs; ++j) {
        A[i][j] = std::pow(u, j);
      }
      b[i][0] = v;
    }
  }
 
#ifdef VISP_HAVE_DISPLAY
  template<typename T>
  void display(const vpImage<T> &I, const vpColor &color, const std::string &legend,
               const unsigned int &vertPosLegend, const unsigned int &horPosLegend)
  {
    unsigned int width = I.getWidth();
    for (unsigned int u = 0; u < width; ++u) {
      int v = static_cast<int>(eval(u));
      vpDisplay::displayPoint(I, v, u, color, 1);
      vpDisplay::displayText(I, vertPosLegend, horPosLegend, legend, color);
    }
  }
#endif
 
private:
  unsigned int m_degree; 
  unsigned int m_height; 
  unsigned int m_width; 
  vpColVector m_coeffs; 
};
 
vpColVector computeABC(const double &x0, const double &y0, const double &x1, const double &y1)
{
  double b = (y1 - y0)/(-0.5*(x1 * x1/x0) + x1 -0.5 * x0);
  double a = -b / (2. * x0);
  double c = y0 - 0.5 * b * x0;
  return vpColVector({ c, b, a });
}
 
vpColVector computeABCD(const double &x0, const double &y0, const double &x1, const double &y1)
{
  double factorA = -2. / (3. * (x1 + x0));
  double factorC = -1. * ((-2. * std::pow(x0, 2))/(x1 + x0) + 2 * x0);
  double b = (y1 - y0)/(factorA * (std::pow(x1, 3) - std::pow(x0, 3)) + (std::pow(x1, 2) - std::pow(x0, 2)) + (x1 - x0) * factorC);
  double a = factorA * b;
  double c = factorC * b;
  double d = y0-(a * std::pow(x0, 3) + b * std::pow(x0, 2) + c * x0);
  return vpColVector({ d, c, b, a });
}
 
double computeY(const double &x, const vpColVector &coeffs)
{
  double y = 0.;
  unsigned int nbCoeffs = coeffs.size();
  for (unsigned int i = 0; i < nbCoeffs; ++i) {
    y += coeffs[i] * std::pow(x, i);
  }
  return y;
}
 
std::vector<vpImagePoint> generateSimulatedImage(const double &xmin, const double &xmax, const double &step, const vpColVector &coeffs)
{
  std::vector<vpImagePoint> points;
  for (double x = xmin; x <= xmax; x += step) {
    double y = computeY(x, coeffs);
    vpImagePoint pt(y, x);
    points.push_back(pt);
  }
  return points;
}
 
#ifdef VISP_HAVE_DISPLAY
template<typename T>
void displayGeneratedImage(const vpImage<T> &I, const std::vector<vpImagePoint> &pts, const vpColor &color,
                           const std::string &legend, const unsigned int &vertOffset, const unsigned int &horOffset)
{
  unsigned int nbPts = static_cast<unsigned int>(pts.size());
  for (unsigned int i = 1; i < nbPts; ++i) {
    vpDisplay::displayPoint(I, pts[i], color, 1);
  }
  vpDisplay::displayText(I, vertOffset, horOffset, legend, color);
}
#endif
 
vpParabolaModel computeInitialGuess(const std::vector<vpImagePoint> &pts, const unsigned int &degree, const unsigned int&height, const unsigned int &width)
{
  vpMatrix A; // The matrix that contains the u^2, u and 1s
  vpMatrix X; // The matrix we want to estimate, that contains the a, b and c coefficients.
  vpMatrix b; // The matrix that contains the v values
 
  // Fill the matrices that form the system we want to solve
  vpParabolaModel::fillSystem(degree, height, width, pts, A, b);
 
  // Compute the parabola coefficients using the least-mean-square method.
  X = A.pseudoInverse() * b;
  vpParabolaModel model(X, height, width);
  return model;
}
 
double evaluate(const std::vector<vpImagePoint> &pts, const vpParabolaModel &model)
{
  double rmse = 0.;
  size_t sizePts = pts.size();
  for (size_t i = 0; i < sizePts; ++i) {
    const vpImagePoint &pt = pts[i];
    double u = pt.get_u();
    double v = pt.get_v();
    double v_model = model.eval(u);
    double error = v - v_model;
    double squareError = error * error;
    rmse += squareError;
  }
  rmse = std::sqrt(rmse / static_cast<double>(pts.size()));
  return rmse;
}
 
vpColVector fx(const vpColVector &coeffs, const double &/*dt*/)
{
  vpColVector updatedCoeffs = coeffs;
  return updatedCoeffs;
}
 
class vpAverageFunctor
{
public:
  vpAverageFunctor(const unsigned int &degree, const unsigned int &height, const unsigned int &width)
    : m_degree(degree)
    , m_height(height)
    , m_width(width)
  { }
 
  vpColVector averagePolynomials(const std::vector<vpColVector> &particles, const std::vector<double> &weights, const vpParticleFilter<std::vector<vpImagePoint>>::vpStateAddFunction &)
  {
    const unsigned int nbParticles = static_cast<unsigned int>(particles.size());
    const double nbParticlesAsDOuble = static_cast<double>(nbParticles);
    const double sumWeight = std::accumulate(weights.begin(), weights.end(), 0.);
    const double nbPointsForAverage = 10. * nbParticlesAsDOuble;
    std::vector<vpImagePoint> initPoints;
    for (unsigned int i = 0; i < nbParticles; ++i) {
      double nbPoints = std::floor(weights[i] * nbPointsForAverage / sumWeight);
      if (nbPoints > 1.) {
        vpParabolaModel curve(particles[i], m_height, m_width);
        double widthAsDouble = static_cast<double>(m_width);
        double step = widthAsDouble / (nbPoints - 1.);
        for (double u = 0.; u < widthAsDouble; u += step) {
          double v = curve.eval(u);
          vpImagePoint pt(v, u);
          initPoints.push_back(pt);
        }
      }
      else if (nbPoints == 1.) {
        vpParabolaModel curve(particles[i], m_height, m_width);
        double u = static_cast<double>(m_width) / 2.;
        double v = curve.eval(u);
        vpImagePoint pt(v, u);
        initPoints.push_back(pt);
      }
    }
    vpMatrix A, X, b;
    vpParabolaModel::fillSystem(m_degree, m_height, m_width, initPoints, A, b);
    X = A.pseudoInverse() * b;
    return vpParabolaModel(X, m_height, m_width).toVpColVector();
  }
 
private:
  unsigned int m_degree; 
  unsigned int m_height; 
  unsigned int m_width; 
};
 
class vpLikelihoodFunctor
{
public:
  vpLikelihoodFunctor(const double &stdev, const unsigned int &height, const unsigned int &width)
    : m_height(height)
    , m_width(width)
  {
    double sigmaDistanceSquared = stdev * stdev;
    m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared);
    m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared);
  }
 
 
  double likelihood(const vpColVector &coeffs, const std::vector<vpImagePoint> &meas)
  {
    double likelihood = 0.;
    unsigned int nbPoints = static_cast<unsigned int>(meas.size());
    vpParabolaModel model(coeffs, m_height, m_width);
    vpColVector residuals(nbPoints);
    double rmse = evaluate(meas, model);
    likelihood = std::exp(rmse * m_constantExpDenominator) * m_constantDenominator;
    likelihood = std::min(likelihood, 1.0); // Clamp to have likelihood <= 1.
    likelihood = std::max(likelihood, 0.); // Clamp to have likelihood >= 0.
    return likelihood;
  }
private:
  double m_constantDenominator; 
  double m_constantExpDenominator; 
  unsigned int m_height; 
  unsigned int m_width; 
};
}
 
TEST_CASE("2nd-degree", "[vpParticleFilter][Polynomial interpolation]")
{
  const unsigned int width = 600; 
  const unsigned int height = 400; 
  const unsigned int degree = 2; 
  const unsigned int nbInitPoints = 10; 
  const uint64_t seedCurve = 4224; 
  const uint64_t seedInitPoints = 2112; 
  const unsigned int nbTestRepet = 10; 
  const unsigned int nbWarmUpIter = 10; 
  const unsigned int nbEvalIter = 20; 
  const double dt = 0.040; 
  const int32_t seedShuffle = 4221; 
 
  // The maximum amplitude for the likelihood compute.
  // A particle whose "distance" with the measurements is greater than this value has a likelihood of 0
  const double ampliMaxLikelihood = 16.;
  const double sigmaLikelihood = ampliMaxLikelihood / 3.; //:< The corresponding standard deviation
  const unsigned int nbParticles = 300; 
  const double ratioAmpliMax(0.25); 
  const long seedPF = 4221; 
  const int nbThreads = 1; //<! Number of threads to use for the PF
  vpUniRand rngCurvePoints(seedCurve);
  vpUniRand rngInitPoints(seedInitPoints);
 
  SECTION("Noise-free", "The init points are directly extracted from the curve points, without any additional noise")
  {
    const double maxToleratedError = 5.;
    double x0 = rngCurvePoints.uniform(0., width);
    double x1 = rngCurvePoints.uniform(0., width);
    double y0 = rngCurvePoints.uniform(0., height);
    double y1 = rngCurvePoints.uniform(0., height);
    vpColVector coeffs = computeABC(x0, y0, x1, y1);
    std::vector<vpImagePoint> curvePoints = generateSimulatedImage(0, width, 1., coeffs);
 
#ifdef VISP_HAVE_DISPLAY
    vpImage<vpRGBa> I(height, width);
    std::shared_ptr<vpDisplay> pDisplay;
    if (opt_display) {
      pDisplay = vpDisplayFactory::createDisplay(I);
    }
#endif
 
    for (unsigned int iter = 0; iter < nbTestRepet; ++iter) {
      // Randomly select the initialization points
      std::vector<vpImagePoint> suffledVector = vpUniRand::shuffleVector(curvePoints, seedShuffle);
      std::vector<vpImagePoint> initPoints;
      for (unsigned int j = 0; j < nbInitPoints; ++j) {
        initPoints.push_back(suffledVector[j]);
      }
 
      // Compute the initial model
      vpParabolaModel modelInitial = computeInitialGuess(initPoints, degree, height, width);
      vpColVector X0 = modelInitial.toVpColVector();
 
      // Initialize the Particle Filter
      std::vector<double> stdevsPF;
      for (unsigned int i = 0; i < degree + 1; ++i) {
        stdevsPF.push_back(ratioAmpliMax * X0[0] / 3.);
      }
      vpParticleFilter<std::vector<vpImagePoint>>::vpProcessFunction processFunc = fx;
      vpLikelihoodFunctor likelihoodFtor(sigmaLikelihood, height, width);
      using std::placeholders::_1;
      using std::placeholders::_2;
      vpParticleFilter<std::vector<vpImagePoint>>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2);
      vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleResamplingCheck;
      vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingFunction resamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleImportanceResampling;
      vpAverageFunctor averageCpter(degree, height, width);
      using std::placeholders::_3;
      vpParticleFilter<std::vector<vpImagePoint>>::vpFilterFunction meanFunc = std::bind(&vpAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3);
      vpParticleFilter<std::vector<vpImagePoint>> filter(nbParticles, stdevsPF, seedPF, nbThreads);
      filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc);
 
      for (unsigned int i = 0; i < nbWarmUpIter; ++i) {
        filter.filter(curvePoints, dt);
      }
 
      double meanError = 0.;
      for (unsigned int i = 0; i < nbEvalIter; ++i) {
        filter.filter(curvePoints, dt);
        vpColVector Xest = filter.computeFilteredState();
        vpParabolaModel model(Xest, height, width);
        double rmse = evaluate(curvePoints, model);
        meanError += rmse;
 
#ifdef VISP_HAVE_DISPLAY
        if (opt_display) {
          vpDisplay::display(I);
          displayGeneratedImage(I, curvePoints, vpColor::red, "GT", 20, 20);
          model.display(I, vpColor::blue, "Model", 40, 20);
          vpDisplay::flush(I);
          vpDisplay::getClick(I);
        }
#endif
      }
      meanError /= static_cast<double>(nbEvalIter);
      std::cout << "Mean(rmse) = " << meanError << std::endl;
      CHECK(meanError <= maxToleratedError);
    }
  }
 
  SECTION("Noisy", "Noise is added to the init points")
  {
    const double maxToleratedError = 12.;
    double x0 = rngCurvePoints.uniform(0., width);
    double x1 = rngCurvePoints.uniform(0., width);
    double y0 = rngCurvePoints.uniform(0., height);
    double y1 = rngCurvePoints.uniform(0., height);
    vpColVector coeffs = computeABC(x0, y0, x1, y1);
    std::vector<vpImagePoint> curvePoints = generateSimulatedImage(0, width, 1., coeffs);
 
#ifdef VISP_HAVE_DISPLAY
    vpImage<vpRGBa> I(height, width);
    std::shared_ptr<vpDisplay> pDisplay;
    if (opt_display) {
      pDisplay = vpDisplayFactory::createDisplay(I);
    }
#endif
 
    const double ampliMaxInitNoise = 24.;
    const double stdevInitNoise = ampliMaxInitNoise / 3.;
    vpGaussRand rngInitNoise(stdevInitNoise, 0., seedInitPoints);
 
    for (unsigned int iter = 0; iter < nbTestRepet; ++iter) {
      // Randomly select the initialization points
      std::vector<vpImagePoint> suffledVector = vpUniRand::shuffleVector(curvePoints, seedShuffle);
      std::vector<vpImagePoint> initPoints;
      for (unsigned int j = 0; j < nbInitPoints; ++j) {
        vpImagePoint noisyPt(suffledVector[j].get_i() + rngInitNoise(), suffledVector[j].get_j() + rngInitNoise());
        initPoints.push_back(noisyPt);
      }
 
      // Compute the initial model
      vpParabolaModel modelInitial = computeInitialGuess(initPoints, degree, height, width);
      vpColVector X0 = modelInitial.toVpColVector();
 
      // Initialize the Particle Filter
      std::vector<double> stdevsPF;
      for (unsigned int i = 0; i < degree + 1; ++i) {
        stdevsPF.push_back(ratioAmpliMax * X0[0] / 3.);
      }
      vpParticleFilter<std::vector<vpImagePoint>>::vpProcessFunction processFunc = fx;
      vpLikelihoodFunctor likelihoodFtor(sigmaLikelihood, height, width);
      using std::placeholders::_1;
      using std::placeholders::_2;
      vpParticleFilter<std::vector<vpImagePoint>>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2);
      vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleResamplingCheck;
      vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingFunction resamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleImportanceResampling;
      vpAverageFunctor averageCpter(degree, height, width);
      using std::placeholders::_3;
      vpParticleFilter<std::vector<vpImagePoint>>::vpFilterFunction meanFunc = std::bind(&vpAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3);
      vpParticleFilter<std::vector<vpImagePoint>> filter(nbParticles, stdevsPF, seedPF, nbThreads);
      filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc);
 
      for (unsigned int i = 0; i < nbWarmUpIter; ++i) {
        filter.filter(curvePoints, dt);
      }
 
      double meanError = 0.;
      for (unsigned int i = 0; i < nbEvalIter; ++i) {
        filter.filter(curvePoints, dt);
        vpColVector Xest = filter.computeFilteredState();
        vpParabolaModel model(Xest, height, width);
        double rmse = evaluate(curvePoints, model);
        meanError += rmse;
 
#ifdef VISP_HAVE_DISPLAY
        if (opt_display) {
          vpDisplay::display(I);
          displayGeneratedImage(I, curvePoints, vpColor::red, "GT", 20, 20);
          model.display(I, vpColor::blue, "Model", 40, 20);
          vpDisplay::flush(I);
          vpDisplay::getClick(I);
        }
#endif
      }
      meanError /= static_cast<double>(nbEvalIter);
      std::cout << "Mean(rmse) = " << meanError << std::endl;
      CHECK(meanError <= maxToleratedError);
    }
  }
}
 
TEST_CASE("3rd-degree", "[vpParticleFilter][Polynomial interpolation]")
{
  const unsigned int width = 600; 
  const unsigned int height = 400; 
  const unsigned int degree = 3; 
  const unsigned int nbInitPoints = 10; 
  const uint64_t seedCurve = 4224; 
  const uint64_t seedInitPoints = 2112; 
  const unsigned int nbTestRepet = 10; 
  const unsigned int nbWarmUpIter = 10; 
  const unsigned int nbEvalIter = 20; 
  const double dt = 0.040; 
  const int32_t seedShuffle = 4221; 
 
  // The maximum amplitude for the likelihood compute.
  // A particle whose "distance" with the measurements is greater than this value has a likelihood of 0
  const double ampliMaxLikelihood = 6.;
  const double sigmaLikelihood = ampliMaxLikelihood / 3.; //:< The corresponding standard deviation
  const unsigned int nbParticles = 300; 
  const double ratioAmpliMax(0.21); 
  const long seedPF = 4221; 
  const int nbThreads = 1; //<! Number of threads to use for the PF
  vpUniRand rngCurvePoints(seedCurve);
  vpUniRand rngInitPoints(seedInitPoints);
 
  SECTION("Noise-free", "The init points are directly extracted from the curve points, without any additional noise")
  {
    const double maxToleratedError = 10.;
    double x0 = rngCurvePoints.uniform(0., width);
    double x1 = rngCurvePoints.uniform(0., width);
    double y0 = rngCurvePoints.uniform(0., height);
    double y1 = rngCurvePoints.uniform(0., height);
    vpColVector coeffs = computeABCD(x0, y0, x1, y1);
    std::vector<vpImagePoint> curvePoints = generateSimulatedImage(0, width, 1., coeffs);
 
#ifdef VISP_HAVE_DISPLAY
    vpImage<vpRGBa> I(height, width);
    std::shared_ptr<vpDisplay> pDisplay;
    if (opt_display) {
      pDisplay = vpDisplayFactory::createDisplay(I);
    }
#endif
 
    for (unsigned int iter = 0; iter < nbTestRepet; ++iter) {
      // Randomly select the initialization points
      std::vector<vpImagePoint> suffledVector = vpUniRand::shuffleVector(curvePoints, seedShuffle);
      std::vector<vpImagePoint> initPoints;
      for (unsigned int j = 0; j < nbInitPoints; ++j) {
        initPoints.push_back(suffledVector[j]);
      }
 
      // Compute the initial model
      vpParabolaModel modelInitial = computeInitialGuess(initPoints, degree, height, width);
      vpColVector X0 = modelInitial.toVpColVector();
 
      // Initialize the Particle Filter
      std::vector<double> stdevsPF;
      for (unsigned int i = 0; i < degree + 1; ++i) {
        stdevsPF.push_back(ratioAmpliMax * std::pow(0.1, i) * X0[0] / 3.);
      }
      vpParticleFilter<std::vector<vpImagePoint>>::vpProcessFunction processFunc = fx;
      vpLikelihoodFunctor likelihoodFtor(sigmaLikelihood, height, width);
      using std::placeholders::_1;
      using std::placeholders::_2;
      vpParticleFilter<std::vector<vpImagePoint>>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2);
      vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleResamplingCheck;
      vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingFunction resamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleImportanceResampling;
      vpAverageFunctor averageCpter(degree, height, width);
      using std::placeholders::_3;
      vpParticleFilter<std::vector<vpImagePoint>>::vpFilterFunction meanFunc = std::bind(&vpAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3);
      vpParticleFilter<std::vector<vpImagePoint>> filter(nbParticles, stdevsPF, seedPF, nbThreads);
      filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc);
 
      for (unsigned int i = 0; i < nbWarmUpIter; ++i) {
        filter.filter(curvePoints, dt);
      }
 
      double meanError = 0.;
      for (unsigned int i = 0; i < nbEvalIter; ++i) {
        filter.filter(curvePoints, dt);
        vpColVector Xest = filter.computeFilteredState();
        vpParabolaModel model(Xest, height, width);
        double rmse = evaluate(curvePoints, model);
        meanError += rmse;
 
#ifdef VISP_HAVE_DISPLAY
        if (opt_display) {
          vpDisplay::display(I);
          displayGeneratedImage(I, curvePoints, vpColor::red, "GT", 20, 20);
          model.display(I, vpColor::blue, "Model", 40, 20);
          vpDisplay::flush(I);
          vpDisplay::getClick(I);
        }
#endif
      }
      meanError /= static_cast<double>(nbEvalIter);
      std::cout << "Mean(rmse) = " << meanError << std::endl;
      CHECK(meanError <= maxToleratedError);
    }
  }
 
 
 
  SECTION("Noisy", "Noise is added to the init points")
  {
    const double maxToleratedError = 17.;
    double x0 = rngCurvePoints.uniform(0., width);
    double x1 = rngCurvePoints.uniform(0., width);
    double y0 = rngCurvePoints.uniform(0., height);
    double y1 = rngCurvePoints.uniform(0., height);
    vpColVector coeffs = computeABCD(x0, y0, x1, y1);
    std::vector<vpImagePoint> curvePoints = generateSimulatedImage(0, width, 1., coeffs);
 
#ifdef VISP_HAVE_DISPLAY
    vpImage<vpRGBa> I(height, width);
    std::shared_ptr<vpDisplay> pDisplay;
    if (opt_display) {
      pDisplay = vpDisplayFactory::createDisplay(I);
    }
#endif
 
    const double ampliMaxInitNoise = 1.5;
    const double stdevInitNoise = ampliMaxInitNoise / 3.;
    vpGaussRand rngInitNoise(stdevInitNoise, 0., seedInitPoints);
 
    for (unsigned int iter = 0; iter < nbTestRepet; ++iter) {
      // Randomly select the initialization points
      std::vector<vpImagePoint> suffledVector = vpUniRand::shuffleVector(curvePoints, seedShuffle);
      std::vector<vpImagePoint> initPoints;
      for (unsigned int j = 0; j < nbInitPoints * 4; ++j) {
        vpImagePoint noisyPt(suffledVector[j].get_i() + rngInitNoise(), suffledVector[j].get_j() + rngInitNoise());
        initPoints.push_back(noisyPt);
      }
 
      // Compute the initial model
      vpParabolaModel modelInitial = computeInitialGuess(initPoints, degree, height, width);
      vpColVector X0 = modelInitial.toVpColVector();
 
      // Initialize the Particle Filter
      std::vector<double> stdevsPF;
      for (unsigned int i = 0; i < degree + 1; ++i) {
        stdevsPF.push_back(ratioAmpliMax * std::pow(.05, i) * X0[0] / 3.);
      }
      vpParticleFilter<std::vector<vpImagePoint>>::vpProcessFunction processFunc = fx;
      vpLikelihoodFunctor likelihoodFtor(sigmaLikelihood * 2., height, width);
      using std::placeholders::_1;
      using std::placeholders::_2;
      vpParticleFilter<std::vector<vpImagePoint>>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2);
      vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleResamplingCheck;
      vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingFunction resamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleImportanceResampling;
      vpAverageFunctor averageCpter(degree, height, width);
      using std::placeholders::_3;
      vpParticleFilter<std::vector<vpImagePoint>>::vpFilterFunction meanFunc = std::bind(&vpAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3);
      vpParticleFilter<std::vector<vpImagePoint>> filter(nbParticles, stdevsPF, seedPF, nbThreads);
      filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc);
 
      for (unsigned int i = 0; i < nbWarmUpIter * 5; ++i) {
        filter.filter(curvePoints, dt);
      }
 
      double meanError = 0.;
      for (unsigned int i = 0; i < nbEvalIter; ++i) {
        filter.filter(curvePoints, dt);
        vpColVector Xest = filter.computeFilteredState();
        vpParabolaModel model(Xest, height, width);
        double rmse = evaluate(curvePoints, model);
        meanError += rmse;
 
#ifdef VISP_HAVE_DISPLAY
        if (opt_display) {
          vpDisplay::display(I);
          displayGeneratedImage(I, curvePoints, vpColor::red, "GT", 20, 20);
          model.display(I, vpColor::blue, "Model", 40, 20);
          vpDisplay::flush(I);
          vpDisplay::getClick(I);
        }
#endif
      }
      meanError /= static_cast<double>(nbEvalIter);
      std::cout << "Mean(rmse) = " << meanError << std::endl;
      CHECK(meanError <= maxToleratedError);
    }
  }
}
 
int main(int argc, char *argv[])
{
  Catch::Session session; // There must be exactly one instance
 
  // Build a new parser on top of Catch's
  using namespace Catch::clara;
  auto cli = session.cli()         // Get Catch's composite command line parser
    | Opt(opt_display)   // bind variable to a new option, with a hint string
    ["--display"] // the option names it will respond to
    ("Activate debug display"); // description string for the help output
 
  // Now pass the new composite back to Catch so it uses that
  session.cli(cli);
 
  // Let Catch (using Clara) parse the command line
  session.applyCommandLine(argc, argv);
 
  int numFailed = session.run();
 
  // numFailed is clamped to 255 as some unices only use the lower 8 bits.
  // This clamping has already been applied, so just return it here
  // You can also do any post run clean-up here
  return numFailed;
}
 
#else
#include <iostream>
 
int main() { return EXIT_SUCCESS; }
#endif