Visual Servoing Platform  version 3.6.1 under development (2025-02-17)
pf-nonlinear-complex-example.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 
77 #include <iostream>
78 
79 // ViSP includes
80 #include <visp3/core/vpConfig.h>
81 #include <visp3/core/vpColVector.h>
82 #include <visp3/core/vpGaussRand.h>
83 #ifdef VISP_HAVE_DISPLAY
84 #include <visp3/gui/vpPlot.h>
85 #endif
86 
87 // PF includes
88 #include <visp3/core/vpParticleFilter.h>
89 
90 
91 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
92 
93 #ifdef ENABLE_VISP_NAMESPACE
94 using namespace VISP_NAMESPACE_NAME;
95 #endif
96 
97 namespace
98 {
105 double normalizeAngle(const double &angle)
106 {
107  double angleIn0to2pi = vpMath::modulo(angle, 2. * M_PI);
108  double angleInMinPiPi = angleIn0to2pi;
109  if (angleInMinPiPi > M_PI) {
110  // Substract 2 PI to be in interval [-Pi; Pi]
111  angleInMinPiPi -= 2. * M_PI;
112  }
113  return angleInMinPiPi;
114 }
115 
124 vpColVector stateAdd(const vpColVector &state, const vpColVector &toAdd)
125 {
126  vpColVector add = state + toAdd;
127  add[2] = normalizeAngle(add[2]);
128  return add;
129 }
130 
138 vpColVector stateMean(const std::vector<vpColVector> &states, const std::vector<double> &wm, const vpParticleFilter<vpColVector>::vpStateAddFunction &/*addFunc*/)
139 {
140  vpColVector mean(3, 0.);
141  unsigned int nbPoints = static_cast<unsigned int>(states.size());
142  double sumCos = 0.;
143  double sumSin = 0.;
144  for (unsigned int i = 0; i < nbPoints; ++i) {
145  mean[0] += wm[i] * states[i][0];
146  mean[1] += wm[i] * states[i][1];
147  sumCos += wm[i] * std::cos(states[i][2]);
148  sumSin += wm[i] * std::sin(states[i][2]);
149  }
150  mean[2] = std::atan2(sumSin, sumCos);
151  return mean;
152 }
153 
163 std::vector<vpColVector> generateTurnCommands(const double &v, const double &angleStart, const double &angleStop, const unsigned int &nbSteps)
164 {
165  std::vector<vpColVector> cmds;
166  double dTheta = vpMath::rad(angleStop - angleStart) / static_cast<double>(nbSteps - 1);
167  for (unsigned int i = 0; i < nbSteps; ++i) {
168  double theta = vpMath::rad(angleStart) + dTheta * static_cast<double>(i);
169  vpColVector cmd(2);
170  cmd[0] = v;
171  cmd[1] = theta;
172  cmds.push_back(cmd);
173  }
174  return cmds;
175 }
176 
182 std::vector<vpColVector> generateCommands()
183 {
184  std::vector<vpColVector> cmds;
185  // Starting by an straight line acceleration
186  unsigned int nbSteps = 30;
187  double dv = (1.1 - 0.001) / static_cast<double>(nbSteps - 1);
188  for (unsigned int i = 0; i < nbSteps; ++i) {
189  vpColVector cmd(2);
190  cmd[0] = 0.001 + static_cast<double>(i) * dv;
191  cmd[1] = 0.;
192  cmds.push_back(cmd);
193  }
194 
195  // Left turn
196  double lastLinearVelocity = cmds[cmds.size() -1][0];
197  std::vector<vpColVector> leftTurnCmds = generateTurnCommands(lastLinearVelocity, 0, 2, 15);
198  cmds.insert(cmds.end(), leftTurnCmds.begin(), leftTurnCmds.end());
199  for (unsigned int i = 0; i < 100; ++i) {
200  cmds.push_back(cmds[cmds.size() -1]);
201  }
202 
203  // Right turn
204  lastLinearVelocity = cmds[cmds.size() -1][0];
205  std::vector<vpColVector> rightTurnCmds = generateTurnCommands(lastLinearVelocity, 2, -2, 15);
206  cmds.insert(cmds.end(), rightTurnCmds.begin(), rightTurnCmds.end());
207  for (unsigned int i = 0; i < 200; ++i) {
208  cmds.push_back(cmds[cmds.size() -1]);
209  }
210 
211  // Left turn again
212  lastLinearVelocity = cmds[cmds.size() -1][0];
213  leftTurnCmds = generateTurnCommands(lastLinearVelocity, -2, 0, 15);
214  cmds.insert(cmds.end(), leftTurnCmds.begin(), leftTurnCmds.end());
215  for (unsigned int i = 0; i < 150; ++i) {
216  cmds.push_back(cmds[cmds.size() -1]);
217  }
218 
219  lastLinearVelocity = cmds[cmds.size() -1][0];
220  leftTurnCmds = generateTurnCommands(lastLinearVelocity, 0, 1, 25);
221  cmds.insert(cmds.end(), leftTurnCmds.begin(), leftTurnCmds.end());
222  for (unsigned int i = 0; i < 150; ++i) {
223  cmds.push_back(cmds[cmds.size() -1]);
224  }
225  return cmds;
226 }
227 }
228 
238 vpColVector computeMotionFromCommand(const vpColVector &u, const vpColVector &x, const double &dt, const double &w)
239 {
240  double heading = x[2];
241  double vel = u[0];
242  double steeringAngle = u[1];
243  double distance = vel * dt;
244 
245  if (std::abs(steeringAngle) > 0.001) {
246  // The robot is turning
247  double beta = (distance / w) * std::tan(steeringAngle);
248  double radius = w / std::tan(steeringAngle);
249  double sinh = std::sin(heading);
250  double sinhb = std::sin(heading + beta);
251  double cosh = std::cos(heading);
252  double coshb = std::cos(heading + beta);
253  vpColVector motion(3);
254  motion[0] = -radius * sinh + radius * sinhb;
255  motion[1] = radius * cosh - radius * coshb;
256  motion[2] = beta;
257  return motion;
258  }
259  else {
260  // The robot is moving in straight line
261  vpColVector motion(3);
262  motion[0] = distance * std::cos(heading);
263  motion[1] = distance * std::sin(heading);
264  motion[2] = 0.;
265  return motion;
266  }
267 }
268 
276 {
277 public:
283  vpProcessFunctor(const double &w)
284  : m_w(w)
285  { }
286 
294  vpColVector processFunction(const vpColVector &x, const double &dt)
295  {
296  vpColVector motion = computeMotionFromCommand(m_u, x, dt, m_w);
297  vpColVector newState = x + motion;
298  newState[2] = normalizeAngle(newState[2]);
299  return newState;
300  }
301 
307  void setCommands(const vpColVector &u)
308  {
309  m_u = u;
310  }
311 private:
312  double m_w;
313  vpColVector m_u;
314 };
315 
319 class vpBicycleModel
320 {
321 public:
327  vpBicycleModel(const double &w)
328  : m_w(w)
329  { }
330 
339  vpColVector computeMotion(const vpColVector &u, const vpColVector &x, const double &dt)
340  {
341  return computeMotionFromCommand(u, x, dt, m_w);
342  }
343 
353  vpColVector move(const vpColVector &u, const vpColVector &x, const double &dt)
354  {
355  vpColVector motion = computeMotion(u, x, dt);
356  vpColVector newX = x + motion;
357  newX[2] = normalizeAngle(newX[2]);
358  return newX;
359  }
360 private:
361  double m_w; // The length of the wheelbase.
362 };
363 
369 {
370 public:
379  vpLandmarkMeasurements(const double &x, const double &y, const double &range_std, const double &rel_angle_std)
380  : m_x(x)
381  , m_y(y)
382  , m_rngRange(range_std, 0., 4224)
383  , m_rngRelativeAngle(rel_angle_std, 0., 2112)
384  { }
385 
393  {
394  vpColVector meas(2);
395  double dx = m_x - particle[0];
396  double dy = m_y - particle[1];
397  meas[0] = std::sqrt(dx * dx + dy * dy);
398  meas[1] = normalizeAngle(std::atan2(dy, dx));
399  return meas;
400  }
401 
410  {
411  double dx = m_x - pos[0];
412  double dy = m_y - pos[1];
413  double range = std::sqrt(dx * dx + dy * dy);
414  double orientation = normalizeAngle(std::atan2(dy, dx));
415  vpColVector measurements(2);
416  measurements[0] = range;
417  measurements[1] = orientation;
418  return measurements;
419  }
420 
429  {
430  vpColVector measurementsGT = measureGT(pos);
431  vpColVector measurementsNoisy = measurementsGT;
432  measurementsNoisy[0] += m_rngRange();
433  measurementsNoisy[1] += m_rngRelativeAngle();
434  measurementsNoisy[1] = normalizeAngle(measurementsNoisy[1]);
435  return measurementsNoisy;
436  }
437 
445  void computePositionFromMeasurements(const vpColVector &meas, double &x, double &y)
446  {
447  double alpha = meas[1];
448  x = m_x - meas[0] * std::cos(alpha);
449  y = m_y - meas[0] * std::sin(alpha);
450  }
451 
452 private:
453  double m_x;
454  double m_y;
455  vpGaussRand m_rngRange;
456  vpGaussRand m_rngRelativeAngle;
457 };
458 
463 class vpLandmarksGrid
464 {
465 public:
472  vpLandmarksGrid(const std::vector<vpLandmarkMeasurements> &landmarks, const double &distMaxAllowed)
473  : m_landmarks(landmarks)
474  , m_nbLandmarks(static_cast<unsigned int>(landmarks.size()))
475  {
476  double sigmaDistance = distMaxAllowed / 3.;
477  double sigmaDistanceSquared = sigmaDistance * sigmaDistance;
478  m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared);
479  m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared);
480  }
481 
489  {
490  vpColVector measurements(2*m_nbLandmarks);
491  for (unsigned int i = 0; i < m_nbLandmarks; ++i) {
492  vpColVector landmarkMeas = m_landmarks[i].state_to_measurement(particle);
493  measurements[2*i] = landmarkMeas[0];
494  measurements[(2*i) + 1] = landmarkMeas[1];
495  }
496  return measurements;
497  }
498 
508  {
509  vpColVector measurements(2*m_nbLandmarks);
510  for (unsigned int i = 0; i < m_nbLandmarks; ++i) {
511  vpColVector landmarkMeas = m_landmarks[i].measureGT(pos);
512  measurements[2*i] = landmarkMeas[0];
513  measurements[(2*i) + 1] = landmarkMeas[1];
514  }
515  return measurements;
516  }
517 
527  {
528  vpColVector measurements(2*m_nbLandmarks);
529  for (unsigned int i = 0; i < m_nbLandmarks; ++i) {
530  vpColVector landmarkMeas = m_landmarks[i].measureWithNoise(pos);
531  measurements[2*i] = landmarkMeas[0];
532  measurements[(2*i) + 1] = landmarkMeas[1];
533  }
534  return measurements;
535  }
536 
546  void computePositionFromMeasurements(const vpColVector &meas, double &x, double &y)
547  {
548  x = 0.;
549  y = 0.;
550  for (unsigned int i = 0; i < m_nbLandmarks; ++i) {
551  vpColVector landmarkMeas({ meas[2*i], meas[(2*i) + 1] });
552  double xLand = 0., yLand = 0.;
553  m_landmarks[i].computePositionFromMeasurements(landmarkMeas, xLand, yLand);
554  x += xLand;
555  y += yLand;
556  }
557  x /= static_cast<double>(m_nbLandmarks);
558  y /= static_cast<double>(m_nbLandmarks);
559  }
560 
572  double likelihood(const vpColVector &particle, const vpColVector &meas)
573  {
574  double meanX = 0., meanY = 0.;
575  computePositionFromMeasurements(meas, meanX, meanY);
576  double dx = meanX - particle[0];
577  double dy = meanY - particle[1];
578  double dist = std::sqrt(dx * dx + dy * dy);
579  double likelihood = std::exp(m_constantExpDenominator * dist) * m_constantDenominator;
580  likelihood = std::min(likelihood, 1.0); // Clamp to have likelihood <= 1.
581  likelihood = std::max(likelihood, 0.); // Clamp to have likelihood >= 0.
582  return likelihood;
583  }
584 
585 private:
586  std::vector<vpLandmarkMeasurements> m_landmarks;
587  const unsigned int m_nbLandmarks;
588  double m_constantDenominator; // Denominator of the Gaussian function used in the likelihood computation.
589  double m_constantExpDenominator; // Denominator of the exponential in the Gaussian function used in the likelihood computation.
590 };
591 
593 {
594  // --- Main loop parameters---
595  static const int SOFTWARE_CONTINUE = 42;
597  unsigned int m_nbStepsWarmUp;
598  // --- PF parameters---
599  unsigned int m_N;
601  double m_ampliMaxX;
602  double m_ampliMaxY;
604  long m_seedPF;
606 
608  : m_useDisplay(true)
609  , m_nbStepsWarmUp(200)
610  , m_N(500)
611  , m_maxDistanceForLikelihood(0.5)
612  , m_ampliMaxX(0.25)
613  , m_ampliMaxY(0.25)
614  , m_ampliMaxTheta(0.1)
615  , m_seedPF(4224)
616  , m_nbThreads(1)
617  { }
618 
619  int parseArgs(const int argc, const char *argv[])
620  {
621  int i = 1;
622  while (i < argc) {
623  std::string arg(argv[i]);
624  if ((arg == "--nb-steps-warmup") && ((i+1) < argc)) {
625  m_nbStepsWarmUp = std::atoi(argv[i + 1]);
626  ++i;
627  }
628  else if ((arg == "--max-distance-likelihood") && ((i+1) < argc)) {
629  m_maxDistanceForLikelihood = std::atof(argv[i + 1]);
630  ++i;
631  }
632  else if (((arg == "-N") || (arg == "--nb-particles")) && ((i+1) < argc)) {
633  m_N = std::atoi(argv[i + 1]);
634  ++i;
635  }
636  else if ((arg == "--seed") && ((i+1) < argc)) {
637  m_seedPF = std::atoi(argv[i + 1]);
638  ++i;
639  }
640  else if ((arg == "--nb-threads") && ((i+1) < argc)) {
641  m_nbThreads = std::atoi(argv[i + 1]);
642  ++i;
643  }
644  else if ((arg == "--ampli-max-X") && ((i+1) < argc)) {
645  m_ampliMaxX = std::atof(argv[i + 1]);
646  ++i;
647  }
648  else if ((arg == "--ampli-max-Y") && ((i+1) < argc)) {
649  m_ampliMaxY = std::atof(argv[i + 1]);
650  ++i;
651  }
652  else if ((arg == "--ampli-max-theta") && ((i+1) < argc)) {
653  m_ampliMaxTheta = std::atof(argv[i + 1]);
654  ++i;
655  }
656  else if (arg == "-d") {
657  m_useDisplay = false;
658  }
659  else if ((arg == "-h") || (arg == "--help")) {
660  printUsage(std::string(argv[0]));
661  SoftwareArguments defaultArgs;
662  defaultArgs.printDetails();
663  return 0;
664  }
665  else {
666  std::cout << "WARNING: unrecognised argument \"" << arg << "\"";
667  if (i + 1 < argc) {
668  std::cout << " with associated value(s) { ";
669  int nbValues = 0;
670  int j = i + 1;
671  bool hasToRun = true;
672  while ((j < argc) && hasToRun) {
673  std::string nextValue(argv[j]);
674  if (nextValue.find("--") == std::string::npos) {
675  std::cout << nextValue << " ";
676  ++nbValues;
677  }
678  else {
679  hasToRun = false;
680  }
681  ++j;
682  }
683  std::cout << "}" << std::endl;
684  i += nbValues;
685  }
686  }
687  ++i;
688  }
689  return SOFTWARE_CONTINUE;
690  }
691 
692 private:
693  void printUsage(const std::string &softName)
694  {
695  std::cout << "SYNOPSIS" << std::endl;
696  std::cout << " " << softName << " [--nb-steps-warmup <uint>]" << std::endl;
697  std::cout << " [--max-distance-likelihood <double>] [-N, --nb-particles <uint>] [--seed <int>] [--nb-threads <int>]" << std::endl;
698  std::cout << " [--ampli-max-X <double>] [--ampli-max-Y <double>] [--ampli-max-theta <double>]" << std::endl;
699  std::cout << " [-d, --no-display] [-h]" << std::endl;
700  }
701 
702  void printDetails()
703  {
704  std::cout << std::endl << std::endl;
705  std::cout << "DETAILS" << std::endl;
706  std::cout << " --nb-steps-warmup" << std::endl;
707  std::cout << " Number of steps in the warmup loop." << std::endl;
708  std::cout << " Default: " << m_nbStepsWarmUp << std::endl;
709  std::cout << std::endl;
710  std::cout << " --max-distance-likelihood" << std::endl;
711  std::cout << " Maximum distance between a particle and the average position computed from the measurements." << std::endl;
712  std::cout << " Above this value, the likelihood of the particle is 0." << std::endl;
713  std::cout << " Default: " << m_maxDistanceForLikelihood << std::endl;
714  std::cout << std::endl;
715  std::cout << " -N, --nb-particles" << std::endl;
716  std::cout << " Number of particles of the Particle Filter." << std::endl;
717  std::cout << " Default: " << m_N << std::endl;
718  std::cout << std::endl;
719  std::cout << " --seed" << std::endl;
720  std::cout << " Seed to initialize the Particle Filter." << std::endl;
721  std::cout << " Use a negative value makes to use the current timestamp instead." << std::endl;
722  std::cout << " Default: " << m_seedPF << std::endl;
723  std::cout << std::endl;
724  std::cout << " --nb-threads" << std::endl;
725  std::cout << " Set the number of threads to use in the Particle Filter (only if OpenMP is available)." << std::endl;
726  std::cout << " Use a negative value to use the maximum number of threads instead." << std::endl;
727  std::cout << " Default: " << m_nbThreads << std::endl;
728  std::cout << std::endl;
729  std::cout << " --ampli-max-X" << std::endl;
730  std::cout << " Maximum amplitude of the noise added to a particle along the X-axis." << std::endl;
731  std::cout << " Default: " << m_ampliMaxX << std::endl;
732  std::cout << std::endl;
733  std::cout << " --ampli-max-Y" << std::endl;
734  std::cout << " Maximum amplitude of the noise added to a particle along the Y-axis." << std::endl;
735  std::cout << " Default: " << m_ampliMaxY << std::endl;
736  std::cout << std::endl;
737  std::cout << " --ampli-max-theta" << std::endl;
738  std::cout << " Maximum amplitude of the noise added to a particle affecting the heading of the robot." << std::endl;
739  std::cout << " Default: " << m_ampliMaxTheta << std::endl;
740  std::cout << std::endl;
741  std::cout << " -d, --no-display" << std::endl;
742  std::cout << " Deactivate display." << std::endl;
743  std::cout << " Default: display is ";
744 #ifdef VISP_HAVE_DISPLAY
745  std::cout << "ON" << std::endl;
746 #else
747  std::cout << "OFF" << std::endl;
748 #endif
749  std::cout << std::endl;
750  std::cout << " -h, --help" << std::endl;
751  std::cout << " Display this help." << std::endl;
752  std::cout << std::endl;
753  }
754 };
755 
756 int main(const int argc, const char *argv[])
757 {
758  SoftwareArguments args;
759  int returnCode = args.parseArgs(argc, argv);
760  if (returnCode != SoftwareArguments::SOFTWARE_CONTINUE) {
761  return returnCode;
762  }
763 
764  const double dt = 0.1; // Period of 0.1s
765  const double step = 1.; // Number of update of the robot position between two PF filtering
766  const double sigmaRange = 0.3; // Standard deviation of the range measurement: 0.3m
767  const double sigmaBearing = vpMath::rad(0.5); // Standard deviation of the bearing angle: 0.5deg
768  const double wheelbase = 0.5; // Wheelbase of 0.5m
769  const std::vector<vpLandmarkMeasurements> landmarks = { vpLandmarkMeasurements(5, 10, sigmaRange, sigmaBearing)
770  , vpLandmarkMeasurements(10, 5, sigmaRange, sigmaBearing)
771  , vpLandmarkMeasurements(15, 15, sigmaRange, sigmaBearing)
772  , vpLandmarkMeasurements(20, 5, sigmaRange, sigmaBearing)
773  , vpLandmarkMeasurements(0, 30, sigmaRange, sigmaBearing)
774  , vpLandmarkMeasurements(50, 30, sigmaRange, sigmaBearing)
775  , vpLandmarkMeasurements(40, 10, sigmaRange, sigmaBearing) }; // Vector of landmarks constituting the grid
776  std::vector<vpColVector> cmds = generateCommands();
777  const unsigned int nbCmds = static_cast<unsigned int>(cmds.size());
778 
779  // Initialize the attributes of the PF
780  std::vector<double> stdevsPF = { args.m_ampliMaxX / 3., args.m_ampliMaxY / 3., args.m_ampliMaxTheta / 3. };
781  int seedPF = args.m_seedPF;
782  unsigned int nbParticles = args.m_N;
783  int nbThreads = args.m_nbThreads;
784 
785  vpColVector X0(3);
786  X0[0] = 2.; // x = 2m
787  X0[1] = 6.; // y = 6m
788  X0[2] = 0.3; // robot orientation = 0.3 rad
789 
790  vpProcessFunctor processFtor(wheelbase);
791  vpLandmarksGrid grid(landmarks, args.m_maxDistanceForLikelihood);
792  vpBicycleModel robot(wheelbase);
793  using std::placeholders::_1;
794  using std::placeholders::_2;
796  vpParticleFilter<vpColVector>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLandmarksGrid::likelihood, &grid, _1, _2);
799  vpParticleFilter<vpColVector>::vpFilterFunction weightedMeanFunc = stateMean;
801 
802  // Initialize the PF
803  vpParticleFilter<vpColVector> filter(nbParticles, stdevsPF, seedPF, nbThreads);
804  filter.init(X0, f, likelihoodFunc, checkResamplingFunc, resamplingFunc, weightedMeanFunc, addFunc);
805 
806 #ifdef VISP_HAVE_DISPLAY
807  vpPlot *plot = nullptr;
808  if (args.m_useDisplay) {
809  // Initialize the plot
810  plot = new vpPlot(1);
811  plot->initGraph(0, 3);
812  plot->setTitle(0, "Position of the robot");
813  plot->setUnitX(0, "Position along x(m)");
814  plot->setUnitY(0, "Position along y (m)");
815  plot->setLegend(0, 0, "GT");
816  plot->setLegend(0, 1, "Filtered");
817  plot->setLegend(0, 2, "Measure");
818  plot->setColor(0, 0, vpColor::red);
819  plot->setColor(0, 1, vpColor::blue);
820  plot->setColor(0, 2, vpColor::black);
821  }
822 #endif
823 
824  // Initialize the simulation
825  vpColVector robot_pos = X0;
826  vpColVector noMotionCommand(2, 0.);
827 
828  // Warm-up step
829  double averageFilteringTime = 0.;
830  for (unsigned int i = 0; i < args.m_nbStepsWarmUp; ++i) {
831  // Perform the measurement
832  vpColVector z = grid.measureWithNoise(robot_pos);
833 
834  double t0 = vpTime::measureTimeMicros();
836  // Update the functor command
837  processFtor.setCommands(noMotionCommand);
838  // Use the PF to filter the measurement
839  filter.filter(z, dt);
841  averageFilteringTime += vpTime::measureTimeMicros() - t0;
842  }
843 
844  double meanErrorFilter = 0., meanErrorNoise = 0.;
845  for (unsigned int i = 0; i < nbCmds; ++i) {
846  robot_pos = robot.move(cmds[i], robot_pos, dt / step);
847  if (i % static_cast<int>(step) == 0) {
848  // Perform the measurement
849  vpColVector z = grid.measureWithNoise(robot_pos);
850 
851  double t0 = vpTime::measureTimeMicros();
853  // Update the functor command
854  processFtor.setCommands(cmds[i]);
855  // Use the PF to filter the measurement
856  filter.filter(z, dt);
858  averageFilteringTime += vpTime::measureTimeMicros() - t0;
859 
861  vpColVector Xest = filter.computeFilteredState();
863 
865  // Compute the error between the filtered state and the Ground Truth
866  // to have statistics at the end of the program
867  double dxFilter = Xest[0] - robot_pos[0];
868  double dyFilter = Xest[1] - robot_pos[1];
869  double errorFilter = std::sqrt(dxFilter * dxFilter + dyFilter * dyFilter);
870  meanErrorFilter += errorFilter;
871 
872  // Compute the error between the noisy measurements and the Ground Truth
873  // to have statistics at the end of the program
874  double xMeas = 0., yMeas = 0.;
875  grid.computePositionFromMeasurements(z, xMeas, yMeas);
876  double dxMeas = xMeas - robot_pos[0];
877  double dyMeas = yMeas - robot_pos[1];
878  meanErrorNoise += std::sqrt(dxMeas * dxMeas + dyMeas * dyMeas);
879 
880 #ifdef VISP_HAVE_DISPLAY
881  if (args.m_useDisplay) {
882  // Plot the filtered state
883  plot->plot(0, 1, Xest[0], Xest[1]);
884  plot->plot(0, 2, xMeas, yMeas);
885  }
886 #endif
887  }
888 
889 #ifdef VISP_HAVE_DISPLAY
890  if (args.m_useDisplay) {
891  // Plot the ground truth
892  plot->plot(0, 0, robot_pos[0], robot_pos[1]);
893  }
894 #endif
895  }
896 
897  // Display the statistics that were computed
898  averageFilteringTime = averageFilteringTime / (static_cast<double>(nbCmds + args.m_nbStepsWarmUp));
899  meanErrorFilter = meanErrorFilter / (static_cast<double>(nbCmds));
900  meanErrorNoise = meanErrorNoise / (static_cast<double>(nbCmds));
901  std::cout << "Mean error filter = " << meanErrorFilter << std::endl;
902  std::cout << "Mean error noise = " << meanErrorNoise << std::endl;
903  std::cout << "Mean filtering time = " << averageFilteringTime << "us" << std::endl;
904 
905  if (args.m_useDisplay) {
906  std::cout << "Press Enter to quit..." << std::endl;
907  std::cin.get();
908  }
909 
910 #ifdef VISP_HAVE_DISPLAY
911  if (args.m_useDisplay) {
912  delete plot;
913  }
914 #endif
915 
916  // Check if the results are the one expected, when this program is used for the unit tests
917  const double maxError = 0.15;
918  if (meanErrorFilter > meanErrorNoise) {
919  std::cerr << "Error: noisy measurements error = " << meanErrorNoise << ", filter error = " << meanErrorFilter << std::endl;
920  return -1;
921  }
922  else if (meanErrorFilter > maxError) {
923  std::cerr << "Error: max tolerated error = " << maxError << ", average error = " << meanErrorFilter << std::endl;
924  return -1;
925  }
926  return 0;
927 }
928 #else
929 int main()
930 {
931  std::cout << "This example is only available if you compile ViSP in C++11 standard or higher." << std::endl;
932  return 0;
933 }
934 #endif
Class that approximates a 4-wheel robot using a bicycle model.
vpColVector computeMotion(const vpColVector &u, const vpColVector &x, const double &dt)
Models the effect of the command on the state model.
vpBicycleModel(const double &w)
Construct a new vpBicycleModel object.
vpColVector move(const vpColVector &u, const vpColVector &x, const double &dt)
Move the robot according to its current position and the commands.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
static const vpColor red
Definition: vpColor.h:198
static const vpColor blue
Definition: vpColor.h:204
static const vpColor black
Definition: vpColor.h:192
Class for generating random number with normal probability density.
Definition: vpGaussRand.h:117
Class that permits to convert the position + heading of the 4-wheel robot into measurements from a la...
vpColVector measureGT(const vpColVector &pos)
Perfect measurement of the range and relative orientation of the robot located at pos.
vpColVector state_to_measurement(const vpColVector &particle)
Convert a particle of the Particle Filter into the measurement space.
vpLandmarkMeasurements(const double &x, const double &y, const double &range_std, const double &rel_angle_std)
Construct a new vpLandmarkMeasurements object.
vpColVector measureWithNoise(const vpColVector &pos)
Noisy measurement of the range and relative orientation that correspond to pos.
void computePositionFromMeasurements(const vpColVector &meas, double &x, double &y)
Compute the position that corresponds to a measurement.
Class that represent a grid of landmarks that measure the distance and relative orientation of the 4-...
void computePositionFromMeasurements(const vpColVector &meas, double &x, double &y)
Compute the position that corresponds to a measurement. As the measurements can be noisy,...
vpColVector state_to_measurement(const vpColVector &particle)
Convert a particle of the Particle Filter into the measurement space.
vpLandmarksGrid(const std::vector< vpLandmarkMeasurements > &landmarks, const double &distMaxAllowed)
Construct a new vpLandmarksGrid object.
double likelihood(const vpColVector &particle, const vpColVector &meas)
Compute the likelihood of a particle (value between 0. and 1.) knowing the measurements....
vpColVector measureGT(const vpColVector &pos)
Perfect measurement from each landmark of the range and relative orientation of the robot located at ...
vpColVector measureWithNoise(const vpColVector &pos)
Noisy measurement from each landmark of the range and relative orientation that correspond to pos.
static double rad(double deg)
Definition: vpMath.h:129
static float modulo(const float &value, const float &modulo)
Gives the rest of value divided by modulo when the quotient can only be an integer.
Definition: vpMath.h:177
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 std::vector< vpColVector > &, const std::vector< double > &, const vpStateAddFunction &)> vpFilterFunction
Filter function, which computes the filtered state of the particle filter. The first argument is the ...
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....
std::function< vpColVector(const vpColVector &, const vpColVector &)> vpStateAddFunction
Function that computes either the equivalent of an addition in the state space. The first argument is...
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:112
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:203
void setUnitY(unsigned int graphNum, const std::string &unity)
Definition: vpPlot.cpp:530
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:552
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:270
void setUnitX(unsigned int graphNum, const std::string &unitx)
Definition: vpPlot.cpp:520
void setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
Definition: vpPlot.cpp:246
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:510
As the state model {x, y, } does not contain any velocity information, it does not evolve without com...
vpProcessFunctor(const double &w)
Construct a new vp Process Functor object.
vpColVector processFunction(const vpColVector &x, const double &dt)
Models the effect of the command on the state model.
void setCommands(const vpColVector &u)
Set the Commands object.
VISP_EXPORT double measureTimeMicros()
bool m_useDisplay
If true, activate the plot and the renderer if VISP_HAVE_DISPLAY is defined.
int m_nbThreads
Number of thread to use in the Particle Filter.
double m_ampliMaxTheta
Amplitude max of the noise for the state component corresponding to the heading.
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...
int parseArgs(const int argc, const char *argv[])
unsigned int m_nbStepsWarmUp
Number of steps for the warmup phase.
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.