Visual Servoing Platform  version 3.6.1 under development (2025-02-18)
tutorial-pf-curve-fitting-all.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 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 
32 
33 // System includes
34 #include <algorithm>
35 #include <vector>
36 
37 // ViSP includes
38 #include <visp3/core/vpConfig.h>
39 #include <visp3/core/vpException.h>
40 #include <visp3/core/vpMath.h>
41 #include <visp3/core/vpMouseButton.h>
42 #include <visp3/core/vpTime.h>
43 
44 #ifdef VISP_HAVE_DISPLAY
45 #include <visp3/gui/vpPlot.h>
46 #endif
47 
49 #include <visp3/core/vpParticleFilter.h>
51 
52 #include "vpTutoCommonData.h"
53 #include "vpTutoMeanSquareFitting.h"
54 #include "vpTutoParabolaModel.h"
55 #include "vpTutoSegmentation.h"
56 
57 #ifdef ENABLE_VISP_NAMESPACE
58 using namespace VISP_NAMESPACE_NAME;
59 #endif
60 
61 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY)
62 #ifndef DOXYGEN_SHOULD_SKIP_THIS
63 namespace tutorial
64 {
66 
73 double evaluate(const vpImagePoint &pt, const vpTutoParabolaModel &model)
74 {
75  double u = pt.get_u();
76  double v = pt.get_v();
77  double v_model = model.eval(u);
78  double error = v - v_model;
79  double squareError = error * error;
80  return squareError;
81 }
82 
93 double evaluate(const vpColVector &coeffs, const unsigned int &height, const unsigned int &width, const std::vector<vpImagePoint> &pts)
94 {
95  unsigned int nbPts = static_cast<unsigned int>(pts.size());
96  vpColVector residuals(nbPts);
97  vpColVector weights(nbPts, 1.);
98  vpTutoParabolaModel model(coeffs, height, width);
99  // Compute the residuals
100  for (unsigned int i = 0; i < nbPts; ++i) {
101  double squareError = evaluate(pts[i], model);
102  residuals[i] = squareError;
103  }
104  double meanSquareError = residuals.sum() / static_cast<double>(nbPts);
105  return std::sqrt(meanSquareError);
106 }
108 
110 
118 template<typename T>
119 void display(const vpColVector &coeffs, const vpImage<T> &I, const vpColor &color,
120  const unsigned int &vertPosLegend, const unsigned int &horPosLegend)
121 {
122 #if defined(VISP_HAVE_DISPLAY)
123  unsigned int width = I.getWidth();
124  vpTutoParabolaModel model(coeffs, I.getHeight(), I.getWidth());
125  for (unsigned int u = 0; u < width; ++u) {
126  unsigned int v = static_cast<unsigned int>(model.eval(u));
127  vpDisplay::displayPoint(I, v, u, color, 1);
128  vpDisplay::displayText(I, vertPosLegend, horPosLegend, "Particle Filter model", color);
129  }
130 #else
131  (void)coeffs;
132  (void)I;
133  (void)color;
134  (void)vertPosLegend;
135  (void)horPosLegend;
136 #endif
137 }
139 
141 
148 std::vector<vpImagePoint> automaticInitialization(tutorial::vpTutoCommonData &data)
149 {
150  // Initialization-related variables
151  const unsigned int minNbPts = data.m_degree + 1;
152  const unsigned int nbPtsToUse = 10 * minNbPts;
153  std::vector<vpImagePoint> initPoints;
154 
155  // Perform HSV segmentation
156  tutorial::performSegmentationHSV(data);
157 
158  // Extracting the skeleton of the mask
159  std::vector<vpImagePoint> edgePoints = tutorial::extractSkeleton(data);
160  unsigned int nbEdgePoints = static_cast<unsigned int>(edgePoints.size());
161 
162  if (nbEdgePoints < nbPtsToUse) {
163  return edgePoints;
164  }
165 
166  // Uniformly extract init points
167  auto ptHasLowerU = [](const vpImagePoint &ptA, const vpImagePoint &ptB) {
168  return ptA.get_u() < ptB.get_u();
169  };
170  std::sort(edgePoints.begin(), edgePoints.end(), ptHasLowerU);
171 
172  unsigned int idStart, idStop;
173  if (nbEdgePoints > nbPtsToUse + 20) {
174  // Avoid extreme points in case it's noise
175  idStart = 10;
176  idStop = static_cast<unsigned int>(edgePoints.size()) - 10;
177  }
178  else {
179  // We need to take all the points because we don't have enough
180  idStart = 0;
181  idStop = static_cast<unsigned int>(edgePoints.size());
182  }
183 
184  // Sample uniformly the points starting from the left of the image to the right
185  unsigned int sizeWindow = idStop - idStart + 1;
186  unsigned int step = sizeWindow / (nbPtsToUse - 1);
187  for (unsigned int id = idStart; id <= idStop; id += step) {
188  initPoints.push_back(edgePoints[id]);
189  }
190  return initPoints;
191 }
192 
199 std::vector<vpImagePoint> manualInitialization(const tutorial::vpTutoCommonData &data)
200 {
201  // Interaction variables
202  const bool waitForClick = true;
203  vpImagePoint ipClick;
205 
206  // Display variables
207  const unsigned int sizeCross = 10;
208  const unsigned int thicknessCross = 2;
209  const vpColor colorCross = vpColor::red;
210 
211  // Initialization-related variables
212  const unsigned int minNbPts = data.m_degree + 1;
213  std::vector<vpImagePoint> initPoints;
214 
215  bool notEnoughPoints = true;
216  while (notEnoughPoints) {
217  // Initial display of the images
218  vpDisplay::display(data.m_I_orig);
219 
220  // Display the how-to
221  vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "Left click to add init point (min.: " + std::to_string(minNbPts) + "), right click to estimate the initial coefficients of the Particle Filter.", data.m_colorLegend);
222  vpDisplay::displayText(data.m_I_orig, data.m_ipLegend + data.m_legendOffset, "A middle click reinitialize the list of init points.", data.m_colorLegend);
223  vpDisplay::displayText(data.m_I_orig, data.m_ipLegend + data.m_legendOffset + data.m_legendOffset, "If not enough points have been selected, a right click has no effect.", data.m_colorLegend);
224 
225  // Display the already selected points
226  unsigned int nbInitPoints = static_cast<unsigned int>(initPoints.size());
227  for (unsigned int i = 0; i < nbInitPoints; ++i) {
228  vpDisplay::displayCross(data.m_I_orig, initPoints[i], sizeCross, colorCross, thicknessCross);
229  }
230 
231  // Update the display
232  vpDisplay::flush(data.m_I_orig);
233 
234  // Get the user input
235  vpDisplay::getClick(data.m_I_orig, ipClick, button, waitForClick);
236 
237  // Either add the clicked point to the list of initial points or stop the loop if enough points are available
238  switch (button) {
239  case vpMouseButton::vpMouseButtonType::button1:
240  initPoints.push_back(ipClick);
241  break;
242  case vpMouseButton::vpMouseButtonType::button2:
243  initPoints.clear();
244  break;
245  case vpMouseButton::vpMouseButtonType::button3:
246  (initPoints.size() >= minNbPts ? notEnoughPoints = false : notEnoughPoints = true);
247  break;
248  default:
249  break;
250  }
251  }
252 
253  return initPoints;
254 }
255 
264 vpColVector computeInitialGuess(tutorial::vpTutoCommonData &data)
265 {
266  // Vector that contains the init points
267  std::vector<vpImagePoint> initPoints;
268 
269 #ifdef VISP_HAVE_DISPLAY
270  // Interaction variables
271  const bool waitForClick = true;
272  vpImagePoint ipClick;
274 
275  // Display variables
276  const unsigned int sizeCross = 10;
277  const unsigned int thicknessCross = 2;
278  const vpColor colorCross = vpColor::red;
279 
280  bool automaticInit = false;
281 
282  // Initial display of the images
283  vpDisplay::display(data.m_I_orig);
284  vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "Left click to manually select the init points, right click to automatically initialize the PF", data.m_colorLegend);
285 
286  // Update the display
287  vpDisplay::flush(data.m_I_orig);
288 
289  // Get the user input
290  vpDisplay::getClick(data.m_I_orig, ipClick, button, waitForClick);
291 
292  // Either use the automatic initialization or the manual one depending on the user input
293  switch (button) {
294  case vpMouseButton::vpMouseButtonType::button1:
295  automaticInit = false;
296  break;
297  case vpMouseButton::vpMouseButtonType::button3:
298  automaticInit = true;
299  break;
300  default:
301  break;
302  }
303 
304  if (automaticInit) {
305  // Get automatically the init points from the segmented image
306  initPoints = tutorial::automaticInitialization(data);
307  }
308  else {
309  // Get manually the init points from the original image
310  initPoints = tutorial::manualInitialization(data);
311  }
312 
313 #else
314  // Get the init points from the segmented image
315  initPoints = tutorial::automaticInitialization(data);
316 #endif
317 
318  // Compute the coefficients of the parabola using Least-Mean-Square minimization.
319  tutorial::vpTutoMeanSquareFitting lmsFitter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth());
320  lmsFitter.fit(initPoints);
321  vpColVector X0 = lmsFitter.getCoeffs();
322  std::cout << "---[Initial fit]---" << std::endl;
323  std::cout << lmsFitter.getModel();
324  std::cout << "---[Initial fit]---" << std::endl;
325 
326  // Display info about the initialization
327  vpDisplay::display(data.m_I_orig);
328  vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "Here are the points selected for the initialization.", data.m_colorLegend);
329  size_t nbInitPoints = initPoints.size();
330  for (size_t i = 0; i < nbInitPoints; ++i) {
331  const vpImagePoint &ip = initPoints[i];
332  vpDisplay::displayCross(data.m_I_orig, ip, sizeCross, colorCross, thicknessCross);
333  }
334 
335  // Update display and wait for click
336  lmsFitter.display(data.m_I_orig, vpColor::red, static_cast<unsigned int>(data.m_ipLegend.get_v() + 2 * data.m_legendOffset.get_v()), static_cast<unsigned int>(data.m_ipLegend.get_u()));
337  vpDisplay::displayText(data.m_I_orig, data.m_ipLegend + data.m_legendOffset, "A click to continue.", data.m_colorLegend);
338  vpDisplay::flush(data.m_I_orig);
339  vpDisplay::getClick(data.m_I_orig, waitForClick);
340 
341  return X0;
342 }
344 
346 vpColVector fx(const vpColVector &coeffs, const double &/*dt*/)
347 {
348  vpColVector updatedCoeffs = coeffs; // We use a constant position model
349  return updatedCoeffs;
350 }
352 
354 class vpTutoAverageFunctor
355 {
356 public:
357  vpTutoAverageFunctor(const unsigned int &degree, const unsigned int &height, const unsigned int &width)
358  : m_degree(degree)
359  , m_height(height)
360  , m_width(width)
361  { }
362 
372  vpColVector averagePolynomials(const std::vector<vpColVector> &particles, const std::vector<double> &weights, const vpParticleFilter<std::vector<vpImagePoint>>::vpStateAddFunction &)
373  {
374  const unsigned int nbParticles = static_cast<unsigned int>(particles.size());
375  const double nbParticlesAsDOuble = static_cast<double>(nbParticles);
376  // Compute the sum of the weights to be able to determine the "importance" of a particle with regard to the whole set
377  const double sumWeight = std::accumulate(weights.begin(), weights.end(), 0.);
378 
379  // Defining the total number of control points we want to generate
380  const double nbPointsForAverage = 10. * nbParticlesAsDOuble;
381  std::vector<vpImagePoint> initPoints;
382 
383  // Creating control points by each particle
384  for (unsigned int i = 0; i < nbParticles; ++i) {
385  // The number of control points a particle can generate is proportional to the ratio of its weight w.r.t. the sum of the weights
386  double nbPoints = std::floor(weights[i] * nbPointsForAverage / sumWeight);
387  if (nbPoints > 1.) {
388  // The particle has a weight high enough to deserve more than one points
389  vpTutoParabolaModel curve(particles[i], m_height, m_width);
390  double widthAsDouble = static_cast<double>(m_width);
391  // Uniform sampling of the control points along the polynomial model
392  double step = widthAsDouble / (nbPoints - 1.);
393  for (double u = 0.; u < widthAsDouble; u += step) {
394  double v = curve.eval(u);
395  vpImagePoint pt(v, u);
396  initPoints.push_back(pt);
397  }
398  }
399  else if (vpMath::equal(nbPoints, 1.)) {
400  // The weight of the particle make it have only one control point
401  // We sample it at the middle of the image
402  vpTutoParabolaModel curve(particles[i], m_height, m_width);
403  double u = static_cast<double>(m_width) / 2.;
404  double v = curve.eval(u);
405  vpImagePoint pt(v, u);
406  initPoints.push_back(pt);
407  }
408  }
409  // We use Least-Mean Square minimization to compute the polynomial model that best fits all the control points
410  vpTutoMeanSquareFitting lms(m_degree, m_height, m_width);
411  lms.fit(initPoints);
412  return lms.getCoeffs();
413  }
414 
415 private:
416  unsigned int m_degree;
417  unsigned int m_height;
418  unsigned int m_width;
419 };
421 
423 class vpTutoLikelihoodFunctor
424 {
425 public:
433  vpTutoLikelihoodFunctor(const double &stdev, const unsigned int &height, const unsigned int &width)
434  : m_height(height)
435  , m_width(width)
436  {
437  double sigmaDistanceSquared = stdev * stdev;
438  m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared);
439  m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared);
440  }
441 
443 
455  double likelihood(const vpColVector &coeffs, const std::vector<vpImagePoint> &meas)
456  {
457  double likelihood = 0.;
458  unsigned int nbPoints = static_cast<unsigned int>(meas.size());
459 
460  // Generate a model from the coefficients stored in the particle state
461  vpTutoParabolaModel model(coeffs, m_height, m_width);
462 
463  // Compute the residual between each measurement point and its equivalent in the model
464  vpColVector residuals(nbPoints);
465  for (unsigned int i = 0; i < nbPoints; ++i) {
466  double squareError = tutorial::evaluate(meas[i], model);
467  residuals[i] = squareError;
468  }
469 
470  // Use Tukey M-estimator to be robust against outliers
471  vpRobust Mestimator;
472  vpColVector w(nbPoints, 1.);
473  Mestimator.MEstimator(vpRobust::TUKEY, residuals, w);
474  double sumError = w.hadamard(residuals).sum();
475 
476  // Compute the likelihood as a Gaussian function
477  likelihood = std::exp(m_constantExpDenominator * sumError / w.sum()) * m_constantDenominator;
478  likelihood = std::min(likelihood, 1.0); // Clamp to have likelihood <= 1.
479  likelihood = std::max(likelihood, 0.); // Clamp to have likelihood >= 0.
480  return likelihood;
481  }
483 private:
484  double m_constantDenominator;
485  double m_constantExpDenominator;
486  unsigned int m_height;
487  unsigned int m_width;
488 };
490 }
491 #endif
492 
493 int main(const int argc, const char *argv[])
494 {
495  tutorial::vpTutoCommonData data;
496  int returnCode = data.init(argc, argv);
497  if (returnCode != tutorial::vpTutoCommonData::SOFTWARE_CONTINUE) {
498  return returnCode;
499  }
500  tutorial::vpTutoMeanSquareFitting lmsFitter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth());
501  const unsigned int vertOffset = static_cast<unsigned int>(data.m_legendOffset.get_i());
502  const unsigned int horOffset = static_cast<unsigned int>(data.m_ipLegend.get_j());
503  const unsigned int legendLmsVert = data.m_I_orig.getHeight() - 3 * vertOffset;
504  const unsigned int legendLmsHor = horOffset;
505  const unsigned int legendPFVert = data.m_I_orig.getHeight() - 2 * vertOffset, legendPFHor = horOffset;
506 
507  // Initialize the attributes of the PF
509  vpColVector X0 = tutorial::computeInitialGuess(data);
511 
513  const double maxDistanceForLikelihood = data.m_pfMaxDistanceForLikelihood; // The maximum allowed distance between a particle and the measurement, leading to a likelihood equal to 0..
514  const double sigmaLikelihood = maxDistanceForLikelihood / 3.; // The standard deviation of likelihood function.
515  const unsigned int nbParticles = data.m_pfN; // Number of particles to use
516  std::vector<double> stdevsPF; // Standard deviation for each state component
517  for (unsigned int i = 0; i < data.m_degree + 1; ++i) {
518  double ampliMax = data.m_pfRatiosAmpliMax[i] * X0[i];
519  stdevsPF.push_back(ampliMax / 3.);
520  }
521  unsigned long seedPF; // Seed for the random generators of the PF
522  const float period = 33.3f; // 33.3ms i.e. 30Hz
523  if (data.m_pfSeed < 0) {
524  seedPF = static_cast<unsigned long>(vpTime::measureTimeMicros());
525  }
526  else {
527  seedPF = data.m_pfSeed;
528  }
529  const int nbThread = data.m_pfNbThreads;
531 
533  vpParticleFilter<std::vector<vpImagePoint>>::vpProcessFunction processFunc = tutorial::fx;
534  tutorial::vpTutoLikelihoodFunctor likelihoodFtor(sigmaLikelihood, data.m_I_orig.getHeight(), data.m_I_orig.getWidth());
535  using std::placeholders::_1;
536  using std::placeholders::_2;
537  vpParticleFilter<std::vector<vpImagePoint>>::vpLikelihoodFunction likelihoodFunc = std::bind(&tutorial::vpTutoLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2);
538  vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleResamplingCheck;
539  vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingFunction resamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleImportanceResampling;
540  tutorial::vpTutoAverageFunctor averageCpter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth());
541  using std::placeholders::_3;
542  vpParticleFilter<std::vector<vpImagePoint>>::vpFilterFunction meanFunc = std::bind(&tutorial::vpTutoAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3);
544 
546  // Initialize the PF
547  vpParticleFilter<std::vector<vpImagePoint>> filter(nbParticles, stdevsPF, seedPF, nbThread);
548  filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc);
550 
552 #ifdef VISP_HAVE_DISPLAY
553  unsigned int plotHeight = 350, plotWidth = 350;
554  int plotXpos = static_cast<int>(data.m_legendOffset.get_u());
555  int plotYpos = static_cast<int>(data.m_I_orig.getHeight() + 4. * data.m_legendOffset.get_v());
556  vpPlot plot(1, plotHeight, plotWidth, plotXpos, plotYpos, "Root mean-square error");
557  plot.initGraph(0, 2);
558  plot.setLegend(0, 0, "LMS estimator");
559  plot.setColor(0, 0, vpColor::gray);
560  plot.setLegend(0, 1, "PF estimator");
561  plot.setColor(0, 1, vpColor::red);
562 #endif
564 
565  bool run = true;
566  unsigned int nbIter = 0;
567  double meanDtLMS = 0., meanDtPF = 0.;
568  double meanRootMeanSquareErrorLMS = 0., meanRootMeanSquareErrorPF = 0.;
569  while (!data.m_grabber.end() && run) {
570  std::cout << "Iter " << nbIter << std::endl;
571  data.m_grabber.acquire(data.m_I_orig);
572 
573  tutorial::performSegmentationHSV(data);
574 
576  std::vector<vpImagePoint> edgePoints = tutorial::extractSkeleton(data);
577 
579  std::vector<vpImagePoint> noisyEdgePoints = tutorial::addSaltAndPepperNoise(edgePoints, data);
580 
581 #ifdef VISP_HAVE_DISPLAY
583  vpDisplay::display(data.m_I_orig);
584  vpDisplay::display(data.m_I_segmented);
585  vpDisplay::display(data.m_IskeletonNoisy);
586 #endif
587 
589  double tLms = vpTime::measureTimeMs();
590  lmsFitter.fit(noisyEdgePoints);
591  double dtLms = vpTime::measureTimeMs() - tLms;
592  double lmsRootMeanSquareError = lmsFitter.evaluate(edgePoints);
593  std::cout << " [Least-Mean Square method] " << std::endl;
594  std::cout << " Coeffs = [" << lmsFitter.getCoeffs().transpose() << " ]" << std::endl;
595  std::cout << " Root Mean Square Error = " << lmsRootMeanSquareError << " pixels" << std::endl;
596  std::cout << " Fitting duration = " << dtLms << " ms" << std::endl;
597  meanDtLMS += dtLms;
598  meanRootMeanSquareErrorLMS += lmsRootMeanSquareError;
599 
601  double tPF = vpTime::measureTimeMs();
603  filter.filter(noisyEdgePoints, period);
605  double dtPF = vpTime::measureTimeMs() - tPF;
606 
608  vpColVector Xest = filter.computeFilteredState();
610 
612  double pfError = tutorial::evaluate(Xest, data.m_I_orig.getHeight(), data.m_I_orig.getWidth(), edgePoints);
614  std::cout << " [Particle Filter method] " << std::endl;
615  std::cout << " Coeffs = [" << Xest.transpose() << " ]" << std::endl;
616  std::cout << " Root Mean Square Error = " << pfError << " pixels" << std::endl;
617  std::cout << " Fitting duration = " << dtPF << " ms" << std::endl;
618  meanDtPF += dtPF;
619  meanRootMeanSquareErrorPF += pfError;
620 
621 #ifdef VISP_HAVE_DISPLAY
622  // Update image overlay
623  lmsFitter.display<unsigned char>(data.m_IskeletonNoisy, vpColor::gray, legendLmsVert, legendLmsHor);
624  tutorial::display(Xest, data.m_IskeletonNoisy, vpColor::red, legendPFVert, legendPFHor);
625 
626  // Update plot
627  plot.plot(0, 0, nbIter, lmsRootMeanSquareError);
628  plot.plot(0, 1, nbIter, pfError);
629  // Display the images with overlayed info
630  data.displayLegend(data.m_I_orig);
631  vpDisplay::flush(data.m_I_orig);
632  vpDisplay::flush(data.m_I_segmented);
633  vpDisplay::flush(data.m_IskeletonNoisy);
634  run = data.manageClicks(data.m_I_orig, data.m_stepbystep);
635 #endif
636  ++nbIter;
637  }
638 
639  double iterAsDouble = static_cast<double>(nbIter);
640 
641  std::cout << std::endl << std::endl << "-----[Statistics summary]-----" << std::endl;
642 
643  std::cout << " [LMS method] " << std::endl;
644  std::cout << " Average Root Mean Square Error = " << meanRootMeanSquareErrorLMS / iterAsDouble << " pixels" << std::endl;
645  std::cout << " Average fitting duration = " << meanDtLMS / iterAsDouble << " ms" << std::endl;
646 
647  std::cout << " [Particle Filter method] " << std::endl;
648  std::cout << " Average Root Mean Square Error = " << meanRootMeanSquareErrorPF / iterAsDouble << " pixels" << std::endl;
649  std::cout << " Average fitting duration = " << meanDtPF / iterAsDouble << " ms" << std::endl;
650 
651 #ifdef VISP_HAVE_DISPLAY
652  if (data.m_grabber.end() && (!data.m_stepbystep)) {
654  vpDisplay::display(data.m_I_orig);
655  vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "End of sequence reached. Click to exit.", data.m_colorLegend);
656 
658  vpDisplay::flush(data.m_I_orig);
659 
661  vpDisplay::getClick(data.m_I_orig, true);
662  }
663 #endif
664  return 0;
665 }
666 #else
667 int main()
668 {
669  std::cerr << "ViSP must be compiled with C++ standard >= C++11 to use this tutorial." << std::endl;
670  std::cerr << "ViSP must also have a 3rd party enabling display features, such as X11 or OpenCV." << std::endl;
671  return EXIT_FAILURE;
672 }
673 #endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpRowVector transpose() const
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
static const vpColor red
Definition: vpColor.h:198
static const vpColor gray
Definition: vpColor.h:195
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
double get_u() const
Definition: vpImagePoint.h:136
double get_v() const
Definition: vpImagePoint.h:147
Definition of the vpImage class member functions.
Definition: vpImage.h:131
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:459
The class permits to use a Particle Filter.
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:112
Contains an M-estimator and various influence function.
Definition: vpRobust.h:84
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:89
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:130
VISP_EXPORT double measureTimeMicros()
VISP_EXPORT double measureTimeMs()