Visual Servoing Platform version 3.7.0
Loading...
Searching...
No Matches
catchParticleFilter.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2025 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Test Particle Filter functionalities.
32 */
33
40#include <visp3/core/vpConfig.h>
41
42#if defined(VISP_HAVE_CATCH2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
43#include <limits>
44#include <vector>
45
46#include <visp3/core/vpColVector.h>
47#include <visp3/core/vpImagePoint.h>
48#include <visp3/core/vpMath.h>
49#include <visp3/core/vpParticleFilter.h>
50#include <visp3/core/vpUniRand.h>
51
52#ifdef VISP_HAVE_DISPLAY
53#include <visp3/core/vpImage.h>
54#include <visp3/gui/vpDisplayFactory.h>
55#endif
56
57#include <catch_amalgamated.hpp>
58
59#ifdef ENABLE_VISP_NAMESPACE
60using namespace VISP_NAMESPACE_NAME;
61#endif
62
63namespace
64{
65bool opt_display = false;
66
71class vpParabolaModel
72{
73public:
74 inline vpParabolaModel(const unsigned int &degree, const unsigned int &height, const unsigned int &width)
75 : m_degree(degree)
76 , m_height(height)
77 , m_width(width)
78 , m_coeffs(degree + 1, 0.)
79 { }
80
89 inline vpParabolaModel(const vpColVector &coeffs, const unsigned int &height, const unsigned int &width)
90 : m_degree(coeffs.size() - 1)
91 , m_height(height)
92 , m_width(width)
93 , m_coeffs(coeffs)
94 { }
95
104 inline vpParabolaModel(const vpMatrix &coeffs, const unsigned int &height, const unsigned int &width)
105 : m_degree(coeffs.getRows() - 1)
106 , m_height(height)
107 , m_width(width)
108 , m_coeffs(coeffs.getCol(0))
109 { }
110
111 inline vpParabolaModel(const vpParabolaModel &other)
112 {
113 *this = other;
114 }
115
122 inline double eval(const double &u) const
123 {
124 double normalizedU = u / m_width;
125 double v = 0.;
126 for (unsigned int i = 0; i <= m_degree; ++i) {
127 v += m_coeffs[i] * std::pow(normalizedU, i);
128 }
129 v *= m_height;
130 return v;
131 }
132
138 inline vpColVector toVpColVector() const
139 {
140 return m_coeffs;
141 }
142
143 inline vpParabolaModel &operator=(const vpParabolaModel &other)
144 {
145 m_degree = other.m_degree;
146 m_height = other.m_height;
147 m_width = other.m_width;
148 m_coeffs = other.m_coeffs;
149 return *this;
150 }
151
166 static void fillSystem(const unsigned int &degree, const unsigned int &height, const unsigned int &width, const std::vector< vpImagePoint> &pts, vpMatrix &A, vpMatrix &b)
167 {
168 const unsigned int nbPts = static_cast<unsigned int>(pts.size());
169 const unsigned int nbCoeffs = degree + 1;
170 std::vector<vpImagePoint> normalizedPts;
171 for (const auto &pt: pts) {
172 normalizedPts.push_back(vpImagePoint(pt.get_i() / height, pt.get_j() / width));
173 }
174 A.resize(nbPts, nbCoeffs, 1.);
175 b.resize(nbPts, 1);
176 for (unsigned int i = 0; i < nbPts; ++i) {
177 double u = normalizedPts[i].get_u();
178 double v = normalizedPts[i].get_v();
179 for (unsigned int j = 0; j < nbCoeffs; ++j) {
180 A[i][j] = std::pow(u, j);
181 }
182 b[i][0] = v;
183 }
184 }
185
186#ifdef VISP_HAVE_DISPLAY
187
188#if defined(__clang__)
189// Mute warning : '\tparam' command used in a comment that is not attached to a template declaration [-Wdocumentation]
190# pragma clang diagnostic push
191# pragma clang diagnostic ignored "-Wexit-time-destructors"
192#endif
193
201 template<typename T>
202 void display(const vpImage<T> &I, const vpColor &color, const std::string &legend,
203 const unsigned int &vertPosLegend, const unsigned int &horPosLegend)
204 {
205 unsigned int width = I.getWidth();
206 for (unsigned int u = 0; u < width; ++u) {
207 int v = static_cast<int>(eval(u));
208 vpDisplay::displayPoint(I, v, u, color, 1);
209 vpDisplay::displayText(I, vertPosLegend, horPosLegend, legend, color);
210 }
211 }
212
213
214#if defined(__clang__)
215# pragma clang diagnostic pop
216#endif
217#endif
218
219private:
220 unsigned int m_degree;
221 unsigned int m_height;
222 unsigned int m_width;
223 vpColVector m_coeffs;
224};
225
236vpColVector computeABC(const double &x0, const double &y0, const double &x1, const double &y1)
237{
238 double b = (y1 - y0)/(-0.5*(x1 * x1/x0) + x1 -0.5 * x0);
239 double a = -b / (2. * x0);
240 double c = y0 - 0.5 * b * x0;
241 return vpColVector({ c, b, a });
242}
243
254vpColVector computeABCD(const double &x0, const double &y0, const double &x1, const double &y1)
255{
256 double factorA = -2. / (3. * (x1 + x0));
257 double factorC = -1. * ((-2. * std::pow(x0, 2))/(x1 + x0) + 2 * x0);
258 double b = (y1 - y0)/(factorA * (std::pow(x1, 3) - std::pow(x0, 3)) + (std::pow(x1, 2) - std::pow(x0, 2)) + (x1 - x0) * factorC);
259 double a = factorA * b;
260 double c = factorC * b;
261 double d = y0-(a * std::pow(x0, 3) + b * std::pow(x0, 2) + c * x0);
262 return vpColVector({ d, c, b, a });
263}
264
272double computeY(const double &x, const vpColVector &coeffs)
273{
274 double y = 0.;
275 unsigned int nbCoeffs = coeffs.size();
276 for (unsigned int i = 0; i < nbCoeffs; ++i) {
277 y += coeffs[i] * std::pow(x, i);
278 }
279 return y;
280}
281
291std::vector<vpImagePoint> generateSimulatedImage(const double &xmin, const double &xmax, const double &step, const vpColVector &coeffs)
292{
293 std::vector<vpImagePoint> points;
294 for (double x = xmin; x <= xmax; x += step) {
295 double y = computeY(x, coeffs);
296 vpImagePoint pt(y, x);
297 points.push_back(pt);
298 }
299 return points;
300}
301
302#ifdef VISP_HAVE_DISPLAY
303template<typename T>
304void displayGeneratedImage(const vpImage<T> &I, const std::vector<vpImagePoint> &pts, const vpColor &color,
305 const std::string &legend, const unsigned int &vertOffset, const unsigned int &horOffset)
306{
307 unsigned int nbPts = static_cast<unsigned int>(pts.size());
308 for (unsigned int i = 1; i < nbPts; ++i) {
309 vpDisplay::displayPoint(I, pts[i], color, 1);
310 }
311 vpDisplay::displayText(I, vertOffset, horOffset, legend, color);
312}
313#endif
314
324vpParabolaModel computeInitialGuess(const std::vector<vpImagePoint> &pts, const unsigned int &degree, const unsigned int &height, const unsigned int &width)
325{
326 vpMatrix A; // The matrix that contains the u^2, u and 1s
327 vpMatrix X; // The matrix we want to estimate, that contains the a, b and c coefficients.
328 vpMatrix b; // The matrix that contains the v values
329
330 // Fill the matrices that form the system we want to solve
331 vpParabolaModel::fillSystem(degree, height, width, pts, A, b);
332
333 // Compute the parabola coefficients using the least-mean-square method.
334 X = A.pseudoInverse() * b;
335 vpParabolaModel model(X, height, width);
336 return model;
337}
338
346double evaluate(const std::vector<vpImagePoint> &pts, const vpParabolaModel &model)
347{
348 double rmse = 0.;
349 size_t sizePts = pts.size();
350 for (size_t i = 0; i < sizePts; ++i) {
351 const vpImagePoint &pt = pts[i];
352 double u = pt.get_u();
353 double v = pt.get_v();
354 double v_model = model.eval(u);
355 double error = v - v_model;
356 double squareError = error * error;
357 rmse += squareError;
358 }
359 rmse = std::sqrt(rmse / static_cast<double>(pts.size()));
360 return rmse;
361}
362
368vpColVector fx(const vpColVector &coeffs, const double &/*dt*/)
369{
370 vpColVector updatedCoeffs = coeffs;
371 return updatedCoeffs;
372}
373
374class vpAverageFunctor
375{
376public:
377 vpAverageFunctor(const unsigned int &degree, const unsigned int &height, const unsigned int &width)
378 : m_degree(degree)
379 , m_height(height)
380 , m_width(width)
381 { }
382
383 vpColVector averagePolynomials(const std::vector<vpColVector> &particles, const std::vector<double> &weights, const vpParticleFilter<std::vector<vpImagePoint>>::vpStateAddFunction &/**/)
384 {
385 const unsigned int nbParticles = static_cast<unsigned int>(particles.size());
386 const double nbParticlesAsDOuble = static_cast<double>(nbParticles);
387 const double sumWeight = std::accumulate(weights.begin(), weights.end(), 0.);
388 const double nbPointsForAverage = 10. * nbParticlesAsDOuble;
389 std::vector<vpImagePoint> initPoints;
390 for (unsigned int i = 0; i < nbParticles; ++i) {
391 double nbPoints = std::floor(weights[i] * nbPointsForAverage / sumWeight);
392 if (nbPoints > 1.) {
393 vpParabolaModel curve(particles[i], m_height, m_width);
394 double widthAsDouble = static_cast<double>(m_width);
395 double step = widthAsDouble / (nbPoints - 1.);
396 for (double u = 0.; u < widthAsDouble; u += step) {
397 double v = curve.eval(u);
398 vpImagePoint pt(v, u);
399 initPoints.push_back(pt);
400 }
401 }
402 else if (vpMath::equal(nbPoints, 1.)) {
403 vpParabolaModel curve(particles[i], m_height, m_width);
404 double u = static_cast<double>(m_width) / 2.;
405 double v = curve.eval(u);
406 vpImagePoint pt(v, u);
407 initPoints.push_back(pt);
408 }
409 }
410 vpMatrix A, X, b;
411 vpParabolaModel::fillSystem(m_degree, m_height, m_width, initPoints, A, b);
412 X = A.pseudoInverse() * b;
413 return vpParabolaModel(X, m_height, m_width).toVpColVector();
414 }
415
416private:
417 unsigned int m_degree;
418 unsigned int m_height;
419 unsigned int m_width;
420};
421
422class vpLikelihoodFunctor
423{
424public:
432 vpLikelihoodFunctor(const double &stdev, const unsigned int &height, const unsigned int &width)
433 : m_height(height)
434 , m_width(width)
435 {
436 double sigmaDistanceSquared = stdev * stdev;
437 m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared);
438 m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared);
439 }
440
442
454 double likelihood(const vpColVector &coeffs, const std::vector<vpImagePoint> &meas)
455 {
456 double likelihood = 0.;
457 unsigned int nbPoints = static_cast<unsigned int>(meas.size());
458 vpParabolaModel model(coeffs, m_height, m_width);
459 vpColVector residuals(nbPoints);
460 double rmse = evaluate(meas, model);
461 likelihood = std::exp(rmse * m_constantExpDenominator) * m_constantDenominator;
462 likelihood = std::min(likelihood, 1.0); // Clamp to have likelihood <= 1.
463 likelihood = std::max(likelihood, 0.); // Clamp to have likelihood >= 0.
464 return likelihood;
465 }
467private:
468 double m_constantDenominator;
469 double m_constantExpDenominator;
470 unsigned int m_height;
471 unsigned int m_width;
472};
473}
474
475TEST_CASE("2nd-degree", "[vpParticleFilter][Polynomial interpolation]")
476{
478 const unsigned int width = 150;
479 const unsigned int height = 100;
480 const unsigned int degree = 2;
481 const unsigned int nbInitPoints = 10;
482 const uint64_t seedCurve = 4224;
483 const uint64_t seedInitPoints = 2112;
484 const unsigned int nbTestRepet = 5;
485 const unsigned int nbWarmUpIter = 10;
486 const unsigned int nbEvalIter = 10;
487 const double dt = 0.040;
488 const int32_t seedShuffle = 4221;
489
491 // The maximum amplitude for the likelihood compute.
492 // A particle whose "distance" with the measurements is greater than this value has a likelihood of 0
493 const double ampliMaxLikelihood = 16.;
494 const double sigmaLikelihood = ampliMaxLikelihood / 3.; //:< The corresponding standard deviation
495 const unsigned int nbParticles = 200;
496 const double ratioAmpliMax(0.25);
497 const long seedPF = 4221;
498 const int nbThreads = 1;
499 vpUniRand rngCurvePoints(seedCurve);
500 vpUniRand rngInitPoints(seedInitPoints);
501
502 SECTION("Noise-free", "The init points are directly extracted from the curve points, without any additional noise")
503 {
504 const double maxToleratedError = 5.;
505 double x0 = rngCurvePoints.uniform(0., width);
506 double x1 = rngCurvePoints.uniform(0., width);
507 double y0 = rngCurvePoints.uniform(0., height);
508 double y1 = rngCurvePoints.uniform(0., height);
509 vpColVector coeffs = computeABC(x0, y0, x1, y1);
510 std::vector<vpImagePoint> curvePoints = generateSimulatedImage(0, width, 1., coeffs);
511
512#ifdef VISP_HAVE_DISPLAY
513 vpImage<vpRGBa> I(height, width);
514 std::shared_ptr<vpDisplay> pDisplay;
515 if (opt_display) {
516 pDisplay = vpDisplayFactory::createDisplay(I);
517 }
518#endif
519
520 for (unsigned int iter = 0; iter < nbTestRepet; ++iter) {
521 // Randomly select the initialization points
522 std::vector<vpImagePoint> suffledVector = vpUniRand::shuffleVector(curvePoints, seedShuffle);
523 std::vector<vpImagePoint> initPoints;
524 for (unsigned int j = 0; j < nbInitPoints; ++j) {
525 initPoints.push_back(suffledVector[j]);
526 }
527
528 // Compute the initial model
529 vpParabolaModel modelInitial = computeInitialGuess(initPoints, degree, height, width);
530 vpColVector X0 = modelInitial.toVpColVector();
531
532 // Initialize the Particle Filter
533 std::vector<double> stdevsPF;
534 for (unsigned int i = 0; i < degree + 1; ++i) {
535 stdevsPF.push_back(ratioAmpliMax * X0[0] / 3.);
536 }
537 vpParticleFilter<std::vector<vpImagePoint>>::vpProcessFunction processFunc = fx;
538 vpLikelihoodFunctor likelihoodFtor(sigmaLikelihood, height, width);
539 using std::placeholders::_1;
540 using std::placeholders::_2;
541 vpParticleFilter<std::vector<vpImagePoint>>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2);
542 vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleResamplingCheck;
543 vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingFunction resamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleImportanceResampling;
544 vpAverageFunctor averageCpter(degree, height, width);
545 using std::placeholders::_3;
546 vpParticleFilter<std::vector<vpImagePoint>>::vpFilterFunction meanFunc = std::bind(&vpAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3);
547 vpParticleFilter<std::vector<vpImagePoint>> filter(nbParticles, stdevsPF, seedPF, nbThreads);
548 filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc);
549
550 for (unsigned int i = 0; i < nbWarmUpIter; ++i) {
551 filter.filter(curvePoints, dt);
552 }
553
554 double meanError = 0.;
555 for (unsigned int i = 0; i < nbEvalIter; ++i) {
556 filter.filter(curvePoints, dt);
557 vpColVector Xest = filter.computeFilteredState();
558 vpParabolaModel model(Xest, height, width);
559 double rmse = evaluate(curvePoints, model);
560 meanError += rmse;
561
562#ifdef VISP_HAVE_DISPLAY
563 if (opt_display) {
564 vpDisplay::display(I);
565 displayGeneratedImage(I, curvePoints, vpColor::red, "GT", 20, 20);
566 model.display(I, vpColor::blue, "Model", 40, 20);
567 vpDisplay::flush(I);
568 vpDisplay::getClick(I);
569 }
570#endif
571 }
572 meanError /= static_cast<double>(nbEvalIter);
573 std::cout << "[2nd degree][Noise-free] Test " << iter << ": Mean(rmse) = " << meanError << std::endl;
574 CHECK(meanError <= maxToleratedError);
575 }
576 }
577
578 SECTION("Noisy", "Noise is added to the init points")
579 {
580 const double maxToleratedError = 12.;
581 double x0 = rngCurvePoints.uniform(0., width);
582 double x1 = rngCurvePoints.uniform(0., width);
583 double y0 = rngCurvePoints.uniform(0., height);
584 double y1 = rngCurvePoints.uniform(0., height);
585 vpColVector coeffs = computeABC(x0, y0, x1, y1);
586 std::vector<vpImagePoint> curvePoints = generateSimulatedImage(0, width, 1., coeffs);
587
588#ifdef VISP_HAVE_DISPLAY
589 vpImage<vpRGBa> I(height, width);
590 std::shared_ptr<vpDisplay> pDisplay;
591 if (opt_display) {
592 pDisplay = vpDisplayFactory::createDisplay(I);
593 }
594#endif
595
596 const double ampliMaxInitNoise = 18.;
597 const double stdevInitNoise = ampliMaxInitNoise / 3.;
598 vpGaussRand rngInitNoise(stdevInitNoise, 0., seedInitPoints);
599
600 for (unsigned int iter = 0; iter < nbTestRepet; ++iter) {
601 // Randomly select the initialization points
602 std::vector<vpImagePoint> suffledVector = vpUniRand::shuffleVector(curvePoints, seedShuffle);
603 std::vector<vpImagePoint> initPoints;
604 for (unsigned int j = 0; j < nbInitPoints; ++j) {
605 vpImagePoint noisyPt(suffledVector[j].get_i() + rngInitNoise(), suffledVector[j].get_j() + rngInitNoise());
606 initPoints.push_back(noisyPt);
607 }
608
609 // Compute the initial model
610 vpParabolaModel modelInitial = computeInitialGuess(initPoints, degree, height, width);
611 vpColVector X0 = modelInitial.toVpColVector();
612
613 // Initialize the Particle Filter
614 std::vector<double> stdevsPF;
615 for (unsigned int i = 0; i < degree + 1; ++i) {
616 stdevsPF.push_back(ratioAmpliMax * X0[0] / 3.);
617 }
618 vpParticleFilter<std::vector<vpImagePoint>>::vpProcessFunction processFunc = fx;
619 vpLikelihoodFunctor likelihoodFtor(sigmaLikelihood, height, width);
620 using std::placeholders::_1;
621 using std::placeholders::_2;
622 vpParticleFilter<std::vector<vpImagePoint>>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2);
623 vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleResamplingCheck;
624 vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingFunction resamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleImportanceResampling;
625 vpAverageFunctor averageCpter(degree, height, width);
626 using std::placeholders::_3;
627 vpParticleFilter<std::vector<vpImagePoint>>::vpFilterFunction meanFunc = std::bind(&vpAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3);
628 vpParticleFilter<std::vector<vpImagePoint>> filter(nbParticles, stdevsPF, seedPF, nbThreads);
629 filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc);
630
631 for (unsigned int i = 0; i < nbWarmUpIter; ++i) {
632 filter.filter(curvePoints, dt);
633 }
634
635 double meanError = 0.;
636 for (unsigned int i = 0; i < nbEvalIter; ++i) {
637 filter.filter(curvePoints, dt);
638 vpColVector Xest = filter.computeFilteredState();
639 vpParabolaModel model(Xest, height, width);
640 double rmse = evaluate(curvePoints, model);
641 meanError += rmse;
642
643#ifdef VISP_HAVE_DISPLAY
644 if (opt_display) {
645 vpDisplay::display(I);
646 displayGeneratedImage(I, curvePoints, vpColor::red, "GT", 20, 20);
647 model.display(I, vpColor::blue, "Model", 40, 20);
648 vpDisplay::flush(I);
649 vpDisplay::getClick(I);
650 }
651#endif
652 }
653 meanError /= static_cast<double>(nbEvalIter);
654 std::cout << "[2nd degree][Noisy] Test " << iter << ": Mean(rmse) = " << meanError << std::endl;
655 CHECK(meanError <= maxToleratedError);
656 }
657 }
658}
659
660TEST_CASE("3rd-degree", "[vpParticleFilter][Polynomial interpolation]")
661{
663 const unsigned int width = 150;
664 const unsigned int height = 100;
665 const unsigned int degree = 3;
666 const unsigned int nbInitPoints = 10;
667 const uint64_t seedCurve = 4224;
668 const uint64_t seedInitPoints = 2112;
669 const unsigned int nbTestRepet = 5;
670 const unsigned int nbWarmUpIter = 10;
671 const unsigned int nbEvalIter = 10;
672 const double dt = 0.040;
673 const int32_t seedShuffle = 4221;
674
676 // The maximum amplitude for the likelihood compute.
677 // A particle whose "distance" with the measurements is greater than this value has a likelihood of 0
678 const double ampliMaxLikelihood = 6.;
679 const double sigmaLikelihood = ampliMaxLikelihood / 3.; //:< The corresponding standard deviation
680 const unsigned int nbParticles = 200;
681 const double ratioAmpliMax(0.21);
682 const long seedPF = 4221;
683 const int nbThreads = 1;
684 vpUniRand rngCurvePoints(seedCurve);
685 vpUniRand rngInitPoints(seedInitPoints);
686
687 SECTION("Noise-free", "The init points are directly extracted from the curve points, without any additional noise")
688 {
689 const double maxToleratedError = 10.;
690 double x0 = rngCurvePoints.uniform(0., width);
691 double x1 = rngCurvePoints.uniform(0., width);
692 double y0 = rngCurvePoints.uniform(0., height);
693 double y1 = rngCurvePoints.uniform(0., height);
694 vpColVector coeffs = computeABCD(x0, y0, x1, y1);
695 std::vector<vpImagePoint> curvePoints = generateSimulatedImage(0, width, 1., coeffs);
696
697#ifdef VISP_HAVE_DISPLAY
698 vpImage<vpRGBa> I(height, width);
699 std::shared_ptr<vpDisplay> pDisplay;
700 if (opt_display) {
701 pDisplay = vpDisplayFactory::createDisplay(I);
702 }
703#endif
704
705 for (unsigned int iter = 0; iter < nbTestRepet; ++iter) {
706 // Randomly select the initialization points
707 std::vector<vpImagePoint> suffledVector = vpUniRand::shuffleVector(curvePoints, seedShuffle);
708 std::vector<vpImagePoint> initPoints;
709 for (unsigned int j = 0; j < nbInitPoints; ++j) {
710 initPoints.push_back(suffledVector[j]);
711 }
712
713 // Compute the initial model
714 vpParabolaModel modelInitial = computeInitialGuess(initPoints, degree, height, width);
715 vpColVector X0 = modelInitial.toVpColVector();
716
717 // Initialize the Particle Filter
718 std::vector<double> stdevsPF;
719 for (unsigned int i = 0; i < degree + 1; ++i) {
720 stdevsPF.push_back(ratioAmpliMax * std::pow(0.1, i) * X0[0] / 3.);
721 }
722 vpParticleFilter<std::vector<vpImagePoint>>::vpProcessFunction processFunc = fx;
723 vpLikelihoodFunctor likelihoodFtor(sigmaLikelihood, height, width);
724 using std::placeholders::_1;
725 using std::placeholders::_2;
726 vpParticleFilter<std::vector<vpImagePoint>>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2);
727 vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleResamplingCheck;
728 vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingFunction resamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleImportanceResampling;
729 vpAverageFunctor averageCpter(degree, height, width);
730 using std::placeholders::_3;
731 vpParticleFilter<std::vector<vpImagePoint>>::vpFilterFunction meanFunc = std::bind(&vpAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3);
732 vpParticleFilter<std::vector<vpImagePoint>> filter(nbParticles, stdevsPF, seedPF, nbThreads);
733 filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc);
734
735 for (unsigned int i = 0; i < nbWarmUpIter; ++i) {
736 filter.filter(curvePoints, dt);
737 }
738
739 double meanError = 0.;
740 for (unsigned int i = 0; i < nbEvalIter; ++i) {
741 filter.filter(curvePoints, dt);
742 vpColVector Xest = filter.computeFilteredState();
743 vpParabolaModel model(Xest, height, width);
744 double rmse = evaluate(curvePoints, model);
745 meanError += rmse;
746
747#ifdef VISP_HAVE_DISPLAY
748 if (opt_display) {
749 vpDisplay::display(I);
750 displayGeneratedImage(I, curvePoints, vpColor::red, "GT", 20, 20);
751 model.display(I, vpColor::blue, "Model", 40, 20);
752 vpDisplay::flush(I);
753 vpDisplay::getClick(I);
754 }
755#endif
756 }
757 meanError /= static_cast<double>(nbEvalIter);
758 std::cout << "[3rd degree][Noise-free] Test " << iter << ": Mean(rmse) = " << meanError << std::endl;
759 CHECK(meanError <= maxToleratedError);
760 }
761 }
762
763
764
765 SECTION("Noisy", "Noise is added to the init points")
766 {
767 const double maxToleratedError = 17.;
768 double x0 = rngCurvePoints.uniform(0., width);
769 double x1 = rngCurvePoints.uniform(0., width);
770 double y0 = rngCurvePoints.uniform(0., height);
771 double y1 = rngCurvePoints.uniform(0., height);
772 vpColVector coeffs = computeABCD(x0, y0, x1, y1);
773 std::vector<vpImagePoint> curvePoints = generateSimulatedImage(0, width, 1., coeffs);
774
775#ifdef VISP_HAVE_DISPLAY
776 vpImage<vpRGBa> I(height, width);
777 std::shared_ptr<vpDisplay> pDisplay;
778 if (opt_display) {
779 pDisplay = vpDisplayFactory::createDisplay(I);
780 }
781#endif
782
783 const double ampliMaxInitNoise = 1.5;
784 const double stdevInitNoise = ampliMaxInitNoise / 3.;
785 vpGaussRand rngInitNoise(stdevInitNoise, 0., seedInitPoints);
786
787 for (unsigned int iter = 0; iter < nbTestRepet; ++iter) {
788 // Randomly select the initialization points
789 std::vector<vpImagePoint> suffledVector = vpUniRand::shuffleVector(curvePoints, seedShuffle);
790 std::vector<vpImagePoint> initPoints;
791 for (unsigned int j = 0; j < nbInitPoints * 4; ++j) {
792 vpImagePoint noisyPt(suffledVector[j].get_i() + rngInitNoise(), suffledVector[j].get_j() + rngInitNoise());
793 initPoints.push_back(noisyPt);
794 }
795
796 // Compute the initial model
797 vpParabolaModel modelInitial = computeInitialGuess(initPoints, degree, height, width);
798 vpColVector X0 = modelInitial.toVpColVector();
799
800 // Initialize the Particle Filter
801 std::vector<double> stdevsPF;
802 for (unsigned int i = 0; i < degree + 1; ++i) {
803 stdevsPF.push_back(ratioAmpliMax * std::pow(.05, i) * X0[0] / 3.);
804 }
805 vpParticleFilter<std::vector<vpImagePoint>>::vpProcessFunction processFunc = fx;
806 vpLikelihoodFunctor likelihoodFtor(sigmaLikelihood * 2., height, width);
807 using std::placeholders::_1;
808 using std::placeholders::_2;
809 vpParticleFilter<std::vector<vpImagePoint>>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2);
810 vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleResamplingCheck;
811 vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingFunction resamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleImportanceResampling;
812 vpAverageFunctor averageCpter(degree, height, width);
813 using std::placeholders::_3;
814 vpParticleFilter<std::vector<vpImagePoint>>::vpFilterFunction meanFunc = std::bind(&vpAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3);
815 vpParticleFilter<std::vector<vpImagePoint>> filter(nbParticles, stdevsPF, seedPF, nbThreads);
816 filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc);
817
818 for (unsigned int i = 0; i < nbWarmUpIter * 5; ++i) {
819 filter.filter(curvePoints, dt);
820 }
821
822 double meanError = 0.;
823 for (unsigned int i = 0; i < nbEvalIter; ++i) {
824 filter.filter(curvePoints, dt);
825 vpColVector Xest = filter.computeFilteredState();
826 vpParabolaModel model(Xest, height, width);
827 double rmse = evaluate(curvePoints, model);
828 meanError += rmse;
829
830#ifdef VISP_HAVE_DISPLAY
831 if (opt_display) {
832 vpDisplay::display(I);
833 displayGeneratedImage(I, curvePoints, vpColor::red, "GT", 20, 20);
834 model.display(I, vpColor::blue, "Model", 40, 20);
835 vpDisplay::flush(I);
836 vpDisplay::getClick(I);
837 }
838#endif
839 }
840 meanError /= static_cast<double>(nbEvalIter);
841 std::cout << "[3rd degree][Noisy] Test " << iter << ": Mean(rmse) = " << meanError << std::endl;
842 CHECK(meanError <= maxToleratedError);
843 }
844 }
845}
846
847int main(int argc, char *argv[])
848{
849 Catch::Session session;
850 auto cli = session.cli()
851 | Catch::Clara::Opt(opt_display)["--display"]("Activate debug display");
852
853 session.cli(cli);
854 session.applyCommandLine(argc, argv);
855
856 int numFailed = session.run();
857 return numFailed;
858}
859
860#else
861#include <iostream>
862
863int main() { return EXIT_SUCCESS; }
864#endif
vpArray2D::resize
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:448
vpArray2D::size
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:435
vpColVector
Implementation of column vector and the associated operations.
Definition vpColVector.h:191
vpColor
Class to define RGB colors available for display functionalities.
Definition vpColor.h:157
vpColor::red
static const vpColor red
Definition vpColor.h:198
vpColor::blue
static const vpColor blue
Definition vpColor.h:204
vpDisplay::getClick
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Definition vpDisplay_uchar.cpp:894
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition vpDisplay_uchar.cpp:869
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition vpDisplay_uchar.cpp:845
vpDisplay::displayPoint
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
Definition vpDisplay_uchar.cpp:600
vpDisplay::displayText
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Definition vpDisplay_uchar.cpp:788
vpGaussRand
Class for generating random number with normal probability density.
Definition vpGaussRand.h:117
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition vpImagePoint.h:82
vpImagePoint::get_u
double get_u() const
Definition vpImagePoint.h:136
vpImagePoint::get_v
double get_v() const
Definition vpImagePoint.h:147
vpImage
Definition of the vpImage class member functions.
Definition vpImage.h:131
vpImage::getWidth
unsigned int getWidth() const
Definition vpImage.h:242
vpMath::equal
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:470
vpMatrix
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:175
vpMatrix::pseudoInverse
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition vpMatrix_pseudo_inverse.cpp:336
vpParticleFilter
The class permits to use a Particle Filter.
Definition vpParticleFilter.h:111
vpUniRand
Class for generating random numbers with uniform probability density.
Definition vpUniRand.h:127
vpUniRand::shuffleVector
static std::vector< T > shuffleVector(const std::vector< T > &inputVector, const int32_t &seed=-1)
Create a new vector that is a shuffled version of the inputVector.
Definition vpUniRand.h:152
VISP_NAMESPACE_NAME
Definition vpEigenConversion.h:44
vpDisplayFactory::createDisplay
std::shared_ptr< vpDisplay > createDisplay()
Return a smart pointer vpDisplay specialization if a GUI library is available or nullptr otherwise.
Definition vpDisplayFactory.h:142