Merge pull request #1458 from rolalaro/tuto_pf_like_ukf

Tutorial and examples for Particle Filter

3rdparty/simdlib/CMakeLists.txt

@@ -173,8 +173,14 @@ elseif(ARM OR AARCH64)
     vp_check_compiler_flag(CXX "-Wno-unused-command-line-argument" HAVE_NO_UNUSED_CMD_LINE_FLAG "${VISP_SOURCE_DIR}/cmake/checks/cpu_warning.cpp")
     vp_check_compiler_flag(CXX "-Wno-asm-operand-widths"           HAVE_NO_ASM_WIDTHS_FLAG      "${VISP_SOURCE_DIR}/cmake/checks/cpu_warning.cpp")
     vp_check_compiler_flag(CXX "-Wno-switch"                       HAVE_NO_SWITCH_FLAG          "${VISP_SOURCE_DIR}/cmake/checks/cpu_warning.cpp")
+
     if(CMAKE_SYSTEM_PROCESSOR MATCHES "arm")
-        set(CXX_NEON_FLAG "-mfpu=neon")
+        if( NOT ((CMAKE_CXX_COMPILER_ID STREQUAL "Clang") OR (CMAKE_CXX_COMPILER MATCHES "clang")))
+            set(CXX_NEON_FLAG "-mfpu=neon -mfpu=neon-fp16")
+        endif()
+        if(CMAKE_CXX_COMPILER_ID MATCHES "GNU")
+            set(CXX_NEON_FLAG "${CXX_NEON_FLAG} -mfp16-format=ieee")
+        endif()
     else()
         set(CXX_NEON_FLAG "")
     endif()

ChangeLog.txt

@@ -62,6 +62,12 @@ ViSP 3.x.x (Version in development)
       https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-hsv-segmentation-pcl.html
     . New tutorial: Rendering a 3D scene with Panda3D
       https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-panda3d.html
+    . New tutorial: Using Unscented Kalman Filter to filter your data
+      https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-ukf.html
+    . New tutorial: Using Particle Filter to filter your data
+      https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-pf.html
+    . New tutorial: Using Particle Filter to model a wire using polynomial interpolation
+      https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-pf-curve-fitting.html
   - Bug fixed
     . [#1251] Bug in vpDisplay::displayFrame()
     . [#1270] Build issue around std::clamp and optional header which are not found with cxx17

doc/tutorial/misc/tutorial-pf-curve-fitting.dox

@@ -0,0 +1,248 @@
+/**
+  \page tutorial-pf-curve-fitting Tutorial: Using Particle Filter to model a wire using polynomial interpolation
+  \tableofcontents
+
+\section tuto-pf-cf-intro Introduction
+
+We suppose that you are already familiar with the \ref tutorial-pf.
+
+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 model a wire using polynomial interpolation . The PF is used to
+filter the noisy pixels in a segmented image in order to compute a model of the wire using polynomial interpolation.
+The color wire is observed by a static camera, in the following configuration:
+
+\htmlonly <style>div.image img[src="img-tutorial-pf-cf-illustration.jpg"]{width:50%;}</style>
+\endhtmlonly
+\image html img-tutorial-pf-cf-illustration.jpg
+
+\subsection tuto-pf-cf-intro-methods The maths beyond the Particle Filter
+
+The maths beyond the Particle Filter are explained in the documentation of the vpParticleFilter class.
+They are also explained in the \ref tuto-pf-cf-intro-methods .
+
+\section tuto-pf-cf-tutorial Explanations about the tutorial
+
+The tutorial is split in three different programs:
+- \ref tutorial-pf-curve-fitting-lms.cpp : this program illustrates how to perform a polynomial interpolation using the
+Least-Mean Square method, and the impact of noise on the resulting interpolated model.
+- \ref tutorial-pf-curve-fitting-pf.cpp : this program illustrates how to perform a polynomial interpolation using a
+Particle Filter.
+- \ref tutorial-pf-curve-fitting-all.cpp : this program uses the methods mentionned above to compare their performances.
+
+\subsection tuto-pf-cf-tutorial-howtorun How to run the tutorial
+
+To run one of the tutorials with the default dataset, please run the following commands:
+
+```
+$ cd $VISP_WS/visp-build/tutorial/particle-filter-curve-fitting
+$ ./tutorial-pf-curve-fitting-{lms, pf, all} --video data/color_image_0%03d.png
+```
+
+To see the arguments the program can take, please run:
+
+```
+$ cd $VISP_WS/visp-build/tutorial/particle-filter-curve-fitting
+$ ./tutorial-pf-curve-fitting-{lms, pf, all} -h
+```
+
+You should see something similar to the following image:
+
+\htmlonly <style>div.image img[src="img-tutorial-pf-cf-helper"]{width:50%;}</style>
+\endhtmlonly
+\image html img-tutorial-pf-cf-helper.jpg "Screenshot of the tutorial Graphical User Interface"
+
+For the tutorial-pf-curve-fitting-pf.cpp and tutorial-pf-curve-fitting-all.cpp tutorials, the PF must
+be initialized. You are asked if you would rather use a manual initialization (left click) or an automatic one (right click).
+
+\htmlonly <style>div.image img[src="img-tutorial-pf-cf-init"]{width:50%;}</style>
+\endhtmlonly
+\image html img-tutorial-pf-cf-init.jpg "Screenshot of the tutorial initialization step"
+
+Then, for all the tutorials, you can either display the images one-by-one (left click) or automatically switch from an
+image to the next one (middle click). You can leave the program whenever you want using a right click.
+
+\htmlonly <style>div.image img[src="img-tutorial-pf-cf-run"]{width:50%;}</style>
+\endhtmlonly
+\image html img-tutorial-pf-cf-run.jpg "Screenshot of the tutorial during its run step"
+
+\subsection tuto-pf-cf-tutorial-general General explanations about the tutorials
+
+\subsubsection tuto-pf-cf-tutorial-general-segmentation Notes about the segmentation and skeletonization of the image
+
+The segmentation of the image using HSV encoding has been presented in the \ref tutorial-hsv-range-tuner .
+
+A calibration file containing the segmentation parameters for the default sequence can be found in the calib folder.
+If you want to use your own sequence, please run the \ref tutorial-hsv-range-tuner.cpp tutorial to extract the
+parameters that correspond to your own sequence and edit the file calib/hsv-thresholds.yml .
+
+The skeletonization of the segmented image is not the topic of this tutorial. Thus, we invite the interested readers
+to read by themselves the corresponding method in the \ref vpTutoSegmentation.cpp file.
+
+\subsubsection tuto-pf-cf-tutorial-general-PF Explanations about the Particle Filter
+
+The internal state of the PF contains the coefficients of the interpolation polynomial. Be
+\f$ v(u) = \sum_{i=0}^N a_i u^i \f$ the interpolation polynomial whose highest degree is N.
+The internal state of the PF is of size N + 1 such as:
+
+\f[
+  \begin{array}{lcl}
+  \textbf{x}[i] &=& a_i
+  \end{array}
+\f]
+
+The measurement \f$ \textbf{z} \f$ corresponds to the vector of vpImagePoint that forms the skeletonized version of the
+segmented image.
+Be \f$ u_i \f$ and \f$ v_i \f$ the horizontal and vertical pixel coordinates of the \f$ i^{th} \f$ marker.
+The measurement vector can be written as:
+
+\f[
+  \begin{array}{lcl}
+  \textbf{z}[i] &=& vpImagePoint(v_i , u_i)
+  \end{array}
+\f]
+
+
+\subsubsection tuto-pf-cf-tutorial-general-LMS Explanations about the Least-Mean Square method
+
+The Least-Mean Square method is a standard method to solve a polynomial interpolation problem.
+The polynomial interpolation problem can be written in matrices form as follow:
+
+\f[
+\begin{array}{llcl}
+  & \textbf{A}\textbf{x} &=& \textbf{b} \\
+  \iff & \textbf{X} &=& \textbf{A}^+\textbf{b} \\
+\end{array}
+\f]
+
+where \f$ \textbf{A}^+ \f$ denotes the Moore-Penrose pseudo-inverse of the matrix A, with:
+
+\f[
+\begin{array}{lcl}
+  \textbf{A}[i][j] &=& u_i^j
+\end{array}
+\\
+\begin{array}{lcl}
+  \textbf{x}[i] &=& a_i
+\end{array}
+\\
+\begin{array}{lcl}
+  \textbf{b}[i] &=& v_i
+\end{array}
+\f]
+
+<b>Important note:</b> due to numerical unstability, the pixel coordinates cannot be used directly
+in the Least-Mean square method. Indeed, using pixel coordinates leads to having an ill-conditionned
+\f$ \textbf{A} \f$ matrix. Consequently, in all the tutorials, the pixel coordinates are normalized
+by dividing them by the height and width of the image before running any interpolation.
+
+\subsection tuto-pf-cf-tutorial-explained Detailed explanations about the tutorials
+
+We will now present the different tutorials and explained their important parts.
+
+\subsubsection tuto-pf-cf-tutorial-explained-lms Details on the tutorial-pf-curve-fitting-lms.cpp
+
+This tutorial goal is to show how a Least-Mean Square method can be used to perform polynomial interpolation
+and the impact of noise on such technique.
+
+To visualize the accuracy of the method, a vpPlot is instantiated in the following part of the code:
+
+\snippet tutorial-pf-curve-fitting-lms.cpp Init_plot
+
+At each iteration, the new frame is read, then segmented and skeletonized in the following section of code:
+
+\snippet tutorial-pf-curve-fitting-lms.cpp Measurements_extraction
+
+The method addSaltAndPepperNoise permits to add a user-defined percentage of salt-and-pepper noise, to evaluate the
+robustness of the method against noise.
+
+The polynomial interpolation is performed in the following section of the code:
+
+\snippet tutorial-pf-curve-fitting-lms.cpp LMS_interpolation
+
+It relies on the following method of the vpTutoMeanSquareFitting class:
+
+\snippet vpTutoMeanSquareFitting.cpp Solve_LMS_system
+
+The matrices that form the system that needs to be solved are filled in the vpTutoParabolaModel class (we remind the
+reader that normalization of the pixel coordinates is performed to avoid numerical unstability):
+
+\snippet vpTutoParabolaModel.h Fill_LMS_system
+
+\subsubsection tuto-pf-cf-tutorial-explained-pf Details on the tutorial-pf-curve-fitting-pf.cpp
+
+This tutorial is meant to show how a Particle Filter could be used to model a wire thanks to
+polynomial interpolation.
+
+To initialize a Particle Filter, a guess of the initial state must be given to it. The more accurate
+is the initial guess, the quicker the PF will converge. The initialization can be done either manually or
+automatically thanks to the following piece of code:
+
+\snippet tutorial-pf-curve-fitting-pf.cpp Initialization_function
+
+A Particle Filter needs a function to evaluate the likelihood of a particle. We decided to use a Gaussian-based function
+that penalizes particles whose polynomial model has a Root Mean Square Error with regard to the measurement points greater than a given threshold.
+To be robust against outliers, we use the Tukey M-estimator. The likelihood function is implemented in a functor
+to be able to pass additional information such as the height and width of the input image:
+
+\snippet tutorial-pf-curve-fitting-pf.cpp Likelihood_functor
+
+This likelihood functor compute the residuals using the following evaluation functions:
+
+\snippet tutorial-pf-curve-fitting-pf.cpp Evaluation_functions
+
+A Particle Filter needs to perform a weighted average to compute the filtered state. Performing a weighted average of
+the particles polynomial coefficients would not lead to satisfying results, as it is not mathematically correct.
+Instead, we decided to generate a given number of "control points" by each particle. The number of control points generated
+by a particle is dictated by its associated weight. Then, we compute the polynomial coefficients that best fit all these
+control points using a Least-Square minimization technique. The weighted average is performed thanks to the following
+functor:
+
+\snippet tutorial-pf-curve-fitting-pf.cpp Average_functor
+
+A Particle Filter needs a process function to project the particles forward in time. For this scenario,
+we decided to use the identity, and let the randomness of the Particle Filter manage the potential motion
+of the wire.
+
+\snippet tutorial-pf-curve-fitting-pf.cpp Process_function
+
+The initialization parameters of the Particle Filter are defined in the following section of code:
+
+\snippet tutorial-pf-curve-fitting-pf.cpp Constants_for_the_PF
+
+The initialization functions of the Particle Filter are defined in the following section of code:
+
+\snippet tutorial-pf-curve-fitting-pf.cpp Init_functions
+
+The Particle Filter is then constructed thanks to the following lines:
+
+\snippet tutorial-pf-curve-fitting-pf.cpp Init_PF
+
+Finally, the filtering is performed thanks to the following line:
+
+\snippet tutorial-pf-curve-fitting-pf.cpp Perform_filtering
+
+The filtered state is retrieve thanks to the following line:
+
+\snippet tutorial-pf-curve-fitting-pf.cpp Get_filtered_state
+
+\subsubsection tuto-pf-cf-tutorial-explained-all Details on the tutorial-pf-curve-fitting-all.cpp
+
+The \ref tutorial-pf-curve-fitting-all.cpp reuse what has already been presented in the \ref tuto-pf-cf-tutorial-explained-lms
+and \ref tuto-pf-cf-tutorial-explained-pf sections.
+
+Its main objective is to compare the time performances and robustness against noise of the two methods.
+The ratio of noise can be set using the Command Line Interface using the --noise option. For instance,
+
+\code
+$ ./tutorial-pf-curve-fitting-all --video data/color_image_0%03d.png --noise <ratio in the intervall [0.; 1.[>
+\endcode
+
+will add 50% of noisy pixels to the skeletonized image.
+
+You can experiment by varying this parameter (and others as well) to see the impact on the different methods.
+With 15% noisy pixels and more, the Particle Filter tends to have a greater accuracy than the LMS method, but
+it takes more times to run.
+*/