diff --git a/doc/image/tutorial/misc/img-tutorial-pf-run.jpg b/doc/image/tutorial/misc/img-tutorial-pf-run.jpg
index 291a562402..e3292d79d1 100644
Binary files a/doc/image/tutorial/misc/img-tutorial-pf-run.jpg and b/doc/image/tutorial/misc/img-tutorial-pf-run.jpg differ
diff --git a/doc/tutorial/misc/tutorial-pf.dox b/doc/tutorial/misc/tutorial-pf.dox
index 78f8d7e46f..6ce37a48e7 100644
--- a/doc/tutorial/misc/tutorial-pf.dox
+++ b/doc/tutorial/misc/tutorial-pf.dox
@@ -8,7 +8,8 @@ The Particle Filters (PF) are a set of Monte Carlo algorithms that
permit to approximate solutions for filtering problems even when
the state-space and/or measurement space are non-linear.
-In this tutorial, we will use a PF to filter the 3D position of a simulated object, which revolves in a plane parallel
+In this tutorial, we will use a PF on the same use-case than presented in \ref tutorial-ukf . The PF is used to
+filter the 3D position of a simulated object, which revolves in a plane parallel
to the ground around a static point, which is the origin of the world frame \f$ {F}_W \f$. The coordinate frame
attached to the object is denoted \f$ {F}_O \f$. The object is observed by a static camera whose coordinate
frame is denoted \f$ {F}_C \f$. The object is supposed plane and having four markers sticked on its surface.
@@ -35,7 +36,7 @@ radius of the revolution around the origin of the world frame.
The maths beyond the Particle Filter are explained in the documentation of the vpParticleFilter class.
We will recall briefly the important steps of the PF.
-Be \f$ \textbf{x}_i \in \textit{S} \f$ a particle representing the internal state of the PF, with \f$ i \in {0 \dots N - 1} \f$
+Be \f$ \textbf{x}_i \in \textit{S} \f$ a particle representing the internal state of the PF, with \f$ i \in \{0 \dots N - 1\} \f$
and \f$ \textit{S} \f$ the state space.
To each particle is associated a weight \f$ w_i \f$ that represents its likelihood knowing the measurements and is used
to compute the filtered state \f$ \textbf{x}_{filtered} \in \textit{S} \f$.
@@ -247,7 +248,7 @@ It takes as input the camera parameters cam, the homogeneous matrix
expresses the rotation between the object frame and world frame wRo and the homogeneous coordinates of the
markers expressed in the object frame markers to be able to convert the 3D position of the object in the
world frame \f$ {}^W \textbf{X} \f$ into 3D positions of the markers in the camera frame \f$ {}^C \textbf{X}^i \f$, where
-\f$ i \f$ denotes the i\f$^th\f$ marker sticked on the object. The standard deviation of the noise noise_stdev
+\f$ i \f$ denotes the i\f$^{th}\f$ marker sticked on the object. The standard deviation of the noise noise_stdev
and the seed value seed are here to initialized the Gaussian noise generator used to simulate noisy measurements.
Additionally, the likelihood standard deviation \f$\sigma_l\f$ is given for the computation of the likelihood of a PF
particle knowing the measurements.
@@ -285,7 +286,7 @@ Here is the corresponding code:
The method computePose compute the 3D pose of an object from the 3D coordinates along with their projection
in the image. Here, we use it to convert the noisy measurements in a noisy 3D pose, in order to
-compare the 3D position estimated by the UKF with regard to the 3D position we would have if we computed the pose directly
+compare the 3D position estimated by the PF and by the UKF with regard to the 3D position we would have if we computed the pose directly
from the noisy measurements.
\snippet tutorial-pf.cpp Pose_for_display
@@ -315,58 +316,55 @@ generate camera parameters for a simulated camera as follow:
\snippet tutorial-pf.cpp Camera_for_measurements
-\subsubsection tuto-pf-tutorial-explained-initukf Details on the initialization of the UKF
+\subsubsection tuto-pf-tutorial-explained-initpf Details on the initialization of the PF
-To initialize the UKF, we need an object that will be able to compute the sigma points and their associated weights. To
-do so, we must create an instance of a class inheriting from the class vpUKSigmaDrawerAbstract. In our case, we decided
-to use the method proposed by E. A. Wan and R. van der Merwe in \cite Merwe00 and implemented in the vpUKSigmaDrawerMerwe class:
+To create the particle filter, we need:
+- the number of particles \f$ N \f$ we want to use,
+- the standard deviations of each of the components of the state \f$ \sigma_j , j \in \{0 \dots dim(\textit{S}) - 1\} \f$,
+- optionnally, the seed to use to create the random noise generators affected to each state components,
+- optionnally, the number of threads to use if OpenMP is available.
-\snippet tutorial-pf.cpp Sigma_points_drawer
+These parameters can be set using the Command Line Interface (CLI) thanks to the following structure:
-The first parameter is the dimension of the state of the UKF. The second parameter is \f$ \alpha \f$: the greater it is
-the further of the mean the sigma points are; it is a real and its value must be between 0 and 1. The third parameter is
-\f$ \beta \f$: it is a real whose value is set to two for Gaussian problems. Finally, the last parameter is \f$ \kappa \f$:
-it is a real whose value must be set to \f$ 3 - n \f$ for most problems.
+\snippet tutorial-pf.cpp CLI
-The UKF needs the covariance matrix of the measurements \f$ \textbf{R} \f$ that represents the uncertainty of the
-measurements:
+They are thereafter used in the following section of code of the main function:
-\snippet tutorial-pf.cpp Covariance_measurements
+\snippet tutorial-pf.cpp Constants_for_the_PF
-The UKF needs the covariance matrix of the process \f$ \textbf{Q} \f$ that represents the uncertainty induced during the
-prediction step:
+Then, to initialize the filters, we need:
+- a guess of the initial state \f$ \textbf{x}(t = 0) \f$,
+- a process function \f$ f \f$,
+- a likelihood function \f$ l \f$,
+- a function that returns true if the filter is degenerated and sampling is needed,
+- a function that performs the resampling,
+- optionnally, a function to perform the weighted mean \f$ wm \f$ if the addition operation cannot be readily performed
+in the state space \f$ \textit{S} \f$,
+- optionnally, a function to perform the addition operation in the state space \f$ \textit{S} \f$.
-\snippet tutorial-pf.cpp Covariance_process
+The section of code corresponding to the declaration of these functions is the following:
-The UKF needs an estimate of the intial state \f$ \textbf{x}_0 \f$ and of its covariance \f$ \textbf{P}_0 \f$:
+\snippet tutorial-pf.cpp Init_functions_pf
-\snippet tutorial-pf.cpp Initial_estimates
+When both the constants and the functions have been declared, it is possible to create the PF
+using the following code:
-Next, we initialize the process function and the measurement function:
+\snippet tutorial-pf.cpp Init_PF
-\snippet tutorial-pf.cpp Init_functions
+\subsubsection tuto-pf-tutorial-explained-initukf Initialization of the UKF, used for comparison purpose
-Finally, we create the UKF and initialize its state:
+We refer the user to \ref tuto-ukf-tutorial-explained-initukf for more detailed explanations on the initialization
+of the UKF, as this tutorial uses the same use-case. The code corresponding to the creation and initialization
+of the UKF is the following:
\snippet tutorial-pf.cpp Init_UKF
-If the internal state cannot use the standard addition and subtraction, it would be necessary to write other methods:
-- a method permitting to add two state vectors (see vpUnscentedKalman::setStateAddFunction),
-- a method permitting to subtract two state vectors (see vpUnscentedKalman::setStateResidualFunction),
-- and a method permitting to compute the mean of several state vectors (see vpUnscentedKalman::setStateMeanFunction).
-
-If some commands are known to have an effect on the internal state, it would be necessary to write other methods:
-- a method for the effects of the commands on the state, without knowing the state (see vpUnscentedKalman::setCommandOnlyFunction),
-- and/or a method for the effects of the commands on the state, knowing the state (see vpUnscentedKalman::setCommandStateFunction).
-
-If the measurement space cannot use the standard addition and subtraction, it would be necessary to write other methods:
-- a method permitting to subtract two measurement vectors (see vpUnscentedKalman::setMeasurementResidualFunction),
-- and a method permitting to compute the mean of several measurement vectors (see vpUnscentedKalman::setMeasurementMeanFunction).
-
\subsubsection tuto-pf-tutorial-explained-initgui Details on the initialization of the Graphical User Interface
If ViSP has been compiled with any of the third-party graphical libraries, we first begin by initializing the
-plot that will display the object x and y coordinates expressed in the world frame:
+plot that will display the object x and y coordinates expressed in the world frame. Then, we initialize a plot that will
+display the error norm between either one of the filtered positions or the noisy position and the Ground Truth position.
+The corresponding code is the following:
\snippet tutorial-pf.cpp Init_plot
@@ -378,7 +376,7 @@ Then, we initialize the simple renderer that displays what the camera sees:
For the initialization of the loop, we initialize an instance of the vpObjectSimulator class that
simulates the moving object. Then, we initialize the current ground-truth 3D position of the object expressed in the
-world frame, which is the frame in which the internal state of the UKF is expressed, as a null homogeneous coordinates
+world frame, which is the frame in which the internal states of both the PF and the UKF are expressed, as a null homogeneous coordinates
vector.
\snippet tutorial-pf.cpp Init_simu
@@ -398,9 +396,13 @@ some noise to the projections using the following line:
\snippet tutorial-pf.cpp Update_measurement
-Then, we use the Unscented Kalman Filter to filter the noisy measurements:
+Then, we use the Particle Filter to filter the noisy measurements:
+
+\snippet tutorial-pf.cpp PF_filtering
-\snippet tutorial-pf.cpp Perform_filtering
+Then, we use the Unscented Kalman Filter to filter the noisy measurements to compare the results:
+
+\snippet tutorial-pf.cpp UKF_filtering
Finally, we update the plot and renderer:
@@ -419,11 +421,5 @@ Finally, we update the renderer that displays the projection in the image of the
\snippet tutorial-pf.cpp Update_renderer
-\subsubsection tuto-pf-tutorial-explained-cleaning Details on the cleaning at the end of the program
-
-Finally, we clean the allocated memory for the renderer:
-
-\snippet tutorial-pf.cpp Delete_renderer
-
The program stops once the `Return` key is pressed.
*/
diff --git a/doc/tutorial/misc/tutorial-ukf.dox b/doc/tutorial/misc/tutorial-ukf.dox
index b8ed268bb4..b74081370c 100644
--- a/doc/tutorial/misc/tutorial-ukf.dox
+++ b/doc/tutorial/misc/tutorial-ukf.dox
@@ -270,7 +270,7 @@ It takes as input the camera parameters cam, the homogeneous matrix
expresses the rotation between the object frame and world frame wRo and the homogeneous coordinates of the
markers expressed in the object frame markers to be able to convert the 3D position of the object in the
world frame \f$ {}^W \textbf{X} \f$ into 3D positions of the markers in the camera frame \f$ {}^C \textbf{X}^i \f$, where
-\f$ i \f$ denotes the i\f$^th\f$ marker sticked on the object. The standard deviation of the noise noise_stdev
+\f$ i \f$ denotes the i\f$^{th}\f$ marker sticked on the object. The standard deviation of the noise noise_stdev
and the seed value seed are here to initialized the Gaussian noise generator used to simulate noisy measurements.
The method state_to_measurement is used to convert the internal state of the UKF into the measurement space
diff --git a/tutorial/particle-filter/tutorial-pf.cpp b/tutorial/particle-filter/tutorial-pf.cpp
index a8d810c06a..ccce9d841a 100644
--- a/tutorial/particle-filter/tutorial-pf.cpp
+++ b/tutorial/particle-filter/tutorial-pf.cpp
@@ -349,6 +349,7 @@ class vpMarkersMeasurements
};
//! [Markers_class]
+//! [CLI]
struct SoftwareArguments
{
// --- Main loop parameters---
@@ -532,6 +533,7 @@ struct SoftwareArguments
std::cout << std::endl;
}
};
+//! [CLI]
int main(const int argc, const char *argv[])
{
@@ -582,36 +584,7 @@ int main(const int argc, const char *argv[])
cam.initPersProjWithoutDistortion(px, py, u0, v0);
//! [Camera_for_measurements]
- // Initialize the attributes of the UKF
- //! [Sigma_points_drawer]
- std::shared_ptr drawer = std::make_shared(4, 0.001, 2., -1);
- //! [Sigma_points_drawer]
-
- //! [Covariance_measurements]
- vpMatrix R1landmark(2, 2, 0.); // The covariance of the noise introduced by the measurement with 1 landmark
- R1landmark[0][0] = sigmaMeasurements*sigmaMeasurements;
- R1landmark[1][1] = sigmaMeasurements*sigmaMeasurements;
- vpMatrix R(2*nbMarkers, 2 * nbMarkers);
- for (unsigned int i = 0; i < nbMarkers; ++i) {
- R.insert(R1landmark, 2*i, 2*i);
- }
- //! [Covariance_measurements]
-
- //! [Covariance_process]
- const double processVariance = 0.000025; // Variance of the process of (0.005cm)^2
- vpMatrix Q; // The covariance of the process
- Q.eye(4);
- Q = Q * processVariance;
- //! [Covariance_process]
-
//! [Initial_estimates]
- vpMatrix P0(4, 4); // The initial guess of the process covariance
- P0.eye(4);
- P0[0][0] = 1.;
- P0[1][1] = 1.;
- P0[2][2] = 1.;
- P0[2][2] = 5.;
-
vpColVector X0(4); // The initial guess for the state
X0[0] = 0.95 * radius * std::cos(phi); // Wrong estimation of the position along the X-axis = 5% of error
X0[1] = 0.95 * radius * std::sin(phi); // Wrong estimation of the position along the Y-axis = 5% of error
@@ -636,22 +609,12 @@ int main(const int argc, const char *argv[])
}
//! [Constants_for_the_PF]
- //! [Init_functions_ukf]
- vpUnscentedKalman::vpProcessFunction f = fx;
+ // Object that converts the pose of the object into measurements
vpMarkersMeasurements markerMeas(cam, cMw, wRo, markers, sigmaMeasurements, seed, sigmaLikelihood);
- using std::placeholders::_1;
- vpUnscentedKalman::vpMeasurementFunction h = std::bind(&vpMarkersMeasurements::state_to_measurement, &markerMeas, _1);
- //! [Init_functions_ukf]
-
- //! [Init_UKF]
- // Initialize the UKF
- vpUnscentedKalman ukf(Q, R, drawer, f, h);
- ukf.init(X0, P0);
- //! [Init_UKF]
//! [Init_functions_pf]
vpParticleFilter::vpProcessFunction processFunc = fx;
- // using std::placeholders::_1;
+ using std::placeholders::_1;
using std::placeholders::_2;
vpParticleFilter::vpLikelihoodFunction likelihoodFunc = std::bind(&vpMarkersMeasurements::likelihood, &markerMeas, _1, _2);
vpParticleFilter::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter::simpleResamplingCheck;
@@ -664,6 +627,43 @@ int main(const int argc, const char *argv[])
pfFilter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc);
//! [Init_PF]
+ //! [Init_UKF]
+ // Initialize the attributes of the UKF
+ // Sigma point drawer
+ std::shared_ptr drawer = std::make_shared(4, 0.001, 2., -1);
+
+ // Measurements covariance
+ vpMatrix R1landmark(2, 2, 0.); // The covariance of the noise introduced by the measurement with 1 landmark
+ R1landmark[0][0] = sigmaMeasurements*sigmaMeasurements;
+ R1landmark[1][1] = sigmaMeasurements*sigmaMeasurements;
+ vpMatrix R(2*nbMarkers, 2 * nbMarkers);
+ for (unsigned int i = 0; i < nbMarkers; ++i) {
+ R.insert(R1landmark, 2*i, 2*i);
+ }
+
+ // Process covariance
+ const double processVariance = 0.000025; // Variance of the process of (0.005cm)^2
+ vpMatrix Q; // The noise introduced during the prediction step
+ Q.eye(4);
+ Q = Q * processVariance;
+
+ // Process covariance initial guess
+ vpMatrix P0(4, 4);
+ P0.eye(4);
+ P0[0][0] = 1.;
+ P0[1][1] = 1.;
+ P0[2][2] = 1.;
+ P0[2][2] = 5.;
+
+ // Functions for the UKF
+ vpUnscentedKalman::vpProcessFunction f = fx;
+ vpUnscentedKalman::vpMeasurementFunction h = std::bind(&vpMarkersMeasurements::state_to_measurement, &markerMeas, _1);
+
+ // Initialize the UKF
+ vpUnscentedKalman ukf(Q, R, drawer, f, h);
+ ukf.init(X0, P0);
+ //! [Init_UKF]
+
//! [Init_plot]
#ifdef VISP_HAVE_DISPLAY
// Initialize the plot
@@ -673,25 +673,25 @@ int main(const int argc, const char *argv[])
plot.setUnitX(0, "Position along x(m)");
plot.setUnitY(0, "Position along y (m)");
plot.setLegend(0, 0, "GT");
- plot.setLegend(0, 1, "UKF");
- plot.setLegend(0, 2, "PF");
+ plot.setLegend(0, 1, "PF");
+ plot.setLegend(0, 2, "UKF");
plot.setLegend(0, 3, "Measure");
plot.initRange(0, -1.25 * radius, 1.25 * radius, -1.25 * radius, 1.25 * radius);
plot.setColor(0, 0, vpColor::red);
- plot.setColor(0, 1, vpColor::blue);
- plot.setColor(0, 2, vpColor::purple);
+ plot.setColor(0, 1, vpColor::purple);
+ plot.setColor(0, 2, vpColor::blue);
plot.setColor(0, 3, vpColor::black);
vpPlot plotError(1, 350, 700, 700, 700, "Error w.r.t. GT");
plotError.initGraph(0, 3);
plotError.setUnitX(0, "Time (s)");
plotError.setUnitY(0, "Error (m)");
- plotError.setLegend(0, 0, "UKF");
- plotError.setLegend(0, 1, "PF");
+ plotError.setLegend(0, 0, "PF");
+ plotError.setLegend(0, 1, "UKF");
plotError.setLegend(0, 2, "Measure");
plotError.initRange(0, 0, nbIter * dt, 0, radius / 2.);
- plotError.setColor(0, 0, vpColor::blue);
- plotError.setColor(0, 1, vpColor::purple);
+ plotError.setColor(0, 0, vpColor::purple);
+ plotError.setColor(0, 1, vpColor::blue);
plotError.setColor(0, 2, vpColor::black);
#endif
//! [Init_plot]
@@ -727,16 +727,19 @@ int main(const int argc, const char *argv[])
vpColVector z = markerMeas.measureWithNoise(object_pos);
//! [Update_measurement]
- //! [Perform_filtering]
- // Use the UKF to filter the measurement
- double tUKF = vpTime::measureTimeMs();
- ukf.filter(z, dt);
- double dtUKF = vpTime::measureTimeMs() - tUKF;
+ //! [PF_filtering]
/// Use the PF to filter the measurement
double tPF = vpTime::measureTimeMs();
pfFilter.filter(z, dt);
double dtPF = vpTime::measureTimeMs() - tPF;
- //! [Perform_filtering]
+ //! [PF_filtering]
+
+ //! [UKF_filtering]
+ // Use the UKF to filter the measurement for comparison
+ double tUKF = vpTime::measureTimeMs();
+ ukf.filter(z, dt);
+ double dtUKF = vpTime::measureTimeMs() - tUKF;
+ //! [UKF_filtering]
//! [Update_displays]
#ifdef VISP_HAVE_DISPLAY
@@ -760,13 +763,14 @@ int main(const int argc, const char *argv[])
// Plot the ground truth
plot.plot(0, 0, object_pos[0], object_pos[1]);
- // Plot the UKF filtered state
- vpColVector XestUKF = ukf.getXest();
- plot.plot(0, 1, XestUKF[0], XestUKF[1]);
-
// Plot the PF filtered state
vpColVector XestPF = pfFilter.computeFilteredState();
- plot.plot(0, 2, XestPF[0], XestPF[1]);
+ plot.plot(0, 1, XestPF[0], XestPF[1]);
+
+
+ // Plot the UKF filtered state
+ vpColVector XestUKF = ukf.getXest();
+ plot.plot(0, 2, XestUKF[0], XestUKF[1]);
// Plot the noisy pose
plot.plot(0, 3, wXnoisy, wYnoisy);
@@ -783,18 +787,19 @@ int main(const int argc, const char *argv[])
vpColVector error_PF = cX_PF - cX_GT;
vpColVector error_UKF = cX_UKF - cX_GT;
- std::cout << " [Unscented Kalman Filter method] " << std::endl;
- std::cout << " Mean square error = " << error_UKF.frobeniusNorm() << " m^2" << std::endl;
- std::cout << " Fitting duration = " << dtUKF << " ms" << std::endl;
std::cout << " [Particle Filter method] " << std::endl;
- std::cout << " Mean square error = " << error_PF.frobeniusNorm() << " m^2" << std::endl;
+ std::cout << " Norm of the error = " << error_PF.frobeniusNorm() << " m^2" << std::endl;
std::cout << " Fitting duration = " << dtPF << " ms" << std::endl;
- // Plot the UKF filtered state error
- plotError.plot(0, 0, t, error_UKF.frobeniusNorm());
+ std::cout << " [Unscented Kalman Filter method] " << std::endl;
+ std::cout << " Norm of the error = " << error_UKF.frobeniusNorm() << " m^2" << std::endl;
+ std::cout << " Fitting duration = " << dtUKF << " ms" << std::endl;
// Plot the PF filtered state error
- plotError.plot(0, 1, t, error_PF.frobeniusNorm());
+ plotError.plot(0, 0, t, error_PF.frobeniusNorm());
+
+ // Plot the UKF filtered state error
+ plotError.plot(0, 1, t, error_UKF.frobeniusNorm());
// Plot the noisy error
plotError.plot(0, 2, t, (cMo_noisy.getTranslationVector() - vpTranslationVector(cX_GT.extract(0, 3))).frobeniusNorm());
@@ -810,12 +815,12 @@ int main(const int argc, const char *argv[])
vpImagePoint markerProjGT(zGT[2*id + 1], zGT[2*id]);
vpDisplay::displayCross(Idisp, markerProjGT, 5, vpColor::red);
- vpImagePoint markerProjFiltUKF(zFiltUkf[2*id + 1], zFiltUkf[2*id]);
- vpDisplay::displayCross(Idisp, markerProjFiltUKF, 5, vpColor::blue);
-
vpImagePoint markerProjFiltPF(zFiltPF[2*id + 1], zFiltPF[2*id]);
vpDisplay::displayCross(Idisp, markerProjFiltPF, 5, vpColor::purple);
+ vpImagePoint markerProjFiltUKF(zFiltUkf[2*id + 1], zFiltUkf[2*id]);
+ vpDisplay::displayCross(Idisp, markerProjFiltUKF, 5, vpColor::blue);
+
vpImagePoint markerProjNoisy(z[2*id + 1], z[2*id]);
vpDisplay::displayCross(Idisp, markerProjNoisy, 5, vpColor::black);
}
@@ -823,10 +828,10 @@ int main(const int argc, const char *argv[])
vpImagePoint ipText(20, 20);
vpDisplay::displayText(Idisp, ipText, std::string("GT"), vpColor::red);
ipText.set_i(ipText.get_i() + 20);
- vpDisplay::displayText(Idisp, ipText, std::string("Filtered by UKF"), vpColor::blue);
- ipText.set_i(ipText.get_i() + 20);
vpDisplay::displayText(Idisp, ipText, std::string("Filtered by PF"), vpColor::purple);
ipText.set_i(ipText.get_i() + 20);
+ vpDisplay::displayText(Idisp, ipText, std::string("Filtered by UKF"), vpColor::blue);
+ ipText.set_i(ipText.get_i() + 20);
vpDisplay::displayText(Idisp, ipText, std::string("Measured"), vpColor::black);
vpDisplay::flush(Idisp);
vpTime::wait(40);