Introduction

We suppose that you are already familiar with the Tutorial: Using Unscented Kalman Filter to filter your data.

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 on the same use-case than presented in Tutorial: Using Unscented Kalman Filter to filter your data. 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}_W$ . The coordinate frame attached to the object is denoted ${F}_O$ . The object is observed by a static camera whose coordinate frame is denoted ${F}_C$ . The object is supposed plane and having four markers sticked on its surface.

The equations that describe the motion of the object in the world frame are the following:

$\begin{array}{lcl} {}^W \textbf{X}_x &=& R cos(\omega t + \phi) \\ {}^W \textbf{X}_y &=& R sin(\omega t + \phi) \\ {}^W \textbf{X}_z &=& constant \end{array}$

where $\omega$ and $\phi$ are respectively the pulsation and the phase of the motion, while is the radius of the revolution around the origin of the world frame.

The maths beyond the Particle Filter

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 $\textbf{x}_i \in \textit{S}$ a particle representing the internal state of the PF, with $i \in \{0 \dots N - 1\}$ and $\textit{S}$ the state space. To each particle is associated a weight that represents its likelihood knowing the measurements and is used to compute the filtered state $\textbf{x}_{filtered} \in \textit{S}$ .

The first step of the PF is the prediction step. During this step, the particles of the PF are projected forward in time. Be $f(\textbf{x}_i, \Delta t) : \textit{S} \times R \rightarrow \textit{S}$ the process function that project the forward in time. All the particles pass through the function , and some noise $\epsilon$ is independently added to each of them to form the new particles:

$\textbf{x}_i(t + \Delta t) = f( \textbf{x}_i(t) , \Delta t ) + \epsilon$

The second step of the PF is to update the weights associated to each particle based on new measurements. The update is based on the likelihood of a particle based on the measurements $\textbf{z} \in \textit{M}$ , where $\textit{M}$ is the measurement space. Be $l: \textit{S} \times \textit{M} \rightarrow [0; 1.]$ the likelihood function, we have:

$w_i = l(\textbf{x}_i, \textbf{z})$

After an update, a check is performed to see if the PF is not degenerated (i.e. if the weigths of most particles became very low). If the PF became degenerated, the particles are resampled depending on a resampling scheme. Different kind of checks and of resampling algorithms exist in the litterature.

Finally, we can compute the new state estimate $\textbf{x}_{filtered}$ by performing a weighted mean of the particles $\textbf{x}_i$ . Be $\textbf{w} = (w_0 \dots w_{N-1})^T \in R^N$ , $\textbf{x} = {\textbf{x}_0 \dots \textbf{x}_{N-1}} \in \textit{S}^N$ and $wm: R^N \times \textit{S}^N \rightarrow \textit{S}$ the weighted mean function of the state space $\textit{S}$ , we have:

$\textbf{x}_{filtered} = wm(\textbf{w}, \textbf{x})$

Explanations about the tutorial

How to run the tutorial

To run the tutorial, please run the following commands:

$ cd $VISP_WS/visp-build/tutorial/particle-filter

$ ./tutorial-pf

visp

Definition: vpIoTools.h:61

To see the arguments the program can take, please run:

$ cd $VISP_WS/visp-build/tutorial/particle-filter

$ ./tutorial-pf -h

You should see something similar to the following image:

Screenshot of the tutorial Graphical User Interface

Press Return to leave the program.

Detailed explanations about the PF tutorial

For this tutorial, we use the main program tutorial-pf.cpp . An Unscented Kalman Filter (UKF) is also implemented to compare the results of both methods. The internal state of both the PF and the UKF is the 3D position of the object expressed in the world frame, along with the pulsation $\omega$ of the motion:

$\begin{array}{lcl} \textbf{x}[0] &=& {}^WX_x \\ \textbf{x}[1] &=& {}^WX_y \\ \textbf{x}[2] &=& {}^WX_z \\ \textbf{x}[3] &=& \omega \Delta t \end{array}$

The measurement $\textbf{z}$ corresponds to the perspective projection of the different markers in the image. Be and the horizontal and vertical pixel coordinates of the $i^{th}$ marker. The measurement vector can be written as:

$\begin{array}{lcl} \textbf{z}[2i] &=& u_i \\ \textbf{z}[2i+1] &=& v_i \end{array}$

Be $\textbf{K}_{intr}$ the camera instrinsic parameters matrix defined by:

$\textbf{K}_{intr} = \begin{pmatrix} p_x & 0 & u_0 \\ 0 & p_y & v_0 \\ 0 & 0 & 1 \end{pmatrix}$

where are the coordinates of the principal point and (resp. ) is the ratio between the focal lens of the camera and the width (resp. height) of a pixel.

Be $\boldsymbol{\pi}$ the projection matrix that is, in the case of a perspective projection model, given by:

$\boldsymbol{\pi} = \begin{pmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{pmatrix}$

The perspective projection $\textbf{p} = (u, v, 1)^T$ of a point ${}^W\textbf{X} = ({}^WX_x, {}^WX_y, {}^WX_z, 1)^T$ is given by:

$\textbf{p} = \textbf{K}_{intr} \boldsymbol{\pi} {}^C\textbf{M}_W {}^W\textbf{X}$

where ${}^C\textbf{M}_W$ is the homogeneous matrix that expresses the pose of the world coordinate frame ${F}_W$ with regard to the camera frame ${F}_C$ .

Details on the includes

To have a Graphical User Interface (GUI), we include the following files.

#ifdef VISP_HAVE_DISPLAY
#include <visp3/gui/vpPlot.h>
#include <visp3/gui/vpDisplayFactory.h>
#endif

To be able to use the PF, we use the following includes:

#include <visp3/core/vpParticleFilter.h>

To be able to use an UKF for comparison purpose, we use the following includes:

#include <visp3/core/vpUKSigmaDrawerMerwe.h>

#include <visp3/core/vpUnscentedKalman.h>

Details on the class simulating a moving object

To make simpler the main loop of the program, we decided to implement a class that will update the 3D position of the object, expressed in the world frame, in a dedicated class.

 
class vpObjectSimulator
{
public:
  vpObjectSimulator(const double &R, const double &w, const double &phi, const double &wZ, const double &stdevRng)
    : m_R(R)
    , m_w(w)
    , m_phi(phi)
    , m_wZ(wZ)
    , m_rng(stdevRng, 0.)
  { }
 
  vpColVector move(const double &t)
  {
    vpColVector wX(4, 1.);
    double tNoisy = (m_w + m_rng())* t + m_phi;
    wX[0] = m_R * std::cos(tNoisy);
    wX[1] = m_R * std::sin(tNoisy);
    wX[2] = m_wZ;
    return wX;
  }
 
private:
  double m_R; 
  double m_w; 
  double m_phi; 
  const double m_wZ; 
  vpGaussRand m_rng; 
};

Details on the process function

As mentionned in The maths beyond the Particle Filter and The maths beyond the Unscented Kalman Filter, both the PF and the UKF rely on a process function which project forward in time their internal state.

We want to express the internal state projected in the future $\textbf{x}_{t + \Delta t}$ as a function of its previous state $\textbf{x}_{t}$ .

As stated in the Introduction, the equations of motion of the object are the following:

$\begin{array}{lcl} {}^W X_x(t) &=& R cos(\omega t + \phi) \\ {}^W X_y(t) &=& R sin(\omega t + \phi) \\ {}^W X_z(t) &=& constant \end{array}$

Thus, we have:

$\begin{array}{lclcl} {}^WX_x( t + \Delta t) &=& R cos(\omega (t + \Delta t) + \phi) &=& R cos((\omega t + \phi) + \omega \Delta t )\\ {}^WX_y( t + \Delta t) &=& R sin(\omega (t + \Delta t) + \phi) &=& R sin((\omega t + \phi) + \omega \Delta t )\\ {}^WX_z( t + \Delta t) &=& constant \end{array}$

Which can be rewritten:

$\begin{array}{lclcl} {}^WX_x( t + \Delta t) &=& R cos((\omega t + \phi) + \omega \Delta t ) &=& R cos(\omega t + \phi) cos (\omega \Delta t ) - R sin(\omega t + \phi) sin(\omega \Delta t) \\ {}^WX_y( t + \Delta t) &=& R sin((\omega t + \phi) + \omega \Delta t ) &=& R cos(\omega t + \phi) sin (\omega \Delta t ) + R sin(\omega t + \phi) cos(\omega \Delta t)\\ {}^WX_z( t + \Delta t) &=& constant \end{array}$

And can be finally written as:

$\begin{array}{lclcl} {}^WX_x( t + \Delta t) &=& R cos(\omega t + \phi) cos (\omega \Delta t ) - R sin(\omega t + \phi) sin(\omega \Delta t) &=& {}^W X_x( t) cos(\omega \Delta t) - {}^W X_y(t) sin(\omega \Delta t) \\ {}^WX_y( t + \Delta t) &=& R cos(\omega t + \phi) sin (\omega \Delta t ) + R sin(\omega t + \phi) cos(\omega \Delta t) &=& {}^W X_x( t) sin(\omega \Delta t) + {}^W X_y(t) cos(\omega \Delta t) \\ {}^WX_z( t + \Delta t) &=& constant \end{array}$

This motivates us to choose the following non-linear process function:

$\begin{array}{lclcl} \textbf{x}[0]_{t + \Delta t} &=& {}^WX_x (t + \Delta t) &=& \textbf{x}[0]_{t} cos(\textbf{x}[3]_{t}) - \textbf{x}[1]_{t} sin(\textbf{x}[3]_{t}) \\ \textbf{x}[1]_{t + \Delta t} &=& {}^WX_y (t + \Delta t) &=& \textbf{x}[0]_{t} sin(\textbf{x}[3]_{t}) + \textbf{x}[1]_{t} cos(\textbf{x}[3]_{t}) \\ \textbf{x}[2]_{t + \Delta t} &=& {}^WX_z (t + \Delta t) &=& \textbf{x}[2]_{t} \\ \textbf{x}[3]_{t + \Delta t} &=& \omega \Delta t &=& \textbf{x}[3]_{t} \end{array}$

As the process function is pretty simple, a simple function called here fx() is enough:

 
vpColVector fx(const vpColVector &x, const double & /*dt*/)
{
  vpColVector x_kPlus1(4);
  x_kPlus1[0] = x[0] * std::cos(x[3]) - x[1] * std::sin(x[3]); // wX
  x_kPlus1[1] = x[0] * std::sin(x[3]) + x[1] * std::cos(x[3]); // wY
  x_kPlus1[2] = x[2]; // wZ
  x_kPlus1[3] = x[3]; // omega * dt
  return x_kPlus1;
}

Details on the class simulating marker measurement

The measurements of the projection of the markers in the image are handled by the following class:

 
class vpMarkersMeasurements
{
public:
  vpMarkersMeasurements(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMw, const vpRotationMatrix &wRo,
                        const std::vector<vpColVector> &markers, const double &noise_stdev, const long &seed,
                        const double &likelihood_stdev)
    : m_cam(cam)
    , m_cMw(cMw)
    , m_wRo(wRo)
    , m_markers(markers)
    , m_rng(noise_stdev, 0., seed)
  {
    double sigmaDistanceSquared = likelihood_stdev * likelihood_stdev;
    m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared);
    m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared);
 
    const unsigned int nbMarkers = static_cast<unsigned int>(m_markers.size());
    for (unsigned int i = 0; i < nbMarkers; ++i) {
      vpColVector marker = markers[i];
      m_markersAsVpPoint.push_back(vpPoint(marker[0], marker[1], marker[2]));
    }
  }
 
 
  vpColVector state_to_measurement(const vpColVector &x)
  {
    unsigned int nbMarkers = static_cast<unsigned int>(m_markers.size());
    vpColVector meas(2*nbMarkers);
    vpHomogeneousMatrix wMo;
    vpTranslationVector wTo(x[0], x[1], x[2]);
    wMo.buildFrom(wTo, m_wRo);
    for (unsigned int i = 0; i < nbMarkers; ++i) {
      vpColVector cX = m_cMw * wMo * m_markers[i];
      double u = 0., v = 0.;
      vpMeterPixelConversion::convertPoint(m_cam, cX[0] / cX[2], cX[1] / cX[2], u, v);
      meas[2*i] = u;
      meas[2*i + 1] = v;
    }
    return meas;
  }
 
 
  vpColVector measureGT(const vpColVector &wX)
  {
    unsigned int nbMarkers = static_cast<unsigned int>(m_markers.size());
    vpColVector meas(2*nbMarkers);
    vpHomogeneousMatrix wMo;
    vpTranslationVector wTo(wX[0], wX[1], wX[2]);
    wMo.buildFrom(wTo, m_wRo);
    for (unsigned int i = 0; i < nbMarkers; ++i) {
      vpColVector cX = m_cMw * wMo * m_markers[i];
      double u = 0., v = 0.;
      vpMeterPixelConversion::convertPoint(m_cam, cX[0] / cX[2], cX[1] / cX[2], u, v);
      meas[2*i] = u;
      meas[2*i + 1] = v;
    }
    return meas;
  }
 
 
  vpColVector measureWithNoise(const vpColVector &wX)
  {
    vpColVector measurementsGT = measureGT(wX);
    vpColVector measurementsNoisy = measurementsGT;
    unsigned int sizeMeasurement = measurementsGT.size();
    for (unsigned int i = 0; i < sizeMeasurement; ++i) {
      measurementsNoisy[i] += m_rng();
    }
    return measurementsNoisy;
  }
 
 
  double likelihood(const vpColVector &x, const vpColVector &meas)
  {
    unsigned int nbMarkers = static_cast<unsigned int>(m_markers.size());
    double likelihood = 0.;
    vpHomogeneousMatrix wMo;
    vpTranslationVector wTo(x[0], x[1], x[2]);
    wMo.buildFrom(wTo, m_wRo);
    const unsigned int sizePt2D = 2;
    const unsigned int idX = 0, idY = 1, idZ = 2;
    double sumError = 0.;
    // Compute the error between the projection of the markers that correspond
    // to the particle position and the actual measurements of the markers
    // projection
    for (unsigned int i = 0; i < nbMarkers; ++i) {
      vpColVector cX = m_cMw * wMo * m_markers[i];
      vpImagePoint projParticle;
      vpMeterPixelConversion::convertPoint(m_cam, cX[idX] / cX[idZ], cX[idY] / cX[idZ], projParticle);
      vpImagePoint measPt(meas[sizePt2D * i + 1], meas[sizePt2D * i]);
      double error = vpImagePoint::sqrDistance(projParticle, measPt);
      sumError += error;
    }
    // Compute the likelihood from the mean error
    likelihood = std::exp(m_constantExpDenominator * sumError / static_cast<double>(nbMarkers)) * 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:
  vpCameraParameters m_cam; // The camera parameters
  vpHomogeneousMatrix m_cMw; // The pose of the world frame with regard to the camera frame.
  vpRotationMatrix m_wRo; // The rotation matrix that expresses the rotation between the world frame and object frame.
  std::vector<vpColVector> m_markers; // The position of the markers in the object frame.
  std::vector<vpPoint> m_markersAsVpPoint; // The position of the markers in the object frame, expressed as vpPoint.
  vpGaussRand m_rng; // Noise simulator for the measurements
  double m_constantDenominator; // Denominator of the Gaussian function used for the likelihood computation.
  double m_constantExpDenominator; // Denominator of the exponential of the Gaussian function used for the likelihood computation.
};

It takes as input the camera parameters cam, the homogeneous matrix expressing the pose of the world frame ${F}_W$ with regard to the camera frame ${F}_C$ cMw, the rotation matrix that 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 ${}^W \textbf{X}$ into 3D positions of the markers in the camera frame ${}^C \textbf{X}^i$ , where $ i $ denotes the i $^{th}$ 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 $\sigma_l$ is given for the computation of the likelihood of a PF particle knowing the measurements.

The method state_to_measurement is used to convert the internal state of the UKF into the measurement space (i.e. the projection in the image of the markers sticked on the object if the object is at this 3D position):

 
  vpColVector state_to_measurement(const vpColVector &x)
  {
    unsigned int nbMarkers = static_cast<unsigned int>(m_markers.size());
    vpColVector meas(2*nbMarkers);
    vpHomogeneousMatrix wMo;
    vpTranslationVector wTo(x[0], x[1], x[2]);
    wMo.buildFrom(wTo, m_wRo);
    for (unsigned int i = 0; i < nbMarkers; ++i) {
      vpColVector cX = m_cMw * wMo * m_markers[i];
      double u = 0., v = 0.;
      vpMeterPixelConversion::convertPoint(m_cam, cX[0] / cX[2], cX[1] / cX[2], u, v);
      meas[2*i] = u;
      meas[2*i + 1] = v;
    }
    return meas;
  }

The method measureGT is used to convert the ground truth 3D position of the object into ground truth projections of the markers in the image:

 
  vpColVector measureGT(const vpColVector &wX)
  {
    unsigned int nbMarkers = static_cast<unsigned int>(m_markers.size());
    vpColVector meas(2*nbMarkers);
    vpHomogeneousMatrix wMo;
    vpTranslationVector wTo(wX[0], wX[1], wX[2]);
    wMo.buildFrom(wTo, m_wRo);
    for (unsigned int i = 0; i < nbMarkers; ++i) {
      vpColVector cX = m_cMw * wMo * m_markers[i];
      double u = 0., v = 0.;
      vpMeterPixelConversion::convertPoint(m_cam, cX[0] / cX[2], cX[1] / cX[2], u, v);
      meas[2*i] = u;
      meas[2*i + 1] = v;
    }
    return meas;
  }

The method measureWithNoise adds noise to the ground truth measurements in order to simulate a noisy measurement process:

 
  vpColVector measureWithNoise(const vpColVector &wX)
  {
    vpColVector measurementsGT = measureGT(wX);
    vpColVector measurementsNoisy = measurementsGT;
    unsigned int sizeMeasurement = measurementsGT.size();
    for (unsigned int i = 0; i < sizeMeasurement; ++i) {
      measurementsNoisy[i] += m_rng();
    }
    return measurementsNoisy;
  }

The method likelihood computes the likelihood of a particle knowing the measurements. We decided to implement a Gaussian function that penalizes the mean distance between the projection of the markers corresponding to the particle position and the measurements of the markers in the image.

$w_i = l(\textbf{x}_i, \textbf{z}) := \frac{1}{\sqrt{2. * \Pi * \sigma_l^2}} exp^{- \frac{\overline{e}}{2 * \sigma_l^2}}$

where $\overline{e} = \frac{\sum_i e_i}{N}$ is the mean reprojection error of the markers.

Here is the corresponding code:

 
  double likelihood(const vpColVector &x, const vpColVector &meas)
  {
    unsigned int nbMarkers = static_cast<unsigned int>(m_markers.size());
    double likelihood = 0.;
    vpHomogeneousMatrix wMo;
    vpTranslationVector wTo(x[0], x[1], x[2]);
    wMo.buildFrom(wTo, m_wRo);
    const unsigned int sizePt2D = 2;
    const unsigned int idX = 0, idY = 1, idZ = 2;
    double sumError = 0.;
    // Compute the error between the projection of the markers that correspond
    // to the particle position and the actual measurements of the markers
    // projection
    for (unsigned int i = 0; i < nbMarkers; ++i) {
      vpColVector cX = m_cMw * wMo * m_markers[i];
      vpImagePoint projParticle;
      vpMeterPixelConversion::convertPoint(m_cam, cX[idX] / cX[idZ], cX[idY] / cX[idZ], projParticle);
      vpImagePoint measPt(meas[sizePt2D * i + 1], meas[sizePt2D * i]);
      double error = vpImagePoint::sqrDistance(projParticle, measPt);
      sumError += error;
    }
    // Compute the likelihood from the mean error
    likelihood = std::exp(m_constantExpDenominator * sumError / static_cast<double>(nbMarkers)) * 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;
  }

Details on the computation of the pose from noisy measurements

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 PF and by the UKF with regard to the 3D position we would have if we computed the pose directly from the noisy measurements.

 
vpHomogeneousMatrix computePose(std::vector<vpPoint> &point, const std::vector<vpImagePoint> &ip, const vpCameraParameters &cam)
{
  vpPose pose;
  double x = 0, y = 0;
  for (unsigned int i = 0; i < point.size(); i++) {
    vpPixelMeterConversion::convertPoint(cam, ip[i], x, y);
    point[i].set_x(x);
    point[i].set_y(y);
    pose.addPoint(point[i]);
  }
 
  vpHomogeneousMatrix cMo;
  pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo);
  return cMo;
}

Details on the constants of the main loop

In the main loop of the program, we first declare some constants that will be used later on:

  const unsigned int nbIter = 200; // Number of time steps for the simulation
  const double dt = 0.001; // Period of 0.001s
  const double sigmaMeasurements = 2.; // Standard deviation of the measurements: 2 pixels
  const double radius = 0.25; // Radius of revolution of 0.25m
  const double w = 2 * M_PI * 10; // Pulsation of the motion of revolution
  const double phi = 2; // Phase of the motion of revolution
 
  // Vector of the markers sticked on the object
  const std::vector<vpColVector> markers = { vpColVector({-0.05, 0.05, 0., 1.}),
                                             vpColVector({0.05, 0.05, 0., 1.}),
                                             vpColVector({0.05, -0.05, 0., 1.}),
                                             vpColVector({-0.05, -0.05, 0., 1.}) };
  const unsigned int nbMarkers = static_cast<unsigned int>(markers.size());
  std::vector<vpPoint> markersAsVpPoint;
  for (unsigned int i = 0; i < nbMarkers; ++i) {
    vpColVector marker = markers[i];
    markersAsVpPoint.push_back(vpPoint(marker[0], marker[1], marker[2]));
  }
 
  const long seed = 42; // Seed for the random generator
  vpHomogeneousMatrix cMw; // Pose of the world frame with regard to the camera frame
  cMw[0][0] = 1.; cMw[0][1] = 0.; cMw[0][2] = 0.; cMw[0][3] = 0.2;
  cMw[1][0] = 0.; cMw[1][1] = -1.; cMw[1][2] = 0.; cMw[1][3] = 0.3;
  cMw[2][0] = 0.; cMw[2][1] = 0.; cMw[2][2] = -1.; cMw[2][3] = 1.;
 
  vpHomogeneousMatrix wMo; // Pose of the object frame with regard to the world frame
  wMo[0][0] = 1.; wMo[0][1] = 0.; wMo[0][2] = 0.; wMo[0][3] = radius;
  wMo[1][0] = 0.; wMo[1][1] = 1.; wMo[1][2] = 0.; wMo[1][3] = 0;
  wMo[2][0] = 0.; wMo[2][1] = 0.; wMo[2][2] = 1.; wMo[2][3] = 0.2;
  vpRotationMatrix wRo; // Rotation between the object frame and world frame
  wMo.extract(wRo);
  const double wZ = wMo[2][3];

Here is their meanings:

dt is the sampling period (the time spent between two acquisitions),
sigmaMeasurements is the standard deviation of the Gaussian noise added to the measurements,
radius is the radius of the revolution of the object around the world frame origin,
w is the pulsation of the motion of revolution,
phi is the phase of the motion of revolution,
markers is a vector containing the homogeneous coordinates expressed in the object frame of the markers,
markersAsVpPoint is a vector containing the 3D coordinates of the markers expressed in the object (to compute the noisy pose as explained previously),
seed is the seed for the Gaussian noise generator that adds noise to the projections of the markers in the image,
cMw is the homogeneous matrix expressing the pose of the world frame with regard to the camera frame,
wMo is the homogeneous matrix expressing the pose of the object frame with regard to the world frame,
wRo is the rotation matrix contained in wMo
wZ is the z-axis coordinate of the origin of the object frame expressed in the world frame.

To convert the 3D position of the object into the projection of its markers in the image, we need camera parameters. We generate camera parameters for a simulated camera as follow:

  // Create a camera parameter container
  // Camera initialization with a perspective projection without distortion model
  double px = 600; double py = 600; double u0 = 320; double v0 = 240;
  vpCameraParameters cam;
  cam.initPersProjWithoutDistortion(px, py, u0, v0);

Details on the initialization of the PF

To create the particle filter, we need:

the number of particles we want to use,
the standard deviations of each of the components of the state $\sigma_j , j \in \{0 \dots dim(\textit{S}) - 1\}$ ,
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.

These parameters can be set using the Command Line Interface (CLI) thanks to the following structure:

struct SoftwareArguments
{
  // --- Main loop parameters---
  static const int SOFTWARE_CONTINUE = 42;
  bool m_useDisplay; 
  unsigned int m_nbStepsWarmUp; 
  unsigned int m_nbSteps; 
  // --- PF parameters---
  unsigned int m_N; 
  double m_maxDistanceForLikelihood; 
  double m_ampliMaxX; 
  double m_ampliMaxY; 
  double m_ampliMaxZ; 
  double m_ampliMaxOmega; 
  long m_seedPF; 
  int m_nbThreads; 
 
  SoftwareArguments()
    : m_useDisplay(true)
    , m_nbStepsWarmUp(200)
    , m_nbSteps(300)
    , m_N(500)
    , m_maxDistanceForLikelihood(10.)
    , m_ampliMaxX(0.02)
    , m_ampliMaxY(0.02)
    , m_ampliMaxZ(0.01)
    , m_ampliMaxOmega(0.02)
    , 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-main") && ((i+1) < argc)) {
        m_nbSteps = std::atoi(argv[i + 1]);
        ++i;
      }
      else 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-Z") && ((i+1) < argc)) {
        m_ampliMaxZ = std::atof(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--ampli-max-omega") && ((i+1) < argc)) {
        m_ampliMaxOmega = 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-main <uint>] [--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-Z <double>] [--ampli-max-omega <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-main" << std::endl;
    std::cout << "    Number of steps in the main loop." << std::endl;
    std::cout << "    Default: " << m_nbSteps << std::endl;
    std::cout << 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 mean distance of the projection of the markers corresponding" << std::endl;
    std::cout << "    to a particle with the measurements. 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-Z" << std::endl;
    std::cout << "    Maximum amplitude of the noise added to a particle along the Z-axis." << std::endl;
    std::cout << "    Default: " << m_ampliMaxZ << std::endl;
    std::cout << std::endl;
    std::cout << "  --ampli-max-omega" << std::endl;
    std::cout << "    Maximum amplitude of the noise added to a particle affecting the pulsation of the motion." << std::endl;
    std::cout << "    Default: " << m_ampliMaxOmega << 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;
  }
};

They are thereafter used in the following section of code of the main function:

  const double maxDistanceForLikelihood = args.m_maxDistanceForLikelihood; // The maximum allowed distance between a particle and the measurement, leading to a likelihood equal to 0..
  const double sigmaLikelihood = maxDistanceForLikelihood / 3.; // The standard deviation of likelihood function.
  const unsigned int nbParticles = args.m_N; // Number of particles to use
  const double ampliMaxX = args.m_ampliMaxX, ampliMaxY = args.m_ampliMaxY, ampliMaxZ = args.m_ampliMaxZ;
  const double ampliMaxOmega = args.m_ampliMaxOmega;
  const std::vector<double> stdevsPF = { ampliMaxX/3., ampliMaxY/3., ampliMaxZ/3., ampliMaxOmega/3. }; // Standard deviation for each state component
  long seedPF = args.m_seedPF; // Seed for the random generators of the PF
  const int nbThread = args.m_nbThreads;
  if (seedPF < 0) {
    seedPF = static_cast<long>(vpTime::measureTimeMicros());
  }

Then, to initialize the filters, we need:

a guess of the initial state $\textbf{x}(t = 0)$ ,
a process function ,
a likelihood function ,
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 if the addition operation cannot be readily performed in the state space $\textit{S}$ ,
optionnally, a function to perform the addition operation in the state space $\textit{S}$ .

The section of code corresponding to the declaration of these functions is the following:

  vpParticleFilter<vpColVector>::vpProcessFunction processFunc = fx;
  using std::placeholders::_1;
  using std::placeholders::_2;
  vpParticleFilter<vpColVector>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpMarkersMeasurements::likelihood, &markerMeas, _1, _2);
  vpParticleFilter<vpColVector>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter<vpColVector>::simpleResamplingCheck;
  vpParticleFilter<vpColVector>::vpResamplingFunction resamplingFunc = vpParticleFilter<vpColVector>::simpleImportanceResampling;

When both the constants and the functions have been declared, it is possible to create the PF using the following code:

  // Initialize the PF
  vpParticleFilter<vpColVector> pfFilter(nbParticles, stdevsPF, seedPF, nbThread);
  pfFilter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc);

Initialization of the UKF, used for comparison purpose

We refer the user to Details on the initialization of the UKF 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:

  // Initialize the attributes of the UKF
  // Sigma point drawer
  std::shared_ptr<vpUKSigmaDrawerAbstract> drawer = std::make_shared<vpUKSigmaDrawerMerwe>(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);

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. 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:

#ifdef VISP_HAVE_DISPLAY
  // Initialize the plot
  vpPlot plot(1);
  plot.initGraph(0, 4);
  plot.setTitle(0, "Position of the robot wX");
  plot.setUnitX(0, "Position along x(m)");
  plot.setUnitY(0, "Position along y (m)");
  plot.setLegend(0, 0, "GT");
  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::green);
  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, "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::green);
  plotError.setColor(0, 1, vpColor::blue);
  plotError.setColor(0, 2, vpColor::black);
#endif

Then, we initialize the simple renderer that displays what the camera sees:

  // Initialize the display
  // Depending on the detected third party libraries, we instantiate here the
  // first video device which is available
#ifdef VISP_HAVE_DISPLAY
  vpImage<vpRGBa> Idisp(700, 700, vpRGBa(255));
  std::shared_ptr<vpDisplay> d = vpDisplayFactory::createDisplay(Idisp, 800, -1, "Projection of the markers");
#endif

Details on the initialization of the loop

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 states of both the PF and the UKF are expressed, as a null homogeneous coordinates vector.

  // Initialize the simulation
  vpObjectSimulator object(radius, w, phi, wZ, ampliMaxOmega / 3.);
  vpColVector object_pos(4, 0.);
  object_pos[3] = 1.;

Details on the loop

The main loop of the program is the following:

  for (unsigned int i = 0; i < nbIter; ++i) {
    double t = dt * static_cast<double>(i);
    std::cout << "[Timestep" << i << ", t = " << t << "]" << std::endl;
    // Update object pose
    object_pos = object.move(t);
 
    // Perform the measurement
    vpColVector z = markerMeas.measureWithNoise(object_pos);
 
    double tPF = vpTime::measureTimeMs();
    pfFilter.filter(z, dt);
    double dtPF = vpTime::measureTimeMs() - tPF;
 
    // Use the UKF to filter the measurement for comparison
    double tUKF = vpTime::measureTimeMs();
    ukf.filter(z, dt);
    double dtUKF = vpTime::measureTimeMs() - tUKF;
 
    // Get the filtered states
    vpColVector XestPF = pfFilter.computeFilteredState();
    vpColVector XestUKF = ukf.getXest();
 
    // Compute satistics
    vpColVector cX_GT = cMw * object_pos;
    vpColVector wX_UKF(4, 1.);
    vpColVector wX_PF(4, 1.);
    for (unsigned int i = 0; i < 3; ++i) {
      wX_PF[i] = XestPF[i];
      wX_UKF[i] = XestUKF[i];
    }
    vpColVector cX_PF = cMw * wX_PF;
    vpColVector cX_UKF = cMw * wX_UKF;
    vpColVector error_PF = cX_PF - cX_GT;
    vpColVector error_UKF = cX_UKF - cX_GT;
 
    // Log statistics
    std::cout << "  [Particle Filter method] " << std::endl;
    std::cout << "    Norm of the error = " << error_PF.frobeniusNorm() << " m^2" << std::endl;
    std::cout << "    Fitting duration = " << dtPF << " ms" << std::endl;
 
    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;
 
#ifdef VISP_HAVE_DISPLAY
    // Prepare the pose computation:
    // the image points corresponding to the noisy markers are needed
    std::vector<vpImagePoint> ip;
    for (unsigned int id = 0; id < nbMarkers; ++id) {
      vpImagePoint markerProjNoisy(z[2*id + 1], z[2*id]);
      ip.push_back(markerProjNoisy);
    }
 
    // Compute the pose using the noisy markers
    vpHomogeneousMatrix cMo_noisy = computePose(markersAsVpPoint, ip, cam);
    vpHomogeneousMatrix wMo_noisy = cMw.inverse() * cMo_noisy;
    double wXnoisy = wMo_noisy[0][3];
    double wYnoisy = wMo_noisy[1][3];
 
    // Plot the ground truth
    plot.plot(0, 0, object_pos[0], object_pos[1]);
 
    // Plot the PF filtered state
    plot.plot(0, 1, XestPF[0], XestPF[1]);
 
 
    // Plot the UKF filtered state
    plot.plot(0, 2, XestUKF[0], XestUKF[1]);
 
    // Plot the noisy pose
    plot.plot(0, 3, wXnoisy, wYnoisy);
 
    // Plot the PF filtered state error
    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());
 
    // Display the projection of the markers
    vpDisplay::display(Idisp);
    vpColVector zGT = markerMeas.measureGT(object_pos);
    vpColVector zFiltUkf = markerMeas.state_to_measurement(XestUKF);
    vpColVector zFiltPF = markerMeas.state_to_measurement(XestPF);
    for (unsigned int id = 0; id < nbMarkers; ++id) {
      vpImagePoint markerProjGT(zGT[2*id + 1], zGT[2*id]);
      vpDisplay::displayCross(Idisp, markerProjGT, 5, vpColor::red);
 
      vpImagePoint markerProjFiltPF(zFiltPF[2*id + 1], zFiltPF[2*id]);
      vpDisplay::displayCross(Idisp, markerProjFiltPF, 5, vpColor::green);
 
      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);
    }
 
    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 PF"), vpColor::green);
    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);
#endif
  }

First, we update the ground-truth 3D position of the object based on the simulated time using the following line:

// Update object pose

object_pos = object.move(t);

Then, we update the measurement by projecting the 3D position of the markers attached to the object in the image and add some noise to the projections using the following line:

// Perform the measurement

vpColVector z = markerMeas.measureWithNoise(object_pos);

Then, we use the Particle Filter to filter the noisy measurements:

    double tPF = vpTime::measureTimeMs();
    pfFilter.filter(z, dt);
    double dtPF = vpTime::measureTimeMs() - tPF;

Then, we use the Unscented Kalman Filter to filter the noisy measurements to compare the results:

    // Use the UKF to filter the measurement for comparison
    double tUKF = vpTime::measureTimeMs();
    ukf.filter(z, dt);
    double dtUKF = vpTime::measureTimeMs() - tUKF;

Finally, we update the plot and renderer:

#ifdef VISP_HAVE_DISPLAY
    // Prepare the pose computation:
    // the image points corresponding to the noisy markers are needed
    std::vector<vpImagePoint> ip;
    for (unsigned int id = 0; id < nbMarkers; ++id) {
      vpImagePoint markerProjNoisy(z[2*id + 1], z[2*id]);
      ip.push_back(markerProjNoisy);
    }
 
    // Compute the pose using the noisy markers
    vpHomogeneousMatrix cMo_noisy = computePose(markersAsVpPoint, ip, cam);
    vpHomogeneousMatrix wMo_noisy = cMw.inverse() * cMo_noisy;
    double wXnoisy = wMo_noisy[0][3];
    double wYnoisy = wMo_noisy[1][3];
 
    // Plot the ground truth
    plot.plot(0, 0, object_pos[0], object_pos[1]);
 
    // Plot the PF filtered state
    plot.plot(0, 1, XestPF[0], XestPF[1]);
 
 
    // Plot the UKF filtered state
    plot.plot(0, 2, XestUKF[0], XestUKF[1]);
 
    // Plot the noisy pose
    plot.plot(0, 3, wXnoisy, wYnoisy);
 
    // Plot the PF filtered state error
    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());
 
    // Display the projection of the markers
    vpDisplay::display(Idisp);
    vpColVector zGT = markerMeas.measureGT(object_pos);
    vpColVector zFiltUkf = markerMeas.state_to_measurement(XestUKF);
    vpColVector zFiltPF = markerMeas.state_to_measurement(XestPF);
    for (unsigned int id = 0; id < nbMarkers; ++id) {
      vpImagePoint markerProjGT(zGT[2*id + 1], zGT[2*id]);
      vpDisplay::displayCross(Idisp, markerProjGT, 5, vpColor::red);
 
      vpImagePoint markerProjFiltPF(zFiltPF[2*id + 1], zFiltPF[2*id]);
      vpDisplay::displayCross(Idisp, markerProjFiltPF, 5, vpColor::green);
 
      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);
    }
 
    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 PF"), vpColor::green);
    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);
#endif

First, we compute the noisy pose using the noisy measurements of the markers, to be able to plot the noisy 3D position of the object:

    // Prepare the pose computation:
    // the image points corresponding to the noisy markers are needed
    std::vector<vpImagePoint> ip;
    for (unsigned int id = 0; id < nbMarkers; ++id) {
      vpImagePoint markerProjNoisy(z[2*id + 1], z[2*id]);
      ip.push_back(markerProjNoisy);
    }
 
    // Compute the pose using the noisy markers
    vpHomogeneousMatrix cMo_noisy = computePose(markersAsVpPoint, ip, cam);
    vpHomogeneousMatrix wMo_noisy = cMw.inverse() * cMo_noisy;
    double wXnoisy = wMo_noisy[0][3];
    double wYnoisy = wMo_noisy[1][3];

Then, we update the plot by plotting the new ground truth, filtered and noisy 3D positions:

    // Plot the ground truth
    plot.plot(0, 0, object_pos[0], object_pos[1]);
 
    // Plot the PF filtered state
    plot.plot(0, 1, XestPF[0], XestPF[1]);
 
 
    // Plot the UKF filtered state
    plot.plot(0, 2, XestUKF[0], XestUKF[1]);
 
    // Plot the noisy pose
    plot.plot(0, 3, wXnoisy, wYnoisy);
 
    // Plot the PF filtered state error
    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());

Finally, we update the renderer that displays the projection in the image of the markers:

    // Display the projection of the markers
    vpDisplay::display(Idisp);
    vpColVector zGT = markerMeas.measureGT(object_pos);
    vpColVector zFiltUkf = markerMeas.state_to_measurement(XestUKF);
    vpColVector zFiltPF = markerMeas.state_to_measurement(XestPF);
    for (unsigned int id = 0; id < nbMarkers; ++id) {
      vpImagePoint markerProjGT(zGT[2*id + 1], zGT[2*id]);
      vpDisplay::displayCross(Idisp, markerProjGT, 5, vpColor::red);
 
      vpImagePoint markerProjFiltPF(zFiltPF[2*id + 1], zFiltPF[2*id]);
      vpDisplay::displayCross(Idisp, markerProjFiltPF, 5, vpColor::green);
 
      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);
    }
 
    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 PF"), vpColor::green);
    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);

The program stops once the Return key is pressed.

Next tutorial

You are now ready to see the next Tutorial: Using Particle Filter to model a wire using polynomial interpolation.

Table of Contents