Visual Servoing Platform  version 3.6.1 under development (2024-10-15)
pf-nonlinear-complex-example.cpp

Example of a complex non-linear use-case of the Particle Filter (PF). The system we are interested in is a 4-wheel robot, moving at a low velocity. As such, it can be modeled using a bicycle model.

The state vector of the PF is:

\[ \begin{array}{lcl} \textbf{x}[0] &=& x \\ \textbf{x}[1] &=& y \\ \textbf{x}[2] &=& \theta \end{array} \]

where $ \theta $ is the heading of the robot.

The measurement $ \textbf{z} $ corresponds to the distance and relative orientation of the robot with different landmarks. Be $ p_x^i $ and $ p_y^i $ the position of the $ i^{th} $ landmark along the x and y axis, the measurement vector can be written as:

\[ \begin{array}{lcl} \textbf{z}[2i] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\ \textbf{z}[2i+1] &=& \tan^{-1}{\frac{p_y^i - y}{p_x^i - x}} - \theta \end{array} \]

Some noise is added to the measurement vector to simulate measurements which are not perfect.

The mean of several angles must be computed in the Particle Fitler inference. The definition we chose to use is the following:

$ mean(\boldsymbol{\theta}) = atan2 (\frac{\sum_{i=1}^n \sin{\theta_i}}{n}, \frac{\sum_{i=1}^n \cos{\theta_i}}{n}) $

As the Particle Filter inference uses a weighted mean, the actual implementation of the weighted mean of several angles is the following:

$ mean_{weighted}(\boldsymbol{\theta}) = atan2 (\sum_{i=1}^n w_m^i \sin{\theta_i}, \sum_{i=1}^n w_m^i \cos{\theta_i}) $

where $ w_m^i $ is the weight associated to the $ i^{th} $ measurements for the weighted mean.

Additionally, the addition and subtraction of angles must be carefully done, as the result must stay in the interval $[- \pi ; \pi ]$ or $[0 ; 2 \pi ]$ . We decided to use the interval $[- \pi ; \pi ]$ .

/*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2024 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See https://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*/
#include <iostream>
// ViSP includes
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpGaussRand.h>
#ifdef VISP_HAVE_DISPLAY
#include <visp3/gui/vpPlot.h>
#endif
// PF includes
#include <visp3/core/vpParticleFilter.h>
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
namespace
{
double normalizeAngle(const double &angle)
{
double angleIn0to2pi = vpMath::modulo(angle, 2. * M_PI);
double angleInMinPiPi = angleIn0to2pi;
if (angleInMinPiPi > M_PI) {
// Substract 2 PI to be in interval [-Pi; Pi]
angleInMinPiPi -= 2. * M_PI;
}
return angleInMinPiPi;
}
vpColVector stateAdd(const vpColVector &state, const vpColVector &toAdd)
{
vpColVector add = state + toAdd;
add[2] = normalizeAngle(add[2]);
return add;
}
vpColVector stateMean(const std::vector<vpColVector> &states, const std::vector<double> &wm, const vpParticleFilter<vpColVector>::vpStateAddFunction &/*addFunc*/)
{
vpColVector mean(3, 0.);
unsigned int nbPoints = static_cast<unsigned int>(states.size());
double sumCos = 0.;
double sumSin = 0.;
for (unsigned int i = 0; i < nbPoints; ++i) {
mean[0] += wm[i] * states[i][0];
mean[1] += wm[i] * states[i][1];
sumCos += wm[i] * std::cos(states[i][2]);
sumSin += wm[i] * std::sin(states[i][2]);
}
mean[2] = std::atan2(sumSin, sumCos);
return mean;
}
std::vector<vpColVector> generateTurnCommands(const double &v, const double &angleStart, const double &angleStop, const unsigned int &nbSteps)
{
std::vector<vpColVector> cmds;
double dTheta = vpMath::rad(angleStop - angleStart) / static_cast<double>(nbSteps - 1);
for (unsigned int i = 0; i < nbSteps; ++i) {
double theta = vpMath::rad(angleStart) + dTheta * static_cast<double>(i);
vpColVector cmd(2);
cmd[0] = v;
cmd[1] = theta;
cmds.push_back(cmd);
}
return cmds;
}
std::vector<vpColVector> generateCommands()
{
std::vector<vpColVector> cmds;
// Starting by an straight line acceleration
unsigned int nbSteps = 30;
double dv = (1.1 - 0.001) / static_cast<double>(nbSteps - 1);
for (unsigned int i = 0; i < nbSteps; ++i) {
vpColVector cmd(2);
cmd[0] = 0.001 + static_cast<double>(i) * dv;
cmd[1] = 0.;
cmds.push_back(cmd);
}
// Left turn
double lastLinearVelocity = cmds[cmds.size() -1][0];
std::vector<vpColVector> leftTurnCmds = generateTurnCommands(lastLinearVelocity, 0, 2, 15);
cmds.insert(cmds.end(), leftTurnCmds.begin(), leftTurnCmds.end());
for (unsigned int i = 0; i < 100; ++i) {
cmds.push_back(cmds[cmds.size() -1]);
}
// Right turn
lastLinearVelocity = cmds[cmds.size() -1][0];
std::vector<vpColVector> rightTurnCmds = generateTurnCommands(lastLinearVelocity, 2, -2, 15);
cmds.insert(cmds.end(), rightTurnCmds.begin(), rightTurnCmds.end());
for (unsigned int i = 0; i < 200; ++i) {
cmds.push_back(cmds[cmds.size() -1]);
}
// Left turn again
lastLinearVelocity = cmds[cmds.size() -1][0];
leftTurnCmds = generateTurnCommands(lastLinearVelocity, -2, 0, 15);
cmds.insert(cmds.end(), leftTurnCmds.begin(), leftTurnCmds.end());
for (unsigned int i = 0; i < 150; ++i) {
cmds.push_back(cmds[cmds.size() -1]);
}
lastLinearVelocity = cmds[cmds.size() -1][0];
leftTurnCmds = generateTurnCommands(lastLinearVelocity, 0, 1, 25);
cmds.insert(cmds.end(), leftTurnCmds.begin(), leftTurnCmds.end());
for (unsigned int i = 0; i < 150; ++i) {
cmds.push_back(cmds[cmds.size() -1]);
}
return cmds;
}
}
vpColVector computeMotionFromCommand(const vpColVector &u, const vpColVector &x, const double &dt, const double &w)
{
double heading = x[2];
double vel = u[0];
double steeringAngle = u[1];
double distance = vel * dt;
if (std::abs(steeringAngle) > 0.001) {
// The robot is turning
double beta = (distance / w) * std::tan(steeringAngle);
double radius = w / std::tan(steeringAngle);
double sinh = std::sin(heading);
double sinhb = std::sin(heading + beta);
double cosh = std::cos(heading);
double coshb = std::cos(heading + beta);
vpColVector motion(3);
motion[0] = -radius * sinh + radius * sinhb;
motion[1] = radius * cosh - radius * coshb;
motion[2] = beta;
return motion;
}
else {
// The robot is moving in straight line
vpColVector motion(3);
motion[0] = distance * std::cos(heading);
motion[1] = distance * std::sin(heading);
motion[2] = 0.;
return motion;
}
}
{
public:
vpProcessFunctor(const double &w)
: m_w(w)
{ }
vpColVector processFunction(const vpColVector &x, const double &dt)
{
vpColVector motion = computeMotionFromCommand(m_u, x, dt, m_w);
vpColVector newState = x + motion;
newState[2] = normalizeAngle(newState[2]);
return newState;
}
void setCommands(const vpColVector &u)
{
m_u = u;
}
private:
double m_w;
};
{
public:
vpBicycleModel(const double &w)
: m_w(w)
{ }
vpColVector computeMotion(const vpColVector &u, const vpColVector &x, const double &dt)
{
return computeMotionFromCommand(u, x, dt, m_w);
}
vpColVector move(const vpColVector &u, const vpColVector &x, const double &dt)
{
vpColVector motion = computeMotion(u, x, dt);
vpColVector newX = x + motion;
newX[2] = normalizeAngle(newX[2]);
return newX;
}
private:
double m_w; // The length of the wheelbase.
};
{
public:
vpLandmarkMeasurements(const double &x, const double &y, const double &range_std, const double &rel_angle_std)
: m_x(x)
, m_y(y)
, m_rngRange(range_std, 0., 4224)
, m_rngRelativeAngle(rel_angle_std, 0., 2112)
{ }
vpColVector state_to_measurement(const vpColVector &particle)
{
vpColVector meas(2);
double dx = m_x - particle[0];
double dy = m_y - particle[1];
meas[0] = std::sqrt(dx * dx + dy * dy);
meas[1] = normalizeAngle(std::atan2(dy, dx));
return meas;
}
vpColVector measureGT(const vpColVector &pos)
{
double dx = m_x - pos[0];
double dy = m_y - pos[1];
double range = std::sqrt(dx * dx + dy * dy);
double orientation = normalizeAngle(std::atan2(dy, dx));
vpColVector measurements(2);
measurements[0] = range;
measurements[1] = orientation;
return measurements;
}
vpColVector measureWithNoise(const vpColVector &pos)
{
vpColVector measurementsGT = measureGT(pos);
vpColVector measurementsNoisy = measurementsGT;
measurementsNoisy[0] += m_rngRange();
measurementsNoisy[1] += m_rngRelativeAngle();
measurementsNoisy[1] = normalizeAngle(measurementsNoisy[1]);
return measurementsNoisy;
}
void computePositionFromMeasurements(const vpColVector &meas, double &x, double &y)
{
double alpha = meas[1];
x = m_x - meas[0] * std::cos(alpha);
y = m_y - meas[0] * std::sin(alpha);
}
private:
double m_x;
double m_y;
vpGaussRand m_rngRange;
vpGaussRand m_rngRelativeAngle;
};
{
public:
vpLandmarksGrid(const std::vector<vpLandmarkMeasurements> &landmarks, const double &distMaxAllowed)
: m_landmarks(landmarks)
, m_nbLandmarks(static_cast<unsigned int>(landmarks.size()))
{
double sigmaDistance = distMaxAllowed / 3.;
double sigmaDistanceSquared = sigmaDistance * sigmaDistance;
m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared);
m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared);
}
vpColVector state_to_measurement(const vpColVector &particle)
{
vpColVector measurements(2*m_nbLandmarks);
for (unsigned int i = 0; i < m_nbLandmarks; ++i) {
vpColVector landmarkMeas = m_landmarks[i].state_to_measurement(particle);
measurements[2*i] = landmarkMeas[0];
measurements[(2*i) + 1] = landmarkMeas[1];
}
return measurements;
}
vpColVector measureGT(const vpColVector &pos)
{
vpColVector measurements(2*m_nbLandmarks);
for (unsigned int i = 0; i < m_nbLandmarks; ++i) {
vpColVector landmarkMeas = m_landmarks[i].measureGT(pos);
measurements[2*i] = landmarkMeas[0];
measurements[(2*i) + 1] = landmarkMeas[1];
}
return measurements;
}
vpColVector measureWithNoise(const vpColVector &pos)
{
vpColVector measurements(2*m_nbLandmarks);
for (unsigned int i = 0; i < m_nbLandmarks; ++i) {
vpColVector landmarkMeas = m_landmarks[i].measureWithNoise(pos);
measurements[2*i] = landmarkMeas[0];
measurements[(2*i) + 1] = landmarkMeas[1];
}
return measurements;
}
void computePositionFromMeasurements(const vpColVector &meas, double &x, double &y)
{
x = 0.;
y = 0.;
for (unsigned int i = 0; i < m_nbLandmarks; ++i) {
vpColVector landmarkMeas({ meas[2*i], meas[(2*i) + 1] });
double xLand = 0., yLand = 0.;
m_landmarks[i].computePositionFromMeasurements(landmarkMeas, xLand, yLand);
x += xLand;
y += yLand;
}
x /= static_cast<double>(m_nbLandmarks);
y /= static_cast<double>(m_nbLandmarks);
}
double likelihood(const vpColVector &particle, const vpColVector &meas)
{
double meanX = 0., meanY = 0.;
computePositionFromMeasurements(meas, meanX, meanY);
double dx = meanX - particle[0];
double dy = meanY - particle[1];
double dist = std::sqrt(dx * dx + dy * dy);
double likelihood = std::exp(m_constantExpDenominator * dist) * m_constantDenominator;
likelihood = std::min(likelihood, 1.0); // Clamp to have likelihood <= 1.
likelihood = std::max(likelihood, 0.); // Clamp to have likelihood >= 0.
return likelihood;
}
private:
std::vector<vpLandmarkMeasurements> m_landmarks;
const unsigned int m_nbLandmarks;
double m_constantDenominator; // Denominator of the Gaussian function used in the likelihood computation.
double m_constantExpDenominator; // Denominator of the exponential in the Gaussian function used in the likelihood computation.
};
{
// --- Main loop parameters---
static const int SOFTWARE_CONTINUE = 42;
bool m_useDisplay;
unsigned int m_nbStepsWarmUp;
// --- PF parameters---
unsigned int m_N;
double m_maxDistanceForLikelihood;
double m_ampliMaxX;
double m_ampliMaxY;
double m_ampliMaxTheta;
long m_seedPF;
int m_nbThreads;
: m_useDisplay(true)
, m_nbStepsWarmUp(200)
, m_N(500)
, m_maxDistanceForLikelihood(0.5)
, m_ampliMaxX(0.25)
, m_ampliMaxY(0.25)
, m_ampliMaxTheta(0.1)
, m_seedPF(4224)
, m_nbThreads(1)
{ }
int parseArgs(const int argc, const char *argv[])
{
int i = 1;
while (i < argc) {
std::string arg(argv[i]);
if ((arg == "--nb-steps-warmup") && ((i+1) < argc)) {
m_nbStepsWarmUp = std::atoi(argv[i + 1]);
++i;
}
else if ((arg == "--max-distance-likelihood") && ((i+1) < argc)) {
m_maxDistanceForLikelihood = std::atof(argv[i + 1]);
++i;
}
else if (((arg == "-N") || (arg == "--nb-particles")) && ((i+1) < argc)) {
m_N = std::atoi(argv[i + 1]);
++i;
}
else if ((arg == "--seed") && ((i+1) < argc)) {
m_seedPF = std::atoi(argv[i + 1]);
++i;
}
else if ((arg == "--nb-threads") && ((i+1) < argc)) {
m_nbThreads = std::atoi(argv[i + 1]);
++i;
}
else if ((arg == "--ampli-max-X") && ((i+1) < argc)) {
m_ampliMaxX = std::atof(argv[i + 1]);
++i;
}
else if ((arg == "--ampli-max-Y") && ((i+1) < argc)) {
m_ampliMaxY = std::atof(argv[i + 1]);
++i;
}
else if ((arg == "--ampli-max-theta") && ((i+1) < argc)) {
m_ampliMaxTheta = std::atof(argv[i + 1]);
++i;
}
else if (arg == "-d") {
m_useDisplay = false;
}
else if ((arg == "-h") || (arg == "--help")) {
printUsage(std::string(argv[0]));
SoftwareArguments defaultArgs;
defaultArgs.printDetails();
return 0;
}
else {
std::cout << "WARNING: unrecognised argument \"" << arg << "\"";
if (i + 1 < argc) {
std::cout << " with associated value(s) { ";
int nbValues = 0;
int j = i + 1;
bool hasToRun = true;
while ((j < argc) && hasToRun) {
std::string nextValue(argv[j]);
if (nextValue.find("--") == std::string::npos) {
std::cout << nextValue << " ";
++nbValues;
}
else {
hasToRun = false;
}
++j;
}
std::cout << "}" << std::endl;
i += nbValues;
}
}
++i;
}
return SOFTWARE_CONTINUE;
}
private:
void printUsage(const std::string &softName)
{
std::cout << "SYNOPSIS" << std::endl;
std::cout << " " << softName << " [--nb-steps-warmup <uint>]" << std::endl;
std::cout << " [--max-distance-likelihood <double>] [-N, --nb-particles <uint>] [--seed <int>] [--nb-threads <int>]" << std::endl;
std::cout << " [--ampli-max-X <double>] [--ampli-max-Y <double>] [--ampli-max-theta <double>]" << std::endl;
std::cout << " [-d, --no-display] [-h]" << std::endl;
}
void printDetails()
{
std::cout << std::endl << std::endl;
std::cout << "DETAILS" << std::endl;
std::cout << " --nb-steps-warmup" << std::endl;
std::cout << " Number of steps in the warmup loop." << std::endl;
std::cout << " Default: " << m_nbStepsWarmUp << std::endl;
std::cout << std::endl;
std::cout << " --max-distance-likelihood" << std::endl;
std::cout << " Maximum distance between a particle and the average position computed from the measurements." << std::endl;
std::cout << " Above this value, the likelihood of the particle is 0." << std::endl;
std::cout << " Default: " << m_maxDistanceForLikelihood << std::endl;
std::cout << std::endl;
std::cout << " -N, --nb-particles" << std::endl;
std::cout << " Number of particles of the Particle Filter." << std::endl;
std::cout << " Default: " << m_N << std::endl;
std::cout << std::endl;
std::cout << " --seed" << std::endl;
std::cout << " Seed to initialize the Particle Filter." << std::endl;
std::cout << " Use a negative value makes to use the current timestamp instead." << std::endl;
std::cout << " Default: " << m_seedPF << std::endl;
std::cout << std::endl;
std::cout << " --nb-threads" << std::endl;
std::cout << " Set the number of threads to use in the Particle Filter (only if OpenMP is available)." << std::endl;
std::cout << " Use a negative value to use the maximum number of threads instead." << std::endl;
std::cout << " Default: " << m_nbThreads << std::endl;
std::cout << std::endl;
std::cout << " --ampli-max-X" << std::endl;
std::cout << " Maximum amplitude of the noise added to a particle along the X-axis." << std::endl;
std::cout << " Default: " << m_ampliMaxX << std::endl;
std::cout << std::endl;
std::cout << " --ampli-max-Y" << std::endl;
std::cout << " Maximum amplitude of the noise added to a particle along the Y-axis." << std::endl;
std::cout << " Default: " << m_ampliMaxY << std::endl;
std::cout << std::endl;
std::cout << " --ampli-max-theta" << std::endl;
std::cout << " Maximum amplitude of the noise added to a particle affecting the heading of the robot." << std::endl;
std::cout << " Default: " << m_ampliMaxTheta << std::endl;
std::cout << std::endl;
std::cout << " -d, --no-display" << std::endl;
std::cout << " Deactivate display." << std::endl;
std::cout << " Default: display is ";
#ifdef VISP_HAVE_DISPLAY
std::cout << "ON" << std::endl;
#else
std::cout << "OFF" << std::endl;
#endif
std::cout << std::endl;
std::cout << " -h, --help" << std::endl;
std::cout << " Display this help." << std::endl;
std::cout << std::endl;
}
};
int main(const int argc, const char *argv[])
{
int returnCode = args.parseArgs(argc, argv);
return returnCode;
}
const double dt = 0.1; // Period of 0.1s
const double step = 1.; // Number of update of the robot position between two PF filtering
const double sigmaRange = 0.3; // Standard deviation of the range measurement: 0.3m
const double sigmaBearing = vpMath::rad(0.5); // Standard deviation of the bearing angle: 0.5deg
const double wheelbase = 0.5; // Wheelbase of 0.5m
const std::vector<vpLandmarkMeasurements> landmarks = { vpLandmarkMeasurements(5, 10, sigmaRange, sigmaBearing)
, vpLandmarkMeasurements(10, 5, sigmaRange, sigmaBearing)
, vpLandmarkMeasurements(15, 15, sigmaRange, sigmaBearing)
, vpLandmarkMeasurements(20, 5, sigmaRange, sigmaBearing)
, vpLandmarkMeasurements(0, 30, sigmaRange, sigmaBearing)
, vpLandmarkMeasurements(50, 30, sigmaRange, sigmaBearing)
, vpLandmarkMeasurements(40, 10, sigmaRange, sigmaBearing) }; // Vector of landmarks constituting the grid
std::vector<vpColVector> cmds = generateCommands();
const unsigned int nbCmds = static_cast<unsigned int>(cmds.size());
// Initialize the attributes of the PF
std::vector<double> stdevsPF = { args.m_ampliMaxX / 3., args.m_ampliMaxY / 3., args.m_ampliMaxTheta / 3. };
int seedPF = args.m_seedPF;
unsigned int nbParticles = args.m_N;
int nbThreads = args.m_nbThreads;
vpColVector X0(3);
X0[0] = 2.; // x = 2m
X0[1] = 6.; // y = 6m
X0[2] = 0.3; // robot orientation = 0.3 rad
vpProcessFunctor processFtor(wheelbase);
vpBicycleModel robot(wheelbase);
using std::placeholders::_1;
using std::placeholders::_2;
// Initialize the PF
vpParticleFilter<vpColVector> filter(nbParticles, stdevsPF, seedPF, nbThreads);
filter.init(X0, f, likelihoodFunc, checkResamplingFunc, resamplingFunc, weightedMeanFunc, addFunc);
#ifdef VISP_HAVE_DISPLAY
vpPlot *plot = nullptr;
if (args.m_useDisplay) {
// Initialize the plot
plot = new vpPlot(1);
plot->initGraph(0, 3);
plot->setTitle(0, "Position of the robot");
plot->setUnitX(0, "Position along x(m)");
plot->setUnitY(0, "Position along y (m)");
plot->setLegend(0, 0, "GT");
plot->setLegend(0, 1, "Filtered");
plot->setLegend(0, 2, "Measure");
plot->setColor(0, 0, vpColor::red);
plot->setColor(0, 1, vpColor::blue);
plot->setColor(0, 2, vpColor::black);
}
#endif
// Initialize the simulation
vpColVector robot_pos = X0;
vpColVector noMotionCommand(2, 0.);
// Warm-up step
double averageFilteringTime = 0.;
for (unsigned int i = 0; i < args.m_nbStepsWarmUp; ++i) {
// Perform the measurement
vpColVector z = grid.measureWithNoise(robot_pos);
// Update the functor command
processFtor.setCommands(noMotionCommand);
// Use the PF to filter the measurement
filter.filter(z, dt);
averageFilteringTime += vpTime::measureTimeMicros() - t0;
}
double meanErrorFilter = 0., meanErrorNoise = 0.;
for (unsigned int i = 0; i < nbCmds; ++i) {
robot_pos = robot.move(cmds[i], robot_pos, dt / step);
if (i % static_cast<int>(step) == 0) {
// Perform the measurement
vpColVector z = grid.measureWithNoise(robot_pos);
// Update the functor command
processFtor.setCommands(cmds[i]);
// Use the PF to filter the measurement
filter.filter(z, dt);
averageFilteringTime += vpTime::measureTimeMicros() - t0;
vpColVector Xest = filter.computeFilteredState();
// Compute the error between the filtered state and the Ground Truth
// to have statistics at the end of the program
double dxFilter = Xest[0] - robot_pos[0];
double dyFilter = Xest[1] - robot_pos[1];
double errorFilter = std::sqrt(dxFilter * dxFilter + dyFilter * dyFilter);
meanErrorFilter += errorFilter;
// Compute the error between the noisy measurements and the Ground Truth
// to have statistics at the end of the program
double xMeas = 0., yMeas = 0.;
grid.computePositionFromMeasurements(z, xMeas, yMeas);
double dxMeas = xMeas - robot_pos[0];
double dyMeas = yMeas - robot_pos[1];
meanErrorNoise += std::sqrt(dxMeas * dxMeas + dyMeas * dyMeas);
#ifdef VISP_HAVE_DISPLAY
if (args.m_useDisplay) {
// Plot the filtered state
plot->plot(0, 1, Xest[0], Xest[1]);
plot->plot(0, 2, xMeas, yMeas);
}
#endif
}
#ifdef VISP_HAVE_DISPLAY
if (args.m_useDisplay) {
// Plot the ground truth
plot->plot(0, 0, robot_pos[0], robot_pos[1]);
}
#endif
}
// Display the statistics that were computed
averageFilteringTime = averageFilteringTime / (static_cast<double>(nbCmds + args.m_nbStepsWarmUp));
meanErrorFilter = meanErrorFilter / (static_cast<double>(nbCmds));
meanErrorNoise = meanErrorNoise / (static_cast<double>(nbCmds));
std::cout << "Mean error filter = " << meanErrorFilter << std::endl;
std::cout << "Mean error noise = " << meanErrorNoise << std::endl;
std::cout << "Mean filtering time = " << averageFilteringTime << "us" << std::endl;
if (args.m_useDisplay) {
std::cout << "Press Enter to quit..." << std::endl;
std::cin.get();
}
#ifdef VISP_HAVE_DISPLAY
if (args.m_useDisplay) {
delete plot;
}
#endif
// Check if the results are the one expected, when this program is used for the unit tests
const double maxError = 0.15;
if (meanErrorFilter > meanErrorNoise) {
std::cerr << "Error: noisy measurements error = " << meanErrorNoise << ", filter error = " << meanErrorFilter << std::endl;
return -1;
}
else if (meanErrorFilter > maxError) {
std::cerr << "Error: max tolerated error = " << maxError << ", average error = " << meanErrorFilter << std::endl;
return -1;
}
return 0;
}
#else
int main()
{
std::cout << "This example is only available if you compile ViSP in C++11 standard or higher." << std::endl;
return 0;
}
#endif
Class that approximates a 4-wheel robot using a bicycle model.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
static const vpColor red
Definition: vpColor.h:217
static const vpColor blue
Definition: vpColor.h:223
static const vpColor black
Definition: vpColor.h:211
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...
Class that represent a grid of landmarks that measure the distance and relative orientation of the 4-...
double likelihood(const vpColVector &particle, const vpColVector &meas)
Compute the likelihood of a particle (value between 0. and 1.) knowing the measurements....
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...
vpColVector processFunction(const vpColVector &x, const double &dt)
Models the effect of the command on the state model.
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.