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>
88 #include <visp3/core/vpParticleFilter.h>
91 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
93 #ifdef ENABLE_VISP_NAMESPACE
105 double normalizeAngle(
const double &angle)
108 double angleInMinPiPi = angleIn0to2pi;
109 if (angleInMinPiPi > M_PI) {
111 angleInMinPiPi -= 2. * M_PI;
113 return angleInMinPiPi;
127 add[2] = normalizeAngle(add[2]);
141 unsigned int nbPoints =
static_cast<unsigned int>(states.size());
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]);
150 mean[2] = std::atan2(sumSin, sumCos);
163 std::vector<vpColVector> generateTurnCommands(
const double &v,
const double &angleStart,
const double &angleStop,
const unsigned int &nbSteps)
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);
182 std::vector<vpColVector> generateCommands()
184 std::vector<vpColVector> cmds;
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) {
190 cmd[0] = 0.001 +
static_cast<double>(i) * dv;
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]);
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]);
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]);
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]);
240 double heading = x[2];
242 double steeringAngle = u[1];
243 double distance = vel * dt;
245 if (std::abs(steeringAngle) > 0.001) {
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);
254 motion[0] = -radius * sinh + radius * sinhb;
255 motion[1] = radius * cosh - radius * coshb;
262 motion[0] = distance * std::cos(heading);
263 motion[1] = distance * std::sin(heading);
296 vpColVector motion = computeMotionFromCommand(m_u, x, dt, m_w);
298 newState[2] = normalizeAngle(newState[2]);
341 return computeMotionFromCommand(u, x, dt, m_w);
357 newX[2] = normalizeAngle(newX[2]);
382 , m_rngRange(range_std, 0., 4224)
383 , m_rngRelativeAngle(rel_angle_std, 0., 2112)
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));
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));
416 measurements[0] = range;
417 measurements[1] = orientation;
432 measurementsNoisy[0] += m_rngRange();
433 measurementsNoisy[1] += m_rngRelativeAngle();
434 measurementsNoisy[1] = normalizeAngle(measurementsNoisy[1]);
435 return measurementsNoisy;
447 double alpha = meas[1];
448 x = m_x - meas[0] * std::cos(alpha);
449 y = m_y - meas[0] * std::sin(alpha);
472 vpLandmarksGrid(
const std::vector<vpLandmarkMeasurements> &landmarks,
const double &distMaxAllowed)
473 : m_landmarks(landmarks)
474 , m_nbLandmarks(static_cast<unsigned int>(landmarks.size()))
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);
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];
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];
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];
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);
557 x /=
static_cast<double>(m_nbLandmarks);
558 y /=
static_cast<double>(m_nbLandmarks);
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);
581 likelihood = std::max(likelihood, 0.);
586 std::vector<vpLandmarkMeasurements> m_landmarks;
587 const unsigned int m_nbLandmarks;
588 double m_constantDenominator;
589 double m_constantExpDenominator;
595 static const int SOFTWARE_CONTINUE = 42;
609 , m_nbStepsWarmUp(200)
611 , m_maxDistanceForLikelihood(0.5)
614 , m_ampliMaxTheta(0.1)
623 std::string arg(argv[i]);
624 if ((arg ==
"--nb-steps-warmup") && ((i+1) < argc)) {
625 m_nbStepsWarmUp = std::atoi(argv[i + 1]);
628 else if ((arg ==
"--max-distance-likelihood") && ((i+1) < argc)) {
629 m_maxDistanceForLikelihood = std::atof(argv[i + 1]);
632 else if (((arg ==
"-N") || (arg ==
"--nb-particles")) && ((i+1) < argc)) {
633 m_N = std::atoi(argv[i + 1]);
636 else if ((arg ==
"--seed") && ((i+1) < argc)) {
637 m_seedPF = std::atoi(argv[i + 1]);
640 else if ((arg ==
"--nb-threads") && ((i+1) < argc)) {
641 m_nbThreads = std::atoi(argv[i + 1]);
644 else if ((arg ==
"--ampli-max-X") && ((i+1) < argc)) {
645 m_ampliMaxX = std::atof(argv[i + 1]);
648 else if ((arg ==
"--ampli-max-Y") && ((i+1) < argc)) {
649 m_ampliMaxY = std::atof(argv[i + 1]);
652 else if ((arg ==
"--ampli-max-theta") && ((i+1) < argc)) {
653 m_ampliMaxTheta = std::atof(argv[i + 1]);
656 else if (arg ==
"-d") {
657 m_useDisplay =
false;
659 else if ((arg ==
"-h") || (arg ==
"--help")) {
660 printUsage(std::string(argv[0]));
662 defaultArgs.printDetails();
666 std::cout <<
"WARNING: unrecognised argument \"" << arg <<
"\"";
668 std::cout <<
" with associated value(s) { ";
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 <<
" ";
683 std::cout <<
"}" << std::endl;
689 return SOFTWARE_CONTINUE;
693 void printUsage(
const std::string &softName)
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;
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;
747 std::cout <<
"OFF" << std::endl;
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;
756 int main(
const int argc,
const char *argv[])
759 int returnCode = args.
parseArgs(argc, argv);
764 const double dt = 0.1;
765 const double step = 1.;
766 const double sigmaRange = 0.3;
768 const double wheelbase = 0.5;
769 const std::vector<vpLandmarkMeasurements> landmarks = {
vpLandmarkMeasurements(5, 10, sigmaRange, sigmaBearing)
776 std::vector<vpColVector> cmds = generateCommands();
777 const unsigned int nbCmds =
static_cast<unsigned int>(cmds.size());
782 unsigned int nbParticles = args.
m_N;
793 using std::placeholders::_1;
794 using std::placeholders::_2;
804 filter.init(X0, f, likelihoodFunc, checkResamplingFunc, resamplingFunc, weightedMeanFunc, addFunc);
806 #ifdef VISP_HAVE_DISPLAY
812 plot->
setTitle(0,
"Position of the robot");
813 plot->
setUnitX(0,
"Position along x(m)");
814 plot->
setUnitY(0,
"Position along y (m)");
829 double averageFilteringTime = 0.;
837 processFtor.setCommands(noMotionCommand);
839 filter.filter(z, dt);
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) {
854 processFtor.setCommands(cmds[i]);
856 filter.filter(z, dt);
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;
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);
880 #ifdef VISP_HAVE_DISPLAY
883 plot->
plot(0, 1, Xest[0], Xest[1]);
884 plot->
plot(0, 2, xMeas, yMeas);
889 #ifdef VISP_HAVE_DISPLAY
892 plot->
plot(0, 0, robot_pos[0], robot_pos[1]);
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;
906 std::cout <<
"Press Enter to quit..." << std::endl;
910 #ifdef VISP_HAVE_DISPLAY
917 const double maxError = 0.15;
918 if (meanErrorFilter > meanErrorNoise) {
919 std::cerr <<
"Error: noisy measurements error = " << meanErrorNoise <<
", filter error = " << meanErrorFilter << std::endl;
922 else if (meanErrorFilter > maxError) {
923 std::cerr <<
"Error: max tolerated error = " << maxError <<
", average error = " << meanErrorFilter << std::endl;
931 std::cout <<
"This example is only available if you compile ViSP in C++11 standard or higher." << std::endl;
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.
static const vpColor blue
static const vpColor black
Class for generating random number with normal probability density.
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)
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.
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...
void initGraph(unsigned int graphNum, unsigned int curveNbr)
void setUnitY(unsigned int graphNum, const std::string &unity)
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
void setUnitX(unsigned int graphNum, const std::string &unitx)
void setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
void setTitle(unsigned int graphNum, const std::string &title)
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.
static const int SOFTWARE_CONTINUE
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.