Visual Servoing Platform  version 3.6.1 under development (2024-12-13)
tutorial-pf.cpp
1 /*
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31 */
32 
64 // ViSP includes
65 #include <visp3/core/vpConfig.h>
66 #include <visp3/core/vpCameraParameters.h>
67 #include <visp3/core/vpGaussRand.h>
68 #include <visp3/core/vpHomogeneousMatrix.h>
69 #include <visp3/core/vpMeterPixelConversion.h>
70 #include <visp3/core/vpPixelMeterConversion.h>
72 #ifdef VISP_HAVE_DISPLAY
73 #include <visp3/gui/vpPlot.h>
74 #include <visp3/gui/vpDisplayFactory.h>
75 #endif
77 #include <visp3/vision/vpPose.h>
78 
80 #include <visp3/core/vpUKSigmaDrawerMerwe.h>
81 #include <visp3/core/vpUnscentedKalman.h>
83 
85 #include <visp3/core/vpParticleFilter.h>
87 
88 #ifdef ENABLE_VISP_NAMESPACE
89 using namespace VISP_NAMESPACE_NAME;
90 #endif
91 
92 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
94 
101 vpColVector fx(const vpColVector &x, const double & /*dt*/)
102 {
103  vpColVector x_kPlus1(4);
104  x_kPlus1[0] = x[0] * std::cos(x[3]) - x[1] * std::sin(x[3]); // wX
105  x_kPlus1[1] = x[0] * std::sin(x[3]) + x[1] * std::cos(x[3]); // wY
106  x_kPlus1[2] = x[2]; // wZ
107  x_kPlus1[3] = x[3]; // omega * dt
108  return x_kPlus1;
109 }
111 
113 
122 vpHomogeneousMatrix computePose(std::vector<vpPoint> &point, const std::vector<vpImagePoint> &ip, const vpCameraParameters &cam)
123 {
124  vpPose pose;
125  double x = 0, y = 0;
126  for (unsigned int i = 0; i < point.size(); i++) {
127  vpPixelMeterConversion::convertPoint(cam, ip[i], x, y);
128  point[i].set_x(x);
129  point[i].set_y(y);
130  pose.addPoint(point[i]);
131  }
132 
135  return cMo;
136 }
138 
140 
144 class vpObjectSimulator
145 {
146 public:
156  vpObjectSimulator(const double &R, const double &w, const double &phi, const double &wZ, const double &stdevRng)
157  : m_R(R)
158  , m_w(w)
159  , m_phi(phi)
160  , m_wZ(wZ)
161  , m_rng(stdevRng, 0.)
162  { }
163 
170  vpColVector move(const double &t)
171  {
172  vpColVector wX(4, 1.);
173  double tNoisy = (m_w + m_rng())* t + m_phi;
174  wX[0] = m_R * std::cos(tNoisy);
175  wX[1] = m_R * std::sin(tNoisy);
176  wX[2] = m_wZ;
177  return wX;
178  }
179 
180 private:
181  double m_R;
182  double m_w;
183  double m_phi;
184  const double m_wZ;
185  vpGaussRand m_rng;
186 };
188 
190 
194 {
195 public:
209  const std::vector<vpColVector> &markers, const double &noise_stdev, const long &seed,
210  const double &likelihood_stdev)
211  : m_cam(cam)
212  , m_cMw(cMw)
213  , m_wRo(wRo)
214  , m_markers(markers)
215  , m_rng(noise_stdev, 0., seed)
216  {
217  double sigmaDistanceSquared = likelihood_stdev * likelihood_stdev;
218  m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared);
219  m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared);
220 
221  const unsigned int nbMarkers = static_cast<unsigned int>(m_markers.size());
222  for (unsigned int i = 0; i < nbMarkers; ++i) {
223  vpColVector marker = markers[i];
224  m_markersAsVpPoint.push_back(vpPoint(marker[0], marker[1], marker[2]));
225  }
226  }
227 
229 
236  {
237  unsigned int nbMarkers = static_cast<unsigned int>(m_markers.size());
238  vpColVector meas(2*nbMarkers);
240  vpTranslationVector wTo(x[0], x[1], x[2]);
241  wMo.buildFrom(wTo, m_wRo);
242  for (unsigned int i = 0; i < nbMarkers; ++i) {
243  vpColVector cX = m_cMw * wMo * m_markers[i];
244  double u = 0., v = 0.;
245  vpMeterPixelConversion::convertPoint(m_cam, cX[0] / cX[2], cX[1] / cX[2], u, v);
246  meas[2*i] = u;
247  meas[2*i + 1] = v;
248  }
249  return meas;
250  }
252 
254 
262  {
263  unsigned int nbMarkers = static_cast<unsigned int>(m_markers.size());
264  vpColVector meas(2*nbMarkers);
266  vpTranslationVector wTo(wX[0], wX[1], wX[2]);
267  wMo.buildFrom(wTo, m_wRo);
268  for (unsigned int i = 0; i < nbMarkers; ++i) {
269  vpColVector cX = m_cMw * wMo * m_markers[i];
270  double u = 0., v = 0.;
271  vpMeterPixelConversion::convertPoint(m_cam, cX[0] / cX[2], cX[1] / cX[2], u, v);
272  meas[2*i] = u;
273  meas[2*i + 1] = v;
274  }
275  return meas;
276  }
278 
280 
288  {
289  vpColVector measurementsGT = measureGT(wX);
290  vpColVector measurementsNoisy = measurementsGT;
291  unsigned int sizeMeasurement = measurementsGT.size();
292  for (unsigned int i = 0; i < sizeMeasurement; ++i) {
293  measurementsNoisy[i] += m_rng();
294  }
295  return measurementsNoisy;
296  }
298 
300 
312  double likelihood(const vpColVector &x, const vpColVector &meas)
313  {
314  unsigned int nbMarkers = static_cast<unsigned int>(m_markers.size());
315  double likelihood = 0.;
317  vpTranslationVector wTo(x[0], x[1], x[2]);
318  wMo.buildFrom(wTo, m_wRo);
319  const unsigned int sizePt2D = 2;
320  const unsigned int idX = 0, idY = 1, idZ = 2;
321  double sumError = 0.;
322  // Compute the error between the projection of the markers that correspond
323  // to the particle position and the actual measurements of the markers
324  // projection
325  for (unsigned int i = 0; i < nbMarkers; ++i) {
326  vpColVector cX = m_cMw * wMo * m_markers[i];
327  vpImagePoint projParticle;
328  vpMeterPixelConversion::convertPoint(m_cam, cX[idX] / cX[idZ], cX[idY] / cX[idZ], projParticle);
329  vpImagePoint measPt(meas[sizePt2D * i + 1], meas[sizePt2D * i]);
330  double error = vpImagePoint::sqrDistance(projParticle, measPt);
331  sumError += error;
332  }
333  // Compute the likelihood from the mean error
334  likelihood = std::exp(m_constantExpDenominator * sumError / static_cast<double>(nbMarkers)) * m_constantDenominator;
335  likelihood = std::min(likelihood, 1.0); // Clamp to have likelihood <= 1.
336  likelihood = std::max(likelihood, 0.); // Clamp to have likelihood >= 0.
337  return likelihood;
338  }
340 private:
341  vpCameraParameters m_cam; // The camera parameters
342  vpHomogeneousMatrix m_cMw; // The pose of the world frame with regard to the camera frame.
343  vpRotationMatrix m_wRo; // The rotation matrix that expresses the rotation between the world frame and object frame.
344  std::vector<vpColVector> m_markers; // The position of the markers in the object frame.
345  std::vector<vpPoint> m_markersAsVpPoint; // The position of the markers in the object frame, expressed as vpPoint.
346  vpGaussRand m_rng; // Noise simulator for the measurements
347  double m_constantDenominator; // Denominator of the Gaussian function used for the likelihood computation.
348  double m_constantExpDenominator; // Denominator of the exponential of the Gaussian function used for the likelihood computation.
349 };
351 
353 struct SoftwareArguments
354 {
355  // --- Main loop parameters---
356  static const int SOFTWARE_CONTINUE = 42;
357  bool m_useDisplay;
358  unsigned int m_nbStepsWarmUp;
359  unsigned int m_nbSteps;
360  // --- PF parameters---
361  unsigned int m_N;
362  double m_maxDistanceForLikelihood;
363  double m_ampliMaxX;
364  double m_ampliMaxY;
365  double m_ampliMaxZ;
367  long m_seedPF;
368  int m_nbThreads;
369 
371  : m_useDisplay(true)
372  , m_nbStepsWarmUp(200)
373  , m_nbSteps(300)
374  , m_N(500)
375  , m_maxDistanceForLikelihood(10.)
376  , m_ampliMaxX(0.02)
377  , m_ampliMaxY(0.02)
378  , m_ampliMaxZ(0.01)
379  , m_ampliMaxOmega(0.02)
380  , m_seedPF(4224)
381  , m_nbThreads(1)
382  { }
383 
384  int parseArgs(const int argc, const char *argv[])
385  {
386  int i = 1;
387  while (i < argc) {
388  std::string arg(argv[i]);
389  if ((arg == "--nb-steps-main") && ((i+1) < argc)) {
390  m_nbSteps = std::atoi(argv[i + 1]);
391  ++i;
392  }
393  else if ((arg == "--nb-steps-warmup") && ((i+1) < argc)) {
394  m_nbStepsWarmUp = std::atoi(argv[i + 1]);
395  ++i;
396  }
397  else if ((arg == "--max-distance-likelihood") && ((i+1) < argc)) {
398  m_maxDistanceForLikelihood = std::atof(argv[i + 1]);
399  ++i;
400  }
401  else if (((arg == "-N") || (arg == "--nb-particles")) && ((i+1) < argc)) {
402  m_N = std::atoi(argv[i + 1]);
403  ++i;
404  }
405  else if ((arg == "--seed") && ((i+1) < argc)) {
406  m_seedPF = std::atoi(argv[i + 1]);
407  ++i;
408  }
409  else if ((arg == "--nb-threads") && ((i+1) < argc)) {
410  m_nbThreads = std::atoi(argv[i + 1]);
411  ++i;
412  }
413  else if ((arg == "--ampli-max-X") && ((i+1) < argc)) {
414  m_ampliMaxX = std::atof(argv[i + 1]);
415  ++i;
416  }
417  else if ((arg == "--ampli-max-Y") && ((i+1) < argc)) {
418  m_ampliMaxY = std::atof(argv[i + 1]);
419  ++i;
420  }
421  else if ((arg == "--ampli-max-Z") && ((i+1) < argc)) {
422  m_ampliMaxZ = std::atof(argv[i + 1]);
423  ++i;
424  }
425  else if ((arg == "--ampli-max-omega") && ((i+1) < argc)) {
426  m_ampliMaxOmega = std::atof(argv[i + 1]);
427  ++i;
428  }
429  else if (arg == "-d") {
430  m_useDisplay = false;
431  }
432  else if ((arg == "-h") || (arg == "--help")) {
433  printUsage(std::string(argv[0]));
434  SoftwareArguments defaultArgs;
435  defaultArgs.printDetails();
436  return 0;
437  }
438  else {
439  std::cout << "WARNING: unrecognised argument \"" << arg << "\"";
440  if (i + 1 < argc) {
441  std::cout << " with associated value(s) { ";
442  int nbValues = 0;
443  int j = i + 1;
444  bool hasToRun = true;
445  while ((j < argc) && hasToRun) {
446  std::string nextValue(argv[j]);
447  if (nextValue.find("--") == std::string::npos) {
448  std::cout << nextValue << " ";
449  ++nbValues;
450  }
451  else {
452  hasToRun = false;
453  }
454  ++j;
455  }
456  std::cout << "}" << std::endl;
457  i += nbValues;
458  }
459  }
460  ++i;
461  }
462  return SOFTWARE_CONTINUE;
463  }
464 
465 private:
466  void printUsage(const std::string &softName)
467  {
468  std::cout << "SYNOPSIS" << std::endl;
469  std::cout << " " << softName << " [--nb-steps-main <uint>] [--nb-steps-warmup <uint>]" << std::endl;
470  std::cout << " [--max-distance-likelihood <double>] [-N, --nb-particles <uint>] [--seed <int>] [--nb-threads <int>]" << std::endl;
471  std::cout << " [--ampli-max-X <double>] [--ampli-max-Y <double>] [--ampli-max-Z <double>] [--ampli-max-omega <double>]" << std::endl;
472  std::cout << " [-d, --no-display] [-h]" << std::endl;
473  }
474 
475  void printDetails()
476  {
477  std::cout << std::endl << std::endl;
478  std::cout << "DETAILS" << std::endl;
479  std::cout << " --nb-steps-main" << std::endl;
480  std::cout << " Number of steps in the main loop." << std::endl;
481  std::cout << " Default: " << m_nbSteps << std::endl;
482  std::cout << std::endl;
483  std::cout << " --nb-steps-warmup" << std::endl;
484  std::cout << " Number of steps in the warmup loop." << std::endl;
485  std::cout << " Default: " << m_nbStepsWarmUp << std::endl;
486  std::cout << std::endl;
487  std::cout << " --max-distance-likelihood" << std::endl;
488  std::cout << " Maximum mean distance of the projection of the markers corresponding" << std::endl;
489  std::cout << " to a particle with the measurements. Above this value, the likelihood of the particle is 0." << std::endl;
490  std::cout << " Default: " << m_maxDistanceForLikelihood << std::endl;
491  std::cout << std::endl;
492  std::cout << " -N, --nb-particles" << std::endl;
493  std::cout << " Number of particles of the Particle Filter." << std::endl;
494  std::cout << " Default: " << m_N << std::endl;
495  std::cout << std::endl;
496  std::cout << " --seed" << std::endl;
497  std::cout << " Seed to initialize the Particle Filter." << std::endl;
498  std::cout << " Use a negative value makes to use the current timestamp instead." << std::endl;
499  std::cout << " Default: " << m_seedPF << std::endl;
500  std::cout << std::endl;
501  std::cout << " --nb-threads" << std::endl;
502  std::cout << " Set the number of threads to use in the Particle Filter (only if OpenMP is available)." << std::endl;
503  std::cout << " Use a negative value to use the maximum number of threads instead." << std::endl;
504  std::cout << " Default: " << m_nbThreads << std::endl;
505  std::cout << std::endl;
506  std::cout << " --ampli-max-X" << std::endl;
507  std::cout << " Maximum amplitude of the noise added to a particle along the X-axis." << std::endl;
508  std::cout << " Default: " << m_ampliMaxX << std::endl;
509  std::cout << std::endl;
510  std::cout << " --ampli-max-Y" << std::endl;
511  std::cout << " Maximum amplitude of the noise added to a particle along the Y-axis." << std::endl;
512  std::cout << " Default: " << m_ampliMaxY << std::endl;
513  std::cout << std::endl;
514  std::cout << " --ampli-max-Z" << std::endl;
515  std::cout << " Maximum amplitude of the noise added to a particle along the Z-axis." << std::endl;
516  std::cout << " Default: " << m_ampliMaxZ << std::endl;
517  std::cout << std::endl;
518  std::cout << " --ampli-max-omega" << std::endl;
519  std::cout << " Maximum amplitude of the noise added to a particle affecting the pulsation of the motion." << std::endl;
520  std::cout << " Default: " << m_ampliMaxOmega << std::endl;
521  std::cout << std::endl;
522  std::cout << " -d, --no-display" << std::endl;
523  std::cout << " Deactivate display." << std::endl;
524  std::cout << " Default: display is ";
525 #ifdef VISP_HAVE_DISPLAY
526  std::cout << "ON" << std::endl;
527 #else
528  std::cout << "OFF" << std::endl;
529 #endif
530  std::cout << std::endl;
531  std::cout << " -h, --help" << std::endl;
532  std::cout << " Display this help." << std::endl;
533  std::cout << std::endl;
534  }
535 };
537 
538 int main(const int argc, const char *argv[])
539 {
540  SoftwareArguments args;
541  int returnCode = args.parseArgs(argc, argv);
542  if (returnCode != SoftwareArguments::SOFTWARE_CONTINUE) {
543  return returnCode;
544  }
545 
547  const unsigned int nbIter = 200; // Number of time steps for the simulation
548  const double dt = 0.001; // Period of 0.001s
549  const double sigmaMeasurements = 2.; // Standard deviation of the measurements: 2 pixels
550  const double radius = 0.25; // Radius of revolution of 0.25m
551  const double w = 2 * M_PI * 10; // Pulsation of the motion of revolution
552  const double phi = 2; // Phase of the motion of revolution
553 
554  // Vector of the markers sticked on the object
555  const std::vector<vpColVector> markers = { vpColVector({-0.05, 0.05, 0., 1.}),
556  vpColVector({0.05, 0.05, 0., 1.}),
557  vpColVector({0.05, -0.05, 0., 1.}),
558  vpColVector({-0.05, -0.05, 0., 1.}) };
559  const unsigned int nbMarkers = static_cast<unsigned int>(markers.size());
560  std::vector<vpPoint> markersAsVpPoint;
561  for (unsigned int i = 0; i < nbMarkers; ++i) {
562  vpColVector marker = markers[i];
563  markersAsVpPoint.push_back(vpPoint(marker[0], marker[1], marker[2]));
564  }
565 
566  const long seed = 42; // Seed for the random generator
567  vpHomogeneousMatrix cMw; // Pose of the world frame with regard to the camera frame
568  cMw[0][0] = 1.; cMw[0][1] = 0.; cMw[0][2] = 0.; cMw[0][3] = 0.2;
569  cMw[1][0] = 0.; cMw[1][1] = -1.; cMw[1][2] = 0.; cMw[1][3] = 0.3;
570  cMw[2][0] = 0.; cMw[2][1] = 0.; cMw[2][2] = -1.; cMw[2][3] = 1.;
571 
572  vpHomogeneousMatrix wMo; // Pose of the object frame with regard to the world frame
573  wMo[0][0] = 1.; wMo[0][1] = 0.; wMo[0][2] = 0.; wMo[0][3] = radius;
574  wMo[1][0] = 0.; wMo[1][1] = 1.; wMo[1][2] = 0.; wMo[1][3] = 0;
575  wMo[2][0] = 0.; wMo[2][1] = 0.; wMo[2][2] = 1.; wMo[2][3] = 0.2;
576  vpRotationMatrix wRo; // Rotation between the object frame and world frame
577  wMo.extract(wRo);
578  const double wZ = wMo[2][3];
580 
582  // Create a camera parameter container
583  // Camera initialization with a perspective projection without distortion model
584  double px = 600; double py = 600; double u0 = 320; double v0 = 240;
585  vpCameraParameters cam;
586  cam.initPersProjWithoutDistortion(px, py, u0, v0);
588 
590  vpColVector X0(4); // The initial guess for the state
591  X0[0] = 0.95 * radius * std::cos(phi); // Wrong estimation of the position along the X-axis = 5% of error
592  X0[1] = 0.95 * radius * std::sin(phi); // Wrong estimation of the position along the Y-axis = 5% of error
593  X0[2] = 0.95 * wZ; // Wrong estimation of the position along the Z-axis: error of 5%
594  X0[3] = 0.95 * w * dt; // Wrong estimation of the pulsation: error of 25%
596 
598  const double maxDistanceForLikelihood = args.m_maxDistanceForLikelihood; // The maximum allowed distance between a particle and the measurement, leading to a likelihood equal to 0..
599  const double sigmaLikelihood = maxDistanceForLikelihood / 3.; // The standard deviation of likelihood function.
600  const unsigned int nbParticles = args.m_N; // Number of particles to use
601  const double ampliMaxX = args.m_ampliMaxX, ampliMaxY = args.m_ampliMaxY, ampliMaxZ = args.m_ampliMaxZ;
602  const double ampliMaxOmega = args.m_ampliMaxOmega;
603  const std::vector<double> stdevsPF = { ampliMaxX/3., ampliMaxY/3., ampliMaxZ/3., ampliMaxOmega/3. }; // Standard deviation for each state component
604  long seedPF = args.m_seedPF; // Seed for the random generators of the PF
605  const int nbThread = args.m_nbThreads;
606  if (seedPF < 0) {
607  seedPF = static_cast<long>(vpTime::measureTimeMicros());
608  }
610 
611  // Object that converts the pose of the object into measurements
612  vpMarkersMeasurements markerMeas(cam, cMw, wRo, markers, sigmaMeasurements, seed, sigmaLikelihood);
613 
616  using std::placeholders::_1;
617  using std::placeholders::_2;
618  vpParticleFilter<vpColVector>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpMarkersMeasurements::likelihood, &markerMeas, _1, _2);
622 
624  // Initialize the PF
625  vpParticleFilter<vpColVector> pfFilter(nbParticles, stdevsPF, seedPF, nbThread);
626  pfFilter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc);
628 
630  // Initialize the attributes of the UKF
631  // Sigma point drawer
632  std::shared_ptr<vpUKSigmaDrawerAbstract> drawer = std::make_shared<vpUKSigmaDrawerMerwe>(4, 0.001, 2., -1);
633 
634  // Measurements covariance
635  vpMatrix R1landmark(2, 2, 0.); // The covariance of the noise introduced by the measurement with 1 landmark
636  R1landmark[0][0] = sigmaMeasurements*sigmaMeasurements;
637  R1landmark[1][1] = sigmaMeasurements*sigmaMeasurements;
638  vpMatrix R(2*nbMarkers, 2 * nbMarkers);
639  for (unsigned int i = 0; i < nbMarkers; ++i) {
640  R.insert(R1landmark, 2*i, 2*i);
641  }
642 
643  // Process covariance
644  const double processVariance = 0.000025; // Variance of the process of (0.005cm)^2
645  vpMatrix Q; // The noise introduced during the prediction step
646  Q.eye(4);
647  Q = Q * processVariance;
648 
649  // Process covariance initial guess
650  vpMatrix P0(4, 4);
651  P0.eye(4);
652  P0[0][0] = 1.;
653  P0[1][1] = 1.;
654  P0[2][2] = 1.;
655  P0[2][2] = 5.;
656 
657  // Functions for the UKF
660 
661  // Initialize the UKF
662  vpUnscentedKalman ukf(Q, R, drawer, f, h);
663  ukf.init(X0, P0);
665 
667 #ifdef VISP_HAVE_DISPLAY
668  // Initialize the plot
669  vpPlot plot(1);
670  plot.initGraph(0, 4);
671  plot.setTitle(0, "Position of the robot wX");
672  plot.setUnitX(0, "Position along x(m)");
673  plot.setUnitY(0, "Position along y (m)");
674  plot.setLegend(0, 0, "GT");
675  plot.setLegend(0, 1, "PF");
676  plot.setLegend(0, 2, "UKF");
677  plot.setLegend(0, 3, "Measure");
678  plot.initRange(0, -1.25 * radius, 1.25 * radius, -1.25 * radius, 1.25 * radius);
679  plot.setColor(0, 0, vpColor::red);
680  plot.setColor(0, 1, vpColor::green);
681  plot.setColor(0, 2, vpColor::blue);
682  plot.setColor(0, 3, vpColor::black);
683 
684  vpPlot plotError(1, 350, 700, 700, 700, "Error w.r.t. GT");
685  plotError.initGraph(0, 3);
686  plotError.setUnitX(0, "Time (s)");
687  plotError.setUnitY(0, "Error (m)");
688  plotError.setLegend(0, 0, "PF");
689  plotError.setLegend(0, 1, "UKF");
690  plotError.setLegend(0, 2, "Measure");
691  plotError.initRange(0, 0, nbIter * dt, 0, radius / 2.);
692  plotError.setColor(0, 0, vpColor::green);
693  plotError.setColor(0, 1, vpColor::blue);
694  plotError.setColor(0, 2, vpColor::black);
695 #endif
697 
699  // Initialize the display
700  // Depending on the detected third party libraries, we instantiate here the
701  // first video device which is available
702 #ifdef VISP_HAVE_DISPLAY
703  vpImage<vpRGBa> Idisp(700, 700, vpRGBa(255));
704  std::shared_ptr<vpDisplay> d = vpDisplayFactory::createDisplay(Idisp, 800, -1, "Projection of the markers");
705 #endif
707 
709  // Initialize the simulation
710  vpObjectSimulator object(radius, w, phi, wZ, ampliMaxOmega / 3.);
711  vpColVector object_pos(4, 0.);
712  object_pos[3] = 1.;
714 
716  for (unsigned int i = 0; i < nbIter; ++i) {
717  double t = dt * static_cast<double>(i);
718  std::cout << "[Timestep" << i << ", t = " << t << "]" << std::endl;
720  // Update object pose
721  object_pos = object.move(t);
723 
725  // Perform the measurement
726  vpColVector z = markerMeas.measureWithNoise(object_pos);
728 
731  double tPF = vpTime::measureTimeMs();
732  pfFilter.filter(z, dt);
733  double dtPF = vpTime::measureTimeMs() - tPF;
735 
737  // Use the UKF to filter the measurement for comparison
738  double tUKF = vpTime::measureTimeMs();
739  ukf.filter(z, dt);
740  double dtUKF = vpTime::measureTimeMs() - tUKF;
742 
743  // Get the filtered states
744  vpColVector XestPF = pfFilter.computeFilteredState();
745  vpColVector XestUKF = ukf.getXest();
746 
747  // Compute satistics
748  vpColVector cX_GT = cMw * object_pos;
749  vpColVector wX_UKF(4, 1.);
750  vpColVector wX_PF(4, 1.);
751  for (unsigned int i = 0; i < 3; ++i) {
752  wX_PF[i] = XestPF[i];
753  wX_UKF[i] = XestUKF[i];
754  }
755  vpColVector cX_PF = cMw * wX_PF;
756  vpColVector cX_UKF = cMw * wX_UKF;
757  vpColVector error_PF = cX_PF - cX_GT;
758  vpColVector error_UKF = cX_UKF - cX_GT;
759 
760  // Log statistics
761  std::cout << " [Particle Filter method] " << std::endl;
762  std::cout << " Norm of the error = " << error_PF.frobeniusNorm() << " m^2" << std::endl;
763  std::cout << " Fitting duration = " << dtPF << " ms" << std::endl;
764 
765  std::cout << " [Unscented Kalman Filter method] " << std::endl;
766  std::cout << " Norm of the error = " << error_UKF.frobeniusNorm() << " m^2" << std::endl;
767  std::cout << " Fitting duration = " << dtUKF << " ms" << std::endl;
768 
770 #ifdef VISP_HAVE_DISPLAY
772  // Prepare the pose computation:
773  // the image points corresponding to the noisy markers are needed
774  std::vector<vpImagePoint> ip;
775  for (unsigned int id = 0; id < nbMarkers; ++id) {
776  vpImagePoint markerProjNoisy(z[2*id + 1], z[2*id]);
777  ip.push_back(markerProjNoisy);
778  }
779 
780  // Compute the pose using the noisy markers
781  vpHomogeneousMatrix cMo_noisy = computePose(markersAsVpPoint, ip, cam);
782  vpHomogeneousMatrix wMo_noisy = cMw.inverse() * cMo_noisy;
783  double wXnoisy = wMo_noisy[0][3];
784  double wYnoisy = wMo_noisy[1][3];
786 
788  // Plot the ground truth
789  plot.plot(0, 0, object_pos[0], object_pos[1]);
790 
791  // Plot the PF filtered state
792  plot.plot(0, 1, XestPF[0], XestPF[1]);
793 
794 
795  // Plot the UKF filtered state
796  plot.plot(0, 2, XestUKF[0], XestUKF[1]);
797 
798  // Plot the noisy pose
799  plot.plot(0, 3, wXnoisy, wYnoisy);
800 
801  // Plot the PF filtered state error
802  plotError.plot(0, 0, t, error_PF.frobeniusNorm());
803 
804  // Plot the UKF filtered state error
805  plotError.plot(0, 1, t, error_UKF.frobeniusNorm());
806 
807  // Plot the noisy error
808  plotError.plot(0, 2, t, (cMo_noisy.getTranslationVector() - vpTranslationVector(cX_GT.extract(0, 3))).frobeniusNorm());
810 
812  // Display the projection of the markers
813  vpDisplay::display(Idisp);
814  vpColVector zGT = markerMeas.measureGT(object_pos);
815  vpColVector zFiltUkf = markerMeas.state_to_measurement(XestUKF);
816  vpColVector zFiltPF = markerMeas.state_to_measurement(XestPF);
817  for (unsigned int id = 0; id < nbMarkers; ++id) {
818  vpImagePoint markerProjGT(zGT[2*id + 1], zGT[2*id]);
819  vpDisplay::displayCross(Idisp, markerProjGT, 5, vpColor::red);
820 
821  vpImagePoint markerProjFiltPF(zFiltPF[2*id + 1], zFiltPF[2*id]);
822  vpDisplay::displayCross(Idisp, markerProjFiltPF, 5, vpColor::green);
823 
824  vpImagePoint markerProjFiltUKF(zFiltUkf[2*id + 1], zFiltUkf[2*id]);
825  vpDisplay::displayCross(Idisp, markerProjFiltUKF, 5, vpColor::blue);
826 
827  vpImagePoint markerProjNoisy(z[2*id + 1], z[2*id]);
828  vpDisplay::displayCross(Idisp, markerProjNoisy, 5, vpColor::black);
829  }
830 
831  vpImagePoint ipText(20, 20);
832  vpDisplay::displayText(Idisp, ipText, std::string("GT"), vpColor::red);
833  ipText.set_i(ipText.get_i() + 20);
834  vpDisplay::displayText(Idisp, ipText, std::string("Filtered by PF"), vpColor::green);
835  ipText.set_i(ipText.get_i() + 20);
836  vpDisplay::displayText(Idisp, ipText, std::string("Filtered by UKF"), vpColor::blue);
837  ipText.set_i(ipText.get_i() + 20);
838  vpDisplay::displayText(Idisp, ipText, std::string("Measured"), vpColor::black);
839  vpDisplay::flush(Idisp);
840  vpTime::wait(40);
842 #endif
844  }
846  std::cout << "Press Enter to quit..." << std::endl;
847  std::cin.get();
848 
849  return 0;
850 }
851 #else
852 int main()
853 {
854  std::cout << "This example is only available if you compile ViSP in C++11 standard or higher." << std::endl;
855  return 0;
856 }
857 #endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
double frobeniusNorm() const
static const vpColor red
Definition: vpColor.h:217
static const vpColor blue
Definition: vpColor.h:223
static const vpColor black
Definition: vpColor.h:211
static const vpColor green
Definition: vpColor.h:220
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 displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Class for generating random number with normal probability density.
Definition: vpGaussRand.h:117
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
void extract(vpRotationMatrix &R) const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
static double sqrDistance(const vpImagePoint &iP1, const vpImagePoint &iP2)
[Object_simulator]
vpMarkersMeasurements(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMw, const vpRotationMatrix &wRo, const std::vector< vpColVector > &markers, const double &noise_stdev, const long &seed, const double &likelihood_stdev)
Construct a new vpMarkersMeasurements object.
vpColVector state_to_measurement(const vpColVector &x)
[Measurement_function]
double likelihood(const vpColVector &x, const vpColVector &meas)
[Noisy_measurements]
vpColVector measureGT(const vpColVector &wX)
[Measurement_function]
vpColVector measureWithNoise(const vpColVector &wX)
[GT_measurements]
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
[Process_function]
vpColVector move(const double &t)
Move the object to its new position, expressed in the world frame.
vpObjectSimulator(const double &R, const double &w, const double &phi, const double &wZ, const double &stdevRng)
Construct a new vpObjectSimulator object.
The class permits to use a Particle Filter.
std::function< vpParticlesWithWeights(const std::vector< vpColVector > &, const std::vector< double > &)> vpResamplingFunction
Function that takes as argument the vector of particles and the vector of associated weights....
std::function< vpColVector(const vpColVector &, const double &)> vpProcessFunction
Process model function, which projects a particle forward in time. The first argument is a particle,...
std::function< bool(const unsigned int &, const std::vector< double > &)> vpResamplingConditionFunction
Function that takes as argument the number of particles and the vector of weights associated to each ...
std::function< double(const vpColVector &, const MeasurementsType &)> vpLikelihoodFunction
Likelihood function, which evaluates the likelihood of a particle with regard to the measurements....
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:112
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:77
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:96
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition: vpPose.h:98
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, FuncCheckValidityPose func=nullptr)
Definition: vpPose.cpp:385
Definition: vpRGBa.h:65
Implementation of a rotation matrix and operations on such kind of matrices.
Class that consider the case of a translation vector.
std::function< vpColVector(const vpColVector &)> vpMeasurementFunction
Measurement function, which converts the prior points in the measurement space. The argument is a poi...
std::function< vpColVector(const vpColVector &, const double &)> vpProcessFunction
Process model function, which projects the sigma points forward in time. The first argument is a sigm...
std::shared_ptr< vpDisplay > createDisplay()
Return a smart pointer vpDisplay specialization if a GUI library is available or nullptr otherwise.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMicros()
VISP_EXPORT double measureTimeMs()
int m_nbThreads
Number of thread to use in the Particle Filter.
double m_ampliMaxY
Amplitude max of the noise for the state component corresponding to the Y coordinate.
unsigned int m_N
The number of particles.
double m_maxDistanceForLikelihood
The maximum allowed distance between a particle and the measurement, leading to a likelihood equal to...
double m_ampliMaxZ
Amplitude max of the noise for the state component corresponding to the Z coordinate.
int parseArgs(const int argc, const char *argv[])
double m_ampliMaxOmega
Amplitude max of the noise for the state component corresponding to the pulsation.
double m_ampliMaxX
Amplitude max of the noise for the state component corresponding to the X coordinate.
long m_seedPF
Seed for the random generators of the PF.