diff --git a/3rdparty/simdlib/CMakeLists.txt b/3rdparty/simdlib/CMakeLists.txt index b3a56b4f80..b0a605b8be 100644 --- a/3rdparty/simdlib/CMakeLists.txt +++ b/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() diff --git a/ChangeLog.txt b/ChangeLog.txt index 6af835006b..281b973c9a 100644 --- a/ChangeLog.txt +++ b/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 diff --git a/doc/image/tutorial/misc/img-tutorial-pf-cf-helper.jpg b/doc/image/tutorial/misc/img-tutorial-pf-cf-helper.jpg new file mode 100644 index 0000000000..3b7b34cd34 Binary files /dev/null and b/doc/image/tutorial/misc/img-tutorial-pf-cf-helper.jpg differ diff --git a/doc/image/tutorial/misc/img-tutorial-pf-cf-illustration.jpg b/doc/image/tutorial/misc/img-tutorial-pf-cf-illustration.jpg new file mode 100644 index 0000000000..ef67aadf1e Binary files /dev/null and b/doc/image/tutorial/misc/img-tutorial-pf-cf-illustration.jpg differ diff --git a/doc/image/tutorial/misc/img-tutorial-pf-cf-init.jpg b/doc/image/tutorial/misc/img-tutorial-pf-cf-init.jpg new file mode 100644 index 0000000000..26d37646cd Binary files /dev/null and b/doc/image/tutorial/misc/img-tutorial-pf-cf-init.jpg differ diff --git a/doc/image/tutorial/misc/img-tutorial-pf-cf-run.jpg b/doc/image/tutorial/misc/img-tutorial-pf-cf-run.jpg new file mode 100644 index 0000000000..c2bf65867e Binary files /dev/null and b/doc/image/tutorial/misc/img-tutorial-pf-cf-run.jpg differ diff --git a/doc/image/tutorial/misc/img-tutorial-pf-run.jpg b/doc/image/tutorial/misc/img-tutorial-pf-run.jpg new file mode 100644 index 0000000000..e3292d79d1 Binary files /dev/null and b/doc/image/tutorial/misc/img-tutorial-pf-run.jpg differ diff --git a/doc/tutorial/misc/tutorial-pf-curve-fitting.dox b/doc/tutorial/misc/tutorial-pf-curve-fitting.dox new file mode 100644 index 0000000000..6837047503 --- /dev/null +++ b/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 +\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 +\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 +\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 +\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] + +Important note: 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 +\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. +*/ diff --git a/doc/tutorial/misc/tutorial-pf.dox b/doc/tutorial/misc/tutorial-pf.dox new file mode 100644 index 0000000000..a8f275b0c0 --- /dev/null +++ b/doc/tutorial/misc/tutorial-pf.dox @@ -0,0 +1,430 @@ +/** + \page tutorial-pf Tutorial: Using Particle Filter to filter your data + \tableofcontents + +\section tuto-pf-intro Introduction + +We suppose that you are already familiar with the \ref tutorial-ukf. + +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 \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. + +The equations that describe the motion of the object in the world frame are the following: + +\f[ + \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} +\f] + +where \f$ \omega \f$ and \f$ \phi \f$ are respectively the pulsation and the phase of the motion, while \f$ R \f$ is the +radius of the revolution around the origin of the world frame. + +\htmlonly +\endhtmlonly +\image html img-tutorial-ukf-illustration.jpg + +\subsection tuto-pf-intro-methods 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 \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$. + +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$ f(\textbf{x}_i, \Delta t) : \textit{S} \times R \rightarrow \textit{S} \f$ the process function that project the forward in time. +All the particles pass through the function , and some noise \f$ \epsilon \f$ is independently added to each of them to form the new +particles: + +\f[ + \textbf{x}_i(t + \Delta t) = f( \textbf{x}_i(t) , \Delta t ) + \epsilon +\f] + +The second step of the PF is to update the weights \f$ w_i \f$ associated to each particle based on new measurements. +The update is based on the likelihood of a particle based on the measurements \f$ \textbf{z} \in \textit{M} \f$, where +\f$ \textit{M} \f$ is the measurement space. Be \f$ l: \textit{S} \times \textit{M} \rightarrow [0; 1.] \f$ the likelihood function, +we have: + +\f[ + w_i = l(\textbf{x}_i, \textbf{z}) +\f] + +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 \f$ \textbf{x}_{filtered} \f$ by performing a weighted mean of the particles +\f$ \textbf{x}_i \f$. Be \f$ \textbf{w} = (w_0 \dots w_{N-1})^T \in R^N \f$, \f$ \textbf{x} = {\textbf{x}_0 \dots \textbf{x}_{N-1}} \in \textit{S}^N \f$ +and \f$ wm: R^N \times \textit{S}^N \rightarrow \textit{S} \f$ the weighted mean function of the state space +\f$ \textit{S} \f$, we have: + +\f[ + \textbf{x}_{filtered} = wm(\textbf{w}, \textbf{x}) +\f] + +\section tuto-pf-tutorial Explanations about the tutorial + +\subsection tuto-pf-tutorial-howtorun How to run the tutorial + +To run the tutorial, please run the following commands: + +``` +$ cd $VISP_WS/visp-build/tutorial/particle-filter +$ ./tutorial-pf +``` + +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: + +\htmlonly +\endhtmlonly +\image html img-tutorial-pf-run.jpg "Screenshot of the tutorial Graphical User Interface" + +Press `Return` to leave the program. + +\subsection tuto-pf-tutorial-explained 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 \f$ \omega \f$ of the motion: + +\f[ + \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} +\f] + +The measurement \f$ \textbf{z} \f$ corresponds to the perspective projection of the different markers in the 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}[2i] &=& u_i \\ + \textbf{z}[2i+1] &=& v_i + \end{array} +\f] + +Be \f$ \textbf{K}_{intr} \f$ the camera instrinsic parameters matrix defined by: + +\f$ \textbf{K}_{intr} = \begin{pmatrix} + p_x & 0 & u_0 \\ + 0 & p_y & v_0 \\ + 0 & 0 & 1 + \end{pmatrix} +\f$ + +where \f$ (u_0, v_0, 1)^T \f$ are the coordinates of the principal point and \f$ p_x \f$ (resp. \f$ p_y \f$) is the ratio +between the focal lens of the camera and the width (resp. height) of a pixel. + +Be \f$ \boldsymbol{\pi} \f$ the projection matrix that is, in the case of a perspective +projection model, given by: + +\f$ \boldsymbol{\pi} = \begin{pmatrix} + 1 & 0 & 0 & 0 \\ + 0 & 1 & 0 & 0 \\ + 0 & 0 & 1 & 0 + \end{pmatrix} +\f$ + +The perspective projection \f$ \textbf{p} = (u, v, 1)^T \f$ of a point \f$ {}^W\textbf{X} = ({}^WX_x, {}^WX_y, {}^WX_z, 1)^T \f$ +is given by: + +\f$ \textbf{p} = \textbf{K}_{intr} \boldsymbol{\pi} {}^C\textbf{M}_W {}^W\textbf{X} \f$ + +where \f$ {}^C\textbf{M}_W \f$ is the homogeneous matrix that expresses the pose of the world coordinate frame \f$ {F}_W \f$ +with regard to the camera frame \f$ {F}_C \f$. + +\subsubsection tuto-pf-tutorial-explained-includes Details on the includes + +To have a Graphical User Interface (GUI), we include the following files. + +\snippet tutorial-pf.cpp Display_includes + +To be able to use the PF, we use the following includes: + +\snippet tutorial-pf.cpp PF_includes + +To be able to use an UKF for comparison purpose, we use the following includes: + +\snippet tutorial-pf.cpp UKF_includes + +\subsubsection tuto-pf-tutorial-explained-plate 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. + +\snippet tutorial-pf.cpp Object_simulator + +\subsubsection tuto-pf-tutorial-explained-fx Details on the process function + +As mentionned in \ref tuto-pf-intro-methods and \ref tuto-ukf-intro-methods, 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 \f$ \textbf{x}_{t + \Delta t} \f$ as a function of its +previous state \f$ \textbf{x}_{t} \f$. + +As stated in the \ref tuto-pf-intro, the equations of motion of the object are the following: + +\f[ + \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} +\f] + +Thus, we have: + +\f[ + \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} +\f] + +Which can be rewritten: +\f[ + \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} +\f] + +And can be finally written as: +\f[ + \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} +\f] + +This motivates us to choose the following non-linear process function: + +\f[ + \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} +\f] + +As the process function is pretty simple, a simple function called here `fx()` is enough: + +\snippet tutorial-pf.cpp Process_function + +\subsubsection tuto-pf-tutorial-explained-markers Details on the class simulating marker measurement + +The measurements of the projection of the markers in the image are handled by the following class: + +\snippet tutorial-pf.cpp Markers_class + +It takes as input the camera parameters cam, the homogeneous matrix expressing the pose of the world frame +\f$ {F}_W \f$ with regard to the camera frame \f$ {F}_C \f$ 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 \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 +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. + +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): + +\snippet tutorial-pf.cpp Measurement_function + +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: + +\snippet tutorial-pf.cpp GT_measurements + +The method measureWithNoise adds noise to the ground truth measurements in order to simulate a noisy +measurement process: + +\snippet tutorial-pf.cpp Noisy_measurements + +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. + +\f[ + w_i = l(\textbf{x}_i, \textbf{z}) := \frac{1}{\sqrt{2. * \Pi * \sigma_l^2}} exp^{- \frac{\overline{e}}{2 * \sigma_l^2}} +\f] + +where \f$ \overline{e} = \frac{\sum_i e_i}{N}\f$ is the mean reprojection error of the markers. + +Here is the corresponding code: + +\snippet tutorial-pf.cpp Likelihood_function + +\subsubsection tuto-pf-tutorial-explained-pose 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. + +\snippet tutorial-pf.cpp Pose_for_display + +\subsubsection tuto-pf-tutorial-explained-constants 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: + +\snippet tutorial-pf.cpp Constants_for_simulation + +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: + +\snippet tutorial-pf.cpp Camera_for_measurements + +\subsubsection tuto-pf-tutorial-explained-initpf Details on the initialization of the PF + +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. + +These parameters can be set using the Command Line Interface (CLI) thanks to the following structure: + +\snippet tutorial-pf.cpp CLI + +They are thereafter used in the following section of code of the main function: + +\snippet tutorial-pf.cpp Constants_for_the_PF + +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$. + +The section of code corresponding to the declaration of these functions is the following: + +\snippet tutorial-pf.cpp Init_functions_pf + +When both the constants and the functions have been declared, it is possible to create the PF +using the following code: + +\snippet tutorial-pf.cpp Init_PF + +\subsubsection tuto-pf-tutorial-explained-initukf Initialization of the UKF, used for comparison purpose + +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 + +\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. 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 + +Then, we initialize the simple renderer that displays what the camera sees: + +\snippet tutorial-pf.cpp Init_renderer + +\subsubsection tuto-pf-tutorial-explained-initloop 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. + +\snippet tutorial-pf.cpp Init_simu + +\subsubsection tuto-pf-tutorial-explained-loop Details on the loop + +The main loop of the program is the following: + +\snippet tutorial-pf.cpp Simu_loop + +First, we update the ground-truth 3D position of the object based on the simulated time using the following line: + +\snippet tutorial-pf.cpp Update obj pose + +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: + +\snippet tutorial-pf.cpp Update_measurement + +Then, we use the Particle Filter to filter the noisy measurements: + +\snippet tutorial-pf.cpp PF_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: + +\snippet tutorial-pf.cpp Update_displays + +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: + +\snippet tutorial-pf.cpp Noisy_pose + +Then, we update the plot by plotting the new ground truth, filtered and noisy 3D positions: + +\snippet tutorial-pf.cpp Update_plot + +Finally, we update the renderer that displays the projection in the image of the markers: + +\snippet tutorial-pf.cpp Update_renderer + +The program stops once the `Return` key is pressed. + +\section tuto-pf_next Next tutorial +You are now ready to see the next \ref tutorial-pf-curve-fitting. +*/ diff --git a/doc/tutorial/misc/tutorial-ukf.dox b/doc/tutorial/misc/tutorial-ukf.dox index 9725d28c1c..7297c2a859 100644 --- a/doc/tutorial/misc/tutorial-ukf.dox +++ b/doc/tutorial/misc/tutorial-ukf.dox @@ -219,7 +219,7 @@ As stated in the \ref tuto-ukf-intro, the equations of motion of the object are Thus, we have: \f[ - \begin{array}{lcl} + \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 @@ -228,7 +228,7 @@ Thus, we have: Which can be rewritten: \f[ - \begin{array}{lcl} + \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 @@ -237,7 +237,7 @@ Which can be rewritten: And can be finally written as: \f[ - \begin{array}{lcl} + \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 @@ -247,7 +247,7 @@ And can be finally written as: This motivates us to choose the following non-linear process function: \f[ - \begin{array}{lcl} + \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} \\ @@ -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 @@ -426,11 +426,9 @@ Finally, we update the renderer that displays the projection in the image of the \snippet tutorial-ukf.cpp Update_renderer -\subsubsection tuto-ukf-tutorial-explained-cleaning Details on the cleaning at the end of the program - -Finally, we clean the allocated memory for the renderer: +The program stops once the `Return` key is pressed. -\snippet tutorial-ukf.cpp Delete_renderer +\section tuto-ukf_next Next tutorial +You are now ready to see the next \ref tutorial-pf. -The program stops once the `Return` key is pressed. */ diff --git a/doc/tutorial/tutorial-users.dox b/doc/tutorial/tutorial-users.dox index 20ae9ff505..2018a214bf 100644 --- a/doc/tutorial/tutorial-users.dox +++ b/doc/tutorial/tutorial-users.dox @@ -195,6 +195,8 @@ This page introduces the user to other tools that may be useful. - \subpage tutorial-synthetic-blenderproc
This tutorial shows you how to easily generate synthetic data from the 3D model of an object and obtain various modalities. This data can then be used to train a neural network for your own task. - \subpage tutorial-spc
This tutorial shows you how to monitor if a signal is "in control" using Statistical Process Control methods. - \subpage tutorial-ukf
This tutorial shows you how to use an Unscented Kalman Filter to filter data when the model and/or measurements are non-linear. +- \subpage tutorial-pf
This tutorial shows you how to use a Particle Filter to filter data when the model and/or measurements are non-linear. +- \subpage tutorial-pf-curve-fitting
This tutorial shows you how to use a Particle Filter to model a wire using polynomial interpolation. - \subpage tutorial-npz
This tutorial shows you how to read / save arrays of data from / to NPZ file format, a binary format compatible with the NumPy library. */ diff --git a/example/device/framegrabber/readRealSenseData.cpp b/example/device/framegrabber/readRealSenseData.cpp index 6d134ecb3d..f4bde88236 100644 --- a/example/device/framegrabber/readRealSenseData.cpp +++ b/example/device/framegrabber/readRealSenseData.cpp @@ -407,7 +407,6 @@ int main(int argc, const char *argv[]) bool quit = false; while (!quit) { double t = vpTime::measureTimeMs(); - #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) { std::lock_guard lock(mutex); @@ -434,9 +433,11 @@ int main(int argc, const char *argv[]) d2.init(I_depth, I_color.getWidth() + 10, 0, "Depth image"); } #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) - pcl_viewer.setPosition(I_color.getWidth() + 10, I_color.getHeight() + 70); - pcl_viewer.setWindowName("3D point cloud"); - pcl_viewer.startThread(std::ref(mutex), pointcloud); + if (pointcloud->size() > 0) { + pcl_viewer.setPosition(I_color.getWidth() + 10, I_color.getHeight() + 70); + pcl_viewer.setWindowName("3D point cloud"); + pcl_viewer.startThread(std::ref(mutex), pointcloud); + } #endif } diff --git a/example/kalman/ukf-nonlinear-complex-example.cpp b/example/kalman/ukf-nonlinear-complex-example.cpp index 6ae7fadb34..fce3da31e6 100644 --- a/example/kalman/ukf-nonlinear-complex-example.cpp +++ b/example/kalman/ukf-nonlinear-complex-example.cpp @@ -352,7 +352,8 @@ class vpBicycleModel } /** - * \brief Models the effect of the command on the state model. + * \brief Move the robot according to its current position and + * the commands. * * \param[in] u The commands. u[0] = velocity ; u[1] = steeringAngle . * \param[in] x The state model. x[0] = x ; x[1] = y ; x[2] = heading @@ -412,7 +413,7 @@ class vpLandmarkMeasurements * \brief Perfect measurement of the range and relative orientation of the robot * located at pos. * - * \param[in] pos The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading. + * \param[in] pos The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading). * \return vpColVector [0] the range [1] the relative orientation of the robot. */ vpColVector measureGT(const vpColVector &pos) @@ -489,7 +490,7 @@ class vpLandmarksGrid * \brief Perfect measurement from each landmark of the range and relative orientation of the robot * located at pos. * - * \param[in] pos The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading. + * \param[in] pos The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading). * \return vpColVector n x ([0] the range [1] the relative orientation of the robot), where * n is the number of landmarks. */ diff --git a/example/particle-filter/CMakeLists.txt b/example/particle-filter/CMakeLists.txt index 7931be8865..a888328ea1 100644 --- a/example/particle-filter/CMakeLists.txt +++ b/example/particle-filter/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(VISP REQUIRED visp_core visp_gui) set(example_cpp) list(APPEND example_cpp pf-nonlinear-example.cpp) +list(APPEND example_cpp pf-nonlinear-complex-example.cpp) foreach(cpp ${example_cpp}) visp_add_target(${cpp}) @@ -15,5 +16,7 @@ foreach(cpp ${example_cpp}) endif() endforeach() -visp_add_test(pf-nonlinear-example-monothread pf-nonlinear-example --nb-particles 500 --ampli-max-X 0.02 --ampli-max-Y 0.02 --ampli-max-Z 0.01 --nb-steps-main 300 --max-distance-likelihood 10 --nb-threads 1 --seed 4224 ${OPTION_TO_DESACTIVE_DISPLAY}) -visp_add_test(pf-nonlinear-example-multithread pf-nonlinear-example --nb-particles 500 --ampli-max-X 0.02 --ampli-max-Y 0.02 --ampli-max-Z 0.01 --nb-steps-main 300 --max-distance-likelihood 10 --nb-threads -1 --seed 4224 ${OPTION_TO_DESACTIVE_DISPLAY}) +visp_add_test(pf-nonlinear-example-monothread pf-nonlinear-example ${OPTION_TO_DESACTIVE_DISPLAY} --dt 3 --stdev-range 5 --stdev-elev-angle 0.5 --stdev-aircraft-vel 0.2> --gt-X0 -500 --gt-Y0 1000 --gt-vX0 10 --gt-vY0 5 --max-distance-likelihood 50 -N 500 --seed 4224 --nb-threads 1 --ampli-max-X 20 --ampli-max-Y 200 --ampli-max-vX 1 --ampli-max-vY 0.5) +visp_add_test(pf-nonlinear-example-multithread pf-nonlinear-example ${OPTION_TO_DESACTIVE_DISPLAY} --dt 3 --stdev-range 5 --stdev-elev-angle 0.5 --stdev-aircraft-vel 0.2> --gt-X0 -500 --gt-Y0 1000 --gt-vX0 10 --gt-vY0 5 --max-distance-likelihood 50 -N 500 --seed 4224 --nb-threads -1 --ampli-max-X 20 --ampli-max-Y 200 --ampli-max-vX 1 --ampli-max-vY 0.5) +visp_add_test(pf-nonlinear-complex-example-monothread pf-nonlinear-complex-example ${OPTION_TO_DESACTIVE_DISPLAY} --max-distance-likelihood 0.5 --ampli-max-X 0.25 --ampli-max-Y 0.25 --ampli-max-theta 0.1 -N 500 --nb-threads 1) +visp_add_test(pf-nonlinear-complex-example-multithread pf-nonlinear-complex-example ${OPTION_TO_DESACTIVE_DISPLAY} --max-distance-likelihood 0.5 --ampli-max-X 0.25 --ampli-max-Y 0.25 --ampli-max-theta 0.1 -N 500 --nb-threads -1) diff --git a/example/particle-filter/pf-nonlinear-complex-example.cpp b/example/particle-filter/pf-nonlinear-complex-example.cpp new file mode 100644 index 0000000000..86ce9da936 --- /dev/null +++ b/example/particle-filter/pf-nonlinear-complex-example.cpp @@ -0,0 +1,932 @@ +/* + * 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. + */ + +/*! + \example 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: + \f[ + \begin{array}{lcl} + \textbf{x}[0] &=& x \\ + \textbf{x}[1] &=& y \\ + \textbf{x}[2] &=& \theta + \end{array} + \f] + where \f$ \theta \f$ is the heading of the robot. + + The measurement \f$ \textbf{z} \f$ corresponds to the distance and relative orientation of the + robot with different landmarks. Be \f$ p_x^i \f$ and \f$ p_y^i \f$ the position of the \f$ i^{th} \f$ landmark + along the x and y axis, the measurement vector can be written as: + \f[ + \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} + \f] + + 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: + + \f$ mean(\boldsymbol{\theta}) = atan2 (\frac{\sum_{i=1}^n \sin{\theta_i}}{n}, \frac{\sum_{i=1}^n \cos{\theta_i}}{n}) \f$ + + As the Particle Filter inference uses a weighted mean, the actual implementation of the weighted mean + of several angles is the following: + + \f$ 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}) \f$ + + where \f$ w_m^i \f$ is the weight associated to the \f$ i^{th} \f$ measurements for the weighted mean. + + Additionally, the addition and subtraction of angles must be carefully done, as the result + must stay in the interval \f$[- \pi ; \pi ]\f$ or \f$[0 ; 2 \pi ]\f$ . We decided to use + the interval \f$[- \pi ; \pi ]\f$ . +*/ + +// ViSP includes +#include +#include +#include +#ifdef VISP_HAVE_DISPLAY +#include +#endif + +// PF includes +#include + + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) + +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + +namespace +{ +/** + * \brief Normalize the \b angle in the interval [-Pi; Pi]. + * + * \param[in] angle Angle to normalize. + * \return double Normalized angle. + */ +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; +} + +/** + * \brief Compute the addition between two vectors expressed in the state space, + * such as v[0] = x ; v[1] = y; v[2] = heading . + * + * \param[in] state State to which we must add something. + * \param[in] toAdd The something we must add. + * \return vpColVector \b state + \b toAdd . + */ +vpColVector stateAdd(const vpColVector &state, const vpColVector &toAdd) +{ + vpColVector add = state + toAdd; + add[2] = normalizeAngle(add[2]); + return add; +} + +/** + * \brief Compute the weighted mean of state vectors. + * + * \param[in] states The state vectors. + * \param[in] wm The associated weights. + * \return vpColVector + */ +vpColVector stateMean(const std::vector &states, const std::vector &wm, const vpParticleFilter::vpStateAddFunction &/*addFunc*/) +{ + vpColVector mean(3, 0.); + unsigned int nbPoints = static_cast(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; +} + +/** + * \brief Compute the commands realising a turn at constant linear velocity. + * + * \param[in] v Constant linear velocity. + * \param[in] angleStart Starting angle (in degrees). + * \param[in] angleStop Stop angle (in degrees). + * \param[in] nbSteps Number of steps to perform the turn. + * \return std::vector The corresponding list of commands. + */ +std::vector generateTurnCommands(const double &v, const double &angleStart, const double &angleStop, const unsigned int &nbSteps) +{ + std::vector cmds; + double dTheta = vpMath::rad(angleStop - angleStart) / static_cast(nbSteps - 1); + for (unsigned int i = 0; i < nbSteps; ++i) { + double theta = vpMath::rad(angleStart) + dTheta * static_cast(i); + vpColVector cmd(2); + cmd[0] = v; + cmd[1] = theta; + cmds.push_back(cmd); + } + return cmds; +} + +/** + * \brief Generate the list of commands for the simulation. + * + * @return std::vector The list of commands to use in the simulation + */ +std::vector generateCommands() +{ + std::vector cmds; + // Starting by an straight line acceleration + unsigned int nbSteps = 30; + double dv = (1.1 - 0.001) / static_cast(nbSteps - 1); + for (unsigned int i = 0; i < nbSteps; ++i) { + vpColVector cmd(2); + cmd[0] = 0.001 + static_cast(i) * dv; + cmd[1] = 0.; + cmds.push_back(cmd); + } + + // Left turn + double lastLinearVelocity = cmds[cmds.size() -1][0]; + std::vector 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 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; +} +} + +/** + * \brief Models the effect of the command on the state model. + * + * \param[in] u The commands. u[0] = velocity ; u[1] = steeringAngle . + * \param[in] x The state model. x[0] = x ; x[1] = y ; x[2] = heading + * \param[in] dt The period. + * \param[in] w The length of the wheelbase. + * \return vpColVector The state model after applying the command. + */ +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; + } +} + +/** + * \brief As the state model {x, y, \f$ \theta \f$} does not contain any velocity + * information, it does not evolve without commands. + * Thus, we create a functor that will save the current command to use it in the + * process function to project a particle in time. + */ +class vpProcessFunctor +{ +public: + /** + * \brief Construct a new vp Process Functor object + * + * \param[in] w The length of the wheelbase. + */ + vpProcessFunctor(const double &w) + : m_w(w) + { } + + /** + * \brief Models the effect of the command on the state model. + * + * \param[in] x The state model. x[0] = x ; x[1] = y ; x[2] = heading + * \param[in] dt The period. + * \return vpColVector The state model after applying the command. + */ + 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; + } + + /** + * \brief Set the Commands object + * + * \param[in] u Set the commands of the current timestep. + */ + void setCommands(const vpColVector &u) + { + m_u = u; + } +private: + double m_w; /*!< The length of the wheelbase.*/ + vpColVector m_u; /*!< The commands.*/ +}; + +/** + * \brief Class that approximates a 4-wheel robot using a bicycle model. + */ +class vpBicycleModel +{ +public: + /** + * \brief Construct a new vpBicycleModel object. + * + * \param[in] w The length of the wheelbase. + */ + vpBicycleModel(const double &w) + : m_w(w) + { } + + /** + * \brief Models the effect of the command on the state model. + * + * \param[in] u The commands. u[0] = velocity ; u[1] = steeringAngle . + * \param[in] x The state model. x[0] = x ; x[1] = y ; x[2] = heading + * \param[in] dt The period. + * \return vpColVector The state model after applying the command. + */ + vpColVector computeMotion(const vpColVector &u, const vpColVector &x, const double &dt) + { + return computeMotionFromCommand(u, x, dt, m_w); + } + + /** + * \brief Move the robot according to its current position and + * the commands. + * + * \param[in] u The commands. u[0] = velocity ; u[1] = steeringAngle . + * \param[in] x The state model. x[0] = x ; x[1] = y ; x[2] = heading + * \param[in] dt The period. + * \return vpColVector The state model after applying the command. + */ + 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. +}; + +/** + * \brief Class that permits to convert the position + heading of the 4-wheel + * robot into measurements from a landmark. + */ +class vpLandmarkMeasurements +{ +public: + /** + * \brief Construct a new vpLandmarkMeasurements object. + * + * \param[in] x The position along the X-axis of the landmark. + * \param[in] y The position along the Y-axis of the landmark. + * \param[in] range_std The standard deviation of the range measurements. + * \param[in] rel_angle_std The standard deviation of the relative angle measurements. + */ + 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) + { } + + /** + * \brief Convert a particle of the Particle Filter into the measurement space. + * + * \param[in] particle The prior. + * \return vpColVector The prior expressed in the measurement space. + */ + 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; + } + + /** + * \brief Perfect measurement of the range and relative orientation of the robot + * located at pos. + * + * \param[in] pos The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading). + * \return vpColVector [0] the range [1] the relative orientation of the robot. + */ + 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; + } + + /** + * \brief Noisy measurement of the range and relative orientation that + * correspond to pos. + * + * \param[in] pos The actual position of the robot (pos[0]: x ; pos[1] = y ; pos[2] = heading). + * \return vpColVector [0] the range [1] the relative orientation. + */ + 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; + } + + /** + * \brief Compute the position that corresponds to a measurement. + * + * \param[in] meas The measurement vector. + * \param[out] x The X-coordinate that corresponds to the measurement. + * \param[out] y The Y-coordinate that corresponds to the measurement. + */ + 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; //!< The position along the X-axis of the landmark + double m_y; //!< The position along the Y-axis of the landmark + vpGaussRand m_rngRange; //!< Noise simulator for the range measurement + vpGaussRand m_rngRelativeAngle; //!< Noise simulator for the relative angle measurement +}; + +/** + * \brief Class that represent a grid of landmarks that measure the distance and + * relative orientation of the 4-wheel robot. + */ +class vpLandmarksGrid +{ +public: + /** + * \brief Construct a new vpLandmarksGrid object. + * + * \param[in] landmarks The list of landmarks forming the grid. + * \param[in] distMaxAllowed Maximum distance allowed for the likelihood computation. + */ + vpLandmarksGrid(const std::vector &landmarks, const double &distMaxAllowed) + : m_landmarks(landmarks) + , m_nbLandmarks(landmarks.size()) + { + double sigmaDistance = distMaxAllowed / 3.; + double sigmaDistanceSquared = sigmaDistance * sigmaDistance; + m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared); + m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared); + } + + /** + * \brief Convert a particle of the Particle Filter into the measurement space. + * + * \param[in] particle The prior. + * \return vpColVector The prior expressed in the measurement space. + */ + 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; + } + + /** + * \brief Perfect measurement from each landmark of the range and relative orientation of the robot + * located at pos. + * + * \param[in] pos The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading). + * \return vpColVector n x ([0] the range [1] the relative orientation of the robot), where + * n is the number of landmarks. + */ + 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; + } + + /** + * \brief Noisy measurement from each landmark of the range and relative orientation that + * correspond to pos. + * + * \param[in] pos The actual position of the robot (pos[0]: x ; pos[1] = y ; pos[2] = heading). + * \return vpColVector n x ([0] the range [1] the relative orientation of the robot), where + * n is the number of landmarks. + */ + 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; + } + + /** + * \brief Compute the position that corresponds to a measurement. + * As the measurements can be noisy, we take the average position + * computed for each landmark individually. + * + * \param[in] meas The measurement vector. + * \param[out] x The X-coordinate that corresponds to the measurement. + * \param[out] y The Y-coordinate that corresponds to the measurement. + */ + 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(m_nbLandmarks); + y /= static_cast(m_nbLandmarks); + } + + /** + * \brief Compute the likelihood of a particle (value between 0. and 1.) + * knowing the measurements. + * The likelihood is computed using a Gaussian function that penalizes + * a particle whose position is "far" from the average position + * computed from the landmarks measurement. + * + * \param[in] particle The particle state. + * \param[in] meas The measurements. + * \return double The likelihood of a particle (value between 0. and 1.) + */ + 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 m_landmarks; /*!< The list of landmarks forming the grid.*/ + const unsigned int m_nbLandmarks; /*!< Number of landmarks that the grid is made of.*/ + 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. +}; + +struct SoftwareArguments +{ + // --- Main loop parameters--- + static const int SOFTWARE_CONTINUE = 42; + bool m_useDisplay; //!< If true, activate the plot and the renderer if VISP_HAVE_DISPLAY is defined. + unsigned int m_nbStepsWarmUp; //!< Number of steps for the warmup phase. + // --- PF parameters--- + 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 0.. + double m_ampliMaxX; //!< Amplitude max of the noise for the state component corresponding to the X coordinate. + double m_ampliMaxY; //!< Amplitude max of the noise for the state component corresponding to the Y coordinate. + double m_ampliMaxTheta; //!< Amplitude max of the noise for the state component corresponding to the heading. + long m_seedPF; //!< Seed for the random generators of the PF. + int m_nbThreads; //!< Number of thread to use in the Particle Filter. + + SoftwareArguments() + : 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 ]" << std::endl; + std::cout << " [--max-distance-likelihood ] [-N, --nb-particles ] [--seed ] [--nb-threads ]" << std::endl; + std::cout << " [--ampli-max-X ] [--ampli-max-Y ] [--ampli-max-theta ]" << 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[]) +{ + SoftwareArguments args; + int returnCode = args.parseArgs(argc, argv); + if (returnCode != SoftwareArguments::SOFTWARE_CONTINUE) { + 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 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 cmds = generateCommands(); + const unsigned int nbCmds = static_cast(cmds.size()); + + // Initialize the attributes of the PF + std::vector stdevsPF = { args.m_ampliMaxX / 3., args.m_ampliMaxY / 3., args.m_ampliMaxTheta / 3. }; ///TODO: define + 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); + vpLandmarksGrid grid(landmarks, args.m_maxDistanceForLikelihood); + vpBicycleModel robot(wheelbase); + using std::placeholders::_1; + using std::placeholders::_2; + vpParticleFilter::vpProcessFunction f = std::bind(&vpProcessFunctor::processFunction, &processFtor, _1, _2); + vpParticleFilter::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLandmarksGrid::likelihood, &grid, _1, _2); + vpParticleFilter::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter::simpleResamplingCheck; + vpParticleFilter::vpResamplingFunction resamplingFunc = vpParticleFilter::simpleImportanceResampling; + vpParticleFilter::vpFilterFunction weightedMeanFunc = stateMean; + vpParticleFilter::vpStateAddFunction addFunc = stateAdd; + + // Initialize the PF + vpParticleFilter 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); + + double t0 = vpTime::measureTimeMicros(); + //! [Perform_filtering] + // Update the functor command + processFtor.setCommands(noMotionCommand); + // Use the PF to filter the measurement + filter.filter(z, dt); + //! [Perform_filtering] + 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(step) == 0) { + // Perform the measurement + vpColVector z = grid.measureWithNoise(robot_pos); + + double t0 = vpTime::measureTimeMicros(); + //! [Perform_filtering] + // Update the functor command + processFtor.setCommands(cmds[i]); + // Use the PF to filter the measurement + filter.filter(z, dt); + //! [Perform_filtering] + averageFilteringTime += vpTime::measureTimeMicros() - t0; + + //! [Get_filtered_state] + vpColVector Xest = filter.computeFilteredState(); + //! [Get_filtered_state] + + //! [Errors_computation] + // 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(nbCmds + args.m_nbStepsWarmUp)); + meanErrorFilter = meanErrorFilter / (static_cast(nbCmds)); + meanErrorNoise = meanErrorNoise / (static_cast(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 diff --git a/example/particle-filter/pf-nonlinear-example.cpp b/example/particle-filter/pf-nonlinear-example.cpp index 0fa342729e..d4784754bd 100644 --- a/example/particle-filter/pf-nonlinear-example.cpp +++ b/example/particle-filter/pf-nonlinear-example.cpp @@ -28,298 +28,297 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. */ -/** \example pf-nonlinear-example.cpp - * Example on how to use a Particle Filter (PF) on a complex non-linear use-case. - * The system is an object, whose coordinate frame origin is the point O, on which are sticked four markers. - * The object revolves in a plane parallel to the ground around a fixed point W whose coordinate frame is the world frame. - * The scene is observed by a pinhole camera whose coordinate frame has the origin C and which is - * fixed to the ceiling. - * - * The state vector of the PF is: - * \f{eqnarray*}{ - * \textbf{x}[0] &=& {}^WX_x \\ - * \textbf{x}[1] &=& {}^WX_y \\ - * \textbf{x}[2] &=& {}^WX_z \\ - * \textbf{x}[3] &=& \omega \Delta t - * \f} - * - * The measurement \f$ \textbf{z} \f$ corresponds to the coordinates in pixels of the different markers. - * 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{eqnarray*}{ - * \textbf{z}[2i] &=& u_i \\ - * \textbf{z}[2i+1] &=& v_i - * \f} - * - * Some noise is added to the measurement vector to simulate measurements which are - * not perfect. - */ +/*! \example pf-nonlinear-example.cpp + Example of a simple non-linear use-case of the Particle Filter (PF). + + The system we are interested in is an aircraft flying in the sky and + observed by a radar station. Its velocity is not completely constant: a Gaussian + noise is added to the velocity to simulate the effect of wind on the motion of the + aircraft. + + We consider the plan perpendicular to the ground and passing by both the radar + station and the aircraft. The x-axis corresponds to the position on the ground + and the y-axis to the altitude. + + The state vector of the PF corresponds to a constant velocity model and can be written as: + \f[ + \begin{array}{lcl} + \textbf{x}[0] &=& x \\ + \textbf{x}[1] &=& \dot{x} \\ + \textbf{x}[1] &=& y \\ + \textbf{x}[2] &=& \dot{y} + \end{array} + \f] + + The measurement \f$ \textbf{z} \f$ corresponds to the distance and angle between the ground and the aircraft + observed by the radar station. Be \f$ p_x \f$ and \f$ p_y \f$ the position of the radar station + along the x and y axis, the measurement vector can be written as: + \f[ + \begin{array}{lcl} + \textbf{z}[0] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\ + \textbf{z}[1] &=& \tan^{-1}{\frac{y - p_y}{x - p_x}} + \end{array} + \f] + + Some noise is added to the measurement vector to simulate a sensor which is + not perfect. +*/ // ViSP includes #include -#include #include -#include -#include -#include -//! [Display_includes] #ifdef VISP_HAVE_DISPLAY #include -#include #endif -//! [Display_includes] -#include -//! [PF_includes] +// PF includes #include -//! [PF_includes] + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) #ifdef ENABLE_VISP_NAMESPACE using namespace VISP_NAMESPACE_NAME; #endif -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) -//! [Process_function] +namespace +{ /** - * \brief Process function that makes evolve the state model {\f$ {}^WX_x \f$, \f$ {}^WX_y \f$, \f$ {}^WX_z \f$, \f$ C = \omega \Delta t \f$} - * over time. + * \brief The process function, that updates the prior. * - * \param[in] x The state vector - * \return vpColVector The state vector at the next iteration. + * \param[in] particle A particle of the PF. + * \param[in] dt The period. + * \return vpColVector The sigma points projected in the future. */ -vpColVector fx(const vpColVector &x, const double & /*dt*/) +vpColVector fx(const vpColVector &particle, 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; + vpColVector point(4); + point[0] = particle[1] * dt + particle[0]; + point[1] = particle[1]; + point[2] = particle[3] * dt + particle[2]; + point[3] = particle[3]; + return point; } -//! [Process_function] -//! [Object_simulator] /** - * \brief Class that simulates the moving object. + * \brief Compute the X and Y coordinates from the measurements. + * + * \param z z[0] = range z[1] = elevation angle. + * \param xRadar The X-axis coordinate that corresponds to the position of the radar on the ground. + * \param yRadar The Y-axis coordinate that correspond to the height at which is located the radar with regard to the ground level. + * \param x The X-axis coordinate that corresponds to the measurements. + * \param y The Y-axis coordinate that correspond to the measurements. */ -class vpObjectSimulator +void computeCoordinatesFromMeasurement(const vpColVector &z, const double &xRadar, const double &yRadar, double &x, double &y) { -public: - /** - * \brief Construct a new vpObjectSimulator object. - * - * \param[in] R The radius of the revolution around the world frame origin. - * \param[in] w The pulsation of the motion. - * \param[in] phi The phase of the motion. - * \param[in] wZ The y-coordinate of the object in the world frame. - */ - vpObjectSimulator(const double &R, const double &w, const double &phi, const double &wZ) - : m_R(R) - , m_w(w) - , m_phi(phi) - , m_wZ(wZ) - { } + double dx = z[0] * std::cos(z[1]); + double dy = z[0] * std::sin(z[1]); + x = dx + xRadar; + y = dy + yRadar; +} - /** - * \brief Move the object to its new position, expressed in the world frame. - * - * \param[in] t The current time. - * \return vpColVector The new position of the object in the world frame, expressed as homogeneous coordinates. - */ - vpColVector move(const double &t) - { - vpColVector wX(4, 1.); - wX[0] = m_R * std::cos(m_w * t + m_phi); - wX[1] = m_R * std::sin(m_w * t + m_phi); - wX[2] = m_wZ; - return wX; - } +/** + * \brief Compute the Eucledian norm of the error between point 0 and point 1. + * \param x0 The X-axis coordinate of point 0. + * \param y0 The Y-axis coordinate of point 0. + * \param x1 The X-axis coordinate of point 1. + * \param y1 The Y-axis coordinate of point 1. + * \return \f[||e|| = \sqrt{(x0 - x1)^2 + (y0 - y1)^2}\f] + */ +double computeError(const double &x0, const double &y0, const double &x1, const double &y1) +{ + double dx = x0 - x1; + double dy = y0 - y1; + double error = std::sqrt(dx * dx + dy * dy); + return error; +} -private: - double m_R; // Radius of the revolution around the world frame origin. - double m_w; // Pulsation of the motion. - double m_phi; // Phase of the motion. - const double m_wZ; // The z-coordinate of the object in the world frame. -}; -//! [Object_simulator] +/** + * \brief Compute the error between a state vector and a ground-truth vector. + * + * \param[in] state state[0] = X, state[1] = vX, state[2] = Y, state[3] = vY + * \param[in] gt_X gt_X[0] = X, gt_X[1] = Y + * \return double The error. + */ +double computeStateError(const vpColVector &state, const vpColVector >_X) +{ + double error = computeError(state[0], state[2], gt_X[0], gt_X[1]); + return error; +} -//! [Markers_class] /** - * \brief Class that permits to convert the 3D position of the object into measurements. + * \brief Compute the error between a measurement vector and a ground-truth vector. + * + * \param[in] z z[0] = range z[1] = elevation angle. + * \param xRadar The X-axis coordinate that corresponds to the position of the radar on the ground. + * \param yRadar The Y-axis coordinate that correspond to the height at which is located the radar with regard to the ground level. + * \param[in] gt_X gt_X[0] = X, gt_X[1] = Y + * \return double The error. + */ +double computeMeasurementsError(const vpColVector &z, const double &xRadar, const double &yRadar, const vpColVector >_X) +{ + double xMeas = 0., yMeas = 0.; + computeCoordinatesFromMeasurement(z, xRadar, yRadar, xMeas, yMeas); + double error = computeError(xMeas, yMeas, gt_X[0], gt_X[1]); + return error; +} +} + +/** + * \brief Class that permits to convert the position of the aircraft into + * range and elevation angle measurements. */ -class vpMarkersMeasurements +class vpRadarStation { public: /** - * \brief Construct a new vpMarkersMeasurements object. + * \brief Construct a new vpRadarStation object. * - * \param[in] cam The camera parameters. - * \param[in] cMw The pose of the world frame with regard to the camera frame. - * \param[in] wRo The rotation matrix expressing the rotation between the world frame and object frame. - * \param[in] markers The position of the markers in the object frame. - * \param[in] sigmaDistance Standard deviation of the Gaussian function used for the computation of the likelihood. - * An error greater than 3 times this standard deviation will lead to a likelihood equal to 0. - * \param[in] noise_stdev The standard deviation for the noise generator - * \param[in] seed The seed for the noise generator + * \param[in] x The position on the ground of the radar. + * \param[in] y The altitude of the radar. + * \param[in] range_std The standard deviation of the range measurements. + * \param[in] elev_angle_std The standard deviation of the elevation angle measurements. + * \param[in] distMaxAllowed Maximum distance allowed for the likelihood computation. */ - vpMarkersMeasurements(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMw, const vpRotationMatrix &wRo, - const std::vector &markers, const double &sigmaDistance, - const double &noise_stdev, const long &seed) - : m_cam(cam) - , m_cMw(cMw) - , m_wRo(wRo) - , m_markers(markers) - , m_rng(noise_stdev, 0., seed) + vpRadarStation(const double &x, const double &y, const double &range_std, const double &elev_angle_std, + const double &distMaxAllowed) + : m_x(x) + , m_y(y) + , m_rngRange(range_std, 0., 4224) + , m_rngElevAngle(elev_angle_std, 0., 2112) { + double sigmaDistance = distMaxAllowed / 3.; double sigmaDistanceSquared = sigmaDistance * sigmaDistance; m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared); m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared); } - //! [Likelihood_function] /** - * \brief Compute the likelihood of a particle compared to the measurements. - * The likelihood equals zero if the particle is completely different of - * the measurements and equals one if it matches completely. - * The chosen likelihood is 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. + * \brief Convert a particle of the Particle Filter into the measurement space. * - * \param[in] x The particle. - * \param[in] meas The measurement vector. meas[2i] = u_i meas[2i + 1] = v_i . - * \return double The likelihood of the particle. + * \param particle The prior. + * \return vpColVector The prior expressed in the measurement space. */ - double likelihoodParticle(const vpColVector &x, const vpColVector &meas) + vpColVector state_to_measurement(const vpColVector &particle) { - unsigned int nbMarkers = static_cast(m_markers.size()); - double likelihood = 0.; - vpHomogeneousMatrix wMo; - vpTranslationVector wTo(x[0], x[1], x[2]); - wMo.build(wTo, m_wRo); - const unsigned int sizePt2D = 2; - const unsigned int idX = 0, idY = 1, idZ = 2; - double sumError = 0.; - 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; - } - likelihood = std::exp(m_constantExpDenominator * sumError / static_cast(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; + vpColVector meas(2); + double dx = particle[0] - m_x; + double dy = particle[2] - m_y; + meas[0] = std::sqrt(dx * dx + dy * dy); + meas[1] = std::atan2(dy, dx); + return meas; } - //! [Likelihood_function] /** - * \brief Convert the state of the PF into the measurement space. + * \brief Perfect measurement of the range and elevation angle that + * correspond to pos. * - * \param[in] x The state of the PF. - * \return vpColVector The state expressed in the measurement space. + * \param pos The actual position of the aircraft (pos[0]: projection of the position + * on the ground, pos[1]: altitude). + * \return vpColVector [0] the range [1] the elevation angle. */ - vpColVector state_to_measurement(const vpColVector &x) + vpColVector measureGT(const vpColVector &pos) { - unsigned int nbMarkers = static_cast(m_markers.size()); - vpColVector meas(2*nbMarkers); - vpHomogeneousMatrix wMo; - vpTranslationVector wTo(x[0], x[1], x[2]); - wMo.build(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; + double dx = pos[0] - m_x; + double dy = pos[1] - m_y; + double range = std::sqrt(dx * dx + dy * dy); + double elevAngle = std::atan2(dy, dx); + vpColVector measurements(2); + measurements[0] = range; + measurements[1] = elevAngle; + return measurements; } - //! [GT_measurements] /** - * \brief Perfect measurement of the projection of the markers in the image when the object - * is located at \b wX. + * \brief Noisy measurement of the range and elevation angle that + * correspond to pos. * - * \param[in] wX The actual position of the robot (wX[0]: x, wX[1]: y, wX[2] = z). - * \return vpColVector [2*i] u_i [2*i + 1] v_i where i is the index of the marker. + * \param pos The actual position of the aircraft (pos[0]: projection of the position + * on the ground, pos[1]: altitude). + * \return vpColVector [0] the range [1] the elevation angle. */ - vpColVector measureGT(const vpColVector &wX) + vpColVector measureWithNoise(const vpColVector &pos) { - unsigned int nbMarkers = static_cast(m_markers.size()); - vpColVector meas(2*nbMarkers); - vpHomogeneousMatrix wMo; - vpTranslationVector wTo(wX[0], wX[1], wX[2]); - wMo.build(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 measurementsGT = measureGT(pos); + vpColVector measurementsNoisy = measurementsGT; + measurementsNoisy[0] += m_rngRange(); + measurementsNoisy[1] += m_rngElevAngle(); + return measurementsNoisy; } - //! [GT_measurements] - //! [Noisy_measurements] /** - * \brief Noisy measurement of the projection of the markers in the image when the object - * is located at \b wX. + * \brief Compute the likelihood of a particle (value between 0. and 1.) + * knowing the measurements. + * The likelihood function is based on a Gaussian function that penalizes + * a particle that is "far" from the position corresponding to the measurements. * - * \param[in] wX The actual position of the robot (wX[0]: x, wX[1]: y, wX[2] = z). - * \return vpColVector [2*i] u_i [2*i + 1] v_i where i is the index of the marker. + * \param[in] particle The particle state. + * \param[in] meas The measurements. + * \return double The likelihood of a particle (value between 0. and 1.) */ - vpColVector measureWithNoise(const vpColVector &wX) + double likelihood(const vpColVector &particle, const vpColVector &meas) { - 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 xParticle = particle[0]; + double yParticle = particle[2]; + double xMeas = 0., yMeas = 0.; + computeCoordinatesFromMeasurement(meas, m_x, m_y, xMeas, yMeas); + double dist = computeError(xParticle, yParticle, xMeas, yMeas); + 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; } - //! [Noisy_measurements] 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 m_markers; // The position of the markers in the object frame. - 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. - vpGaussRand m_rng; // Noise simulator for the measurements + double m_x; // The position on the ground of the radar + double m_y; // The altitude of the radar + vpGaussRand m_rngRange; // Noise simulator for the range measurement + vpGaussRand m_rngElevAngle; // Noise simulator for the elevation angle measurement + 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. }; -//! [Markers_class] -//! [Pose_for_display] /** - * \brief Compute the pose from the 3D coordinates of the markers and their coordinates in pixels - * in the image. - * - * \param[in] point The 3D coordinates of the markers in the object frame. - * \param[in] ip The pixel coordinates of the markers in the image. - * \param[in] cam The camera parameters used to acquire the image. - * \return vpHomogeneousMatrix The pose of the object in the camera frame. + * \brief Class to simulate a flying aircraft. */ -vpHomogeneousMatrix computePose(std::vector &point, const std::vector &ip, const vpCameraParameters &cam) +class vpACSimulator { - 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]); +public: + /** + * \brief Construct a new vpACSimulator object. + * + * \param[in] X0 Initial position of the aircraft. + * \param[in] vel Velocity of the aircraft. + * \param[in] vel_std Standard deviation of the variation of the velocity. + */ + vpACSimulator(const vpColVector &X0, const vpColVector &vel, const double &vel_std) + : m_pos(X0) + , m_vel(vel) + , m_rngVel(vel_std, 0.) + { + } - vpHomogeneousMatrix cMo; - pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo); - return cMo; -} -//! [Pose_for_display] + /** + * \brief Compute the new position of the aircraft after dt seconds have passed + * since the last update. + * + * \param[in] dt Period since the last update. + * \return vpColVector The new position of the aircraft. + */ + vpColVector update(const double &dt) + { + vpColVector dx = m_vel * dt; + dx[0] += m_rngVel() * dt; + dx[1] += m_rngVel() * dt; + m_pos += dx; + return m_pos; + } + +private: + vpColVector m_pos; // Position of the simulated aircraft + vpColVector m_vel; // Velocity of the simulated aircraft + vpGaussRand m_rngVel; // Random generator for slight variations of the velocity of the aircraft +}; struct SoftwareArguments { @@ -327,27 +326,47 @@ struct SoftwareArguments static const int SOFTWARE_CONTINUE = 42; bool m_useDisplay; //!< If true, activate the plot and the renderer if VISP_HAVE_DISPLAY is defined. unsigned int m_nbStepsWarmUp; //!< Number of steps for the warmup phase. - unsigned int m_nbSteps; //!< ?umber of steps for the main loop. + unsigned int m_nbSteps; //!< Number of steps for the main loop. + double m_dt; // Period, expressed in seconds + double m_sigmaRange; // Standard deviation of the range measurement, expressed in meters. + double m_sigmaElevAngle; // Standard deviation of the elevation angle measurent, expressed in radians. + double m_stdevAircraftVelocity; // Standard deviation of the velocity of the simulated aircraft, to make it deviate a bit from the constant velocity model + double m_radar_X; // Radar position along the X-axis, in meters + double m_radar_Y; // Radar position along the Y-axis, in meters + double m_gt_X_init; // Ground truth initial position along the X-axis, in meters + double m_gt_Y_init; // Ground truth initial position along the Y-axis, in meters + double m_gt_vX_init; // Ground truth initial velocity along the X-axis, in meters + double m_gt_vY_init; // Ground truth initial velocity along the Y-axis, in meters // --- PF parameters--- 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 0.. double m_ampliMaxX; //!< Amplitude max of the noise for the state component corresponding to the X coordinate. double m_ampliMaxY; //!< Amplitude max of the noise for the state component corresponding to the Y coordinate. - double m_ampliMaxZ; //!< Amplitude max of the noise for the state component corresponding to the Z coordinate. - double m_ampliMaxOmega; //!< Amplitude max of the noise for the state component corresponding to the pulsation. + double m_ampliMaxVx; //!< Amplitude max of the noise for the state component corresponding to the velocity along the X-axis. + double m_ampliMaxVy; //!< Amplitude max of the noise for the state component corresponding to the velocity along the Y-axis. long m_seedPF; //!< Seed for the random generators of the PF. - unsigned int m_nbThreads; //!< Number of thread to use in the Particle Filter. + int m_nbThreads; //!< Number of thread to use in the Particle Filter. SoftwareArguments() : m_useDisplay(true) , m_nbStepsWarmUp(200) , m_nbSteps(300) + , m_dt(3.) + , m_sigmaRange(5) + , m_sigmaElevAngle(vpMath::rad(0.5)) + , m_stdevAircraftVelocity(0.2) + , m_radar_X(0.) + , m_radar_Y(0.) + , m_gt_X_init(-500.) + , m_gt_Y_init(1000.) + , m_gt_vX_init(10.) + , m_gt_vY_init(5.) , m_N(500) - , m_maxDistanceForLikelihood(10.) - , m_ampliMaxX(0.02) - , m_ampliMaxY(0.02) - , m_ampliMaxZ(0.01) - , m_ampliMaxOmega(0.02) + , m_maxDistanceForLikelihood(50.) + , m_ampliMaxX(20.) + , m_ampliMaxY(200.) + , m_ampliMaxVx(1.) + , m_ampliMaxVy(0.5) , m_seedPF(4224) , m_nbThreads(1) { } @@ -365,6 +384,46 @@ struct SoftwareArguments m_nbStepsWarmUp = std::atoi(argv[i + 1]); ++i; } + else if ((arg == "--dt") && ((i+1) < argc)) { + m_dt = std::atoi(argv[i + 1]); + ++i; + } + else if ((arg == "--stdev-range") && ((i+1) < argc)) { + m_sigmaRange = std::atof(argv[i + 1]); + ++i; + } + else if ((arg == "--stdev-elev-angle") && ((i+1) < argc)) { + m_sigmaElevAngle = vpMath::rad(std::atof(argv[i + 1])); + ++i; + } + else if ((arg == "--stdev-aircraft-vel") && ((i+1) < argc)) { + m_stdevAircraftVelocity = std::atof(argv[i + 1]); + ++i; + } + else if ((arg == "--radar-X") && ((i+1) < argc)) { + m_radar_X = std::atof(argv[i + 1]); + ++i; + } + else if ((arg == "--radar-Y") && ((i+1) < argc)) { + m_radar_Y = std::atof(argv[i + 1]); + ++i; + } + else if ((arg == "--gt-X0") && ((i+1) < argc)) { + m_gt_X_init = std::atof(argv[i + 1]); + ++i; + } + else if ((arg == "--gt-Y0") && ((i+1) < argc)) { + m_gt_Y_init = std::atof(argv[i + 1]); + ++i; + } + else if ((arg == "--gt-vX0") && ((i+1) < argc)) { + m_gt_vX_init = std::atof(argv[i + 1]); + ++i; + } + else if ((arg == "--gt-vY0") && ((i+1) < argc)) { + m_gt_vY_init = std::atof(argv[i + 1]); + ++i; + } else if ((arg == "--max-distance-likelihood") && ((i+1) < argc)) { m_maxDistanceForLikelihood = std::atof(argv[i + 1]); ++i; @@ -389,12 +448,12 @@ struct SoftwareArguments m_ampliMaxY = std::atof(argv[i + 1]); ++i; } - else if ((arg == "--ampli-max-Z") && ((i+1) < argc)) { - m_ampliMaxZ = std::atof(argv[i + 1]); + else if ((arg == "--ampli-max-vX") && ((i+1) < argc)) { + m_ampliMaxVx = std::atof(argv[i + 1]); ++i; } - else if ((arg == "--ampli-max-omega") && ((i+1) < argc)) { - m_ampliMaxOmega = std::atof(argv[i + 1]); + else if ((arg == "--ampli-max-vY") && ((i+1) < argc)) { + m_ampliMaxVy = std::atof(argv[i + 1]); ++i; } else if (arg == "-d") { @@ -438,8 +497,11 @@ struct SoftwareArguments { std::cout << "SYNOPSIS" << std::endl; std::cout << " " << softName << " [--nb-steps-main ] [--nb-steps-warmup ]" << std::endl; + std::cout << " [--dt ] [--stdev-range ] [--stdev-elev-angle ] [--stdev-aircraft-vel ]" << std::endl; + std::cout << " [--radar-X ] [--radar-Y ]" << std::endl; + std::cout << " [--gt-X0 ] [--gt-Y0 ] [--gt-vX0 ] [--gt-vY0 ]" << std::endl; std::cout << " [--max-distance-likelihood ] [-N, --nb-particles ] [--seed ] [--nb-threads ]" << std::endl; - std::cout << " [--ampli-max-X ] [--ampli-max-Y ] [--ampli-max-Z ] [--ampli-max-omega ]" << std::endl; + std::cout << " [--ampli-max-X ] [--ampli-max-Y ] [--ampli-max-vX ] [--ampli-max-vY ]" << std::endl; std::cout << " [-d, --no-display] [-h]" << std::endl; } @@ -455,9 +517,55 @@ struct SoftwareArguments std::cout << " Number of steps in the warmup loop." << std::endl; std::cout << " Default: " << m_nbStepsWarmUp << std::endl; std::cout << std::endl; + std::cout << " --dt" << std::endl; + std::cout << " Timestep of the simulation, in seconds." << std::endl; + std::cout << " Default: " << m_dt << std::endl; + std::cout << std::endl; + std::cout << " --stdev-range" << std::endl; + std::cout << " Standard deviation of the range measurements, in meters." << std::endl; + std::cout << " Default: " << m_sigmaRange << std::endl; + std::cout << std::endl; + std::cout << " --stdev-elev-angle" << std::endl; + std::cout << " Standard deviation of the elevation angle measurements, in degrees." << std::endl; + std::cout << " Default: " << vpMath::deg(m_sigmaElevAngle) << std::endl; + std::cout << std::endl; + std::cout << " --stdev-aircraft-vel" << std::endl; + std::cout << " Standard deviation of the aircraft velocity, in m/s." << std::endl; + std::cout << " Default: " << m_stdevAircraftVelocity << std::endl; + std::cout << std::endl; + std::cout << " --radar-X" << std::endl; + std::cout << " Position along the X-axis of the radar, in meters." << std::endl; + std::cout << " Be careful, because singularities happen if the aircraft flies above the radar." << std::endl; + std::cout << " Default: " << m_radar_X << std::endl; + std::cout << std::endl; + std::cout << " --radar-Y" << std::endl; + std::cout << " Position along the Y-axis of the radar, in meters." << std::endl; + std::cout << " Be careful, because singularities happen if the aircraft flies above the radar." << std::endl; + std::cout << " Default: " << m_radar_Y << std::endl; + std::cout << std::endl; + std::cout << " --gt-X0" << std::endl; + std::cout << " Initial position along the X-axis of the aircraft, in meters." << std::endl; + std::cout << " Be careful, because singularities happen if the aircraft flies above the radar." << std::endl; + std::cout << " Default: " << m_gt_X_init << std::endl; + std::cout << std::endl; + std::cout << " --gt-Y0" << std::endl; + std::cout << " Initial position along the Y-axis of the aircraft, in meters." << std::endl; + std::cout << " Be careful, because singularities happen if the aircraft flies above the radar." << std::endl; + std::cout << " Default: " << m_gt_Y_init << std::endl; + std::cout << std::endl; + std::cout << " --gt-vX0" << std::endl; + std::cout << " Initial velocity along the X-axis of the aircraft, in m/s." << std::endl; + std::cout << " Be careful, because singularities happen if the aircraft flies above the radar." << std::endl; + std::cout << " Default: " << m_gt_vX_init << std::endl; + std::cout << std::endl; + std::cout << " --gt-vY0" << std::endl; + std::cout << " Initial velocity along the Y-axis of the aircraft, in m/s." << std::endl; + std::cout << " Be careful, because singularities happen if the aircraft flies above the radar." << std::endl; + std::cout << " Default: " << m_gt_vY_init << 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 << " Maximum tolerated distance between a particle and 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; @@ -482,13 +590,13 @@ struct SoftwareArguments 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 << " --ampli-max-vX" << std::endl; + std::cout << " Maximum amplitude of the noise added to a particle to the velocity along the X-axis component." << std::endl; + std::cout << " Default: " << m_ampliMaxVx << 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 << " --ampli-max-vY" << std::endl; + std::cout << " Maximum amplitude of the noise added to a particle to the velocity along the Y-axis component." << std::endl; + std::cout << " Default: " << m_ampliMaxVy << std::endl; std::cout << std::endl; std::cout << " -d, --no-display" << std::endl; std::cout << " Deactivate display." << std::endl; @@ -513,240 +621,172 @@ int main(const int argc, const char *argv[]) return returnCode; } - //! [Constants_for_simulation] - const double dt = 0.001; // Period of 0.1s - 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 - const long seedMeasurements = 42; // Seed for the measurements random generator - const std::vector 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.}) }; // Vector of the markers sticked on the object - unsigned int nbMarkers = static_cast(markers.size()); - std::vector markersAsVpPoint; - for (unsigned int i = 0; i < nbMarkers; ++i) { - vpColVector marker = markers[i]; - markersAsVpPoint.push_back(vpPoint(marker[0], marker[1], marker[2])); - } - - 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]; - //! [Constants_for_simulation] - - //! [Camera_for_measurements] - // Create a camera parameter container - // Camera initialization with a perspective projection without distortion model - const double px = 600., py = 600., u0 = 320., v0 = 240.; - vpCameraParameters cam; - cam.initPersProjWithoutDistortion(px, py, u0, v0); - //! [Camera_for_measurements] - // Initialize the attributes of the PF - //! [Constants_for_the_PF] - 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 stdevsPF = { ampliMaxX/3., ampliMaxY/3., ampliMaxZ/3., ampliMaxOmega/3. }; // Standard deviation for each state component - const long seedPF = args.m_seedPF; // Seed for the random generators of the PF - const unsigned int nbThread = args.m_nbThreads; - //! [Constants_for_the_PF] - - //! [Initial_estimates] - vpColVector X0(4U); // The initial guess for the state - X0[0] = radius; // wX = radius m - X0[1] = 0.; // wY = 0m - X0[2] = 0.95 * wZ; // Wrong estimation of the position along the z-axis: error of 5% - X0[3] = 0.95 * w * dt; // Wrong estimation of the pulsation: error of 5% - //! [Initial_estimates] - - //! [Init_functions] - vpParticleFilter::vpProcessFunction processFunc = fx; - vpMarkersMeasurements markerMeas(cam, cMw, wRo, markers, sigmaLikelihood, sigmaMeasurements, seedMeasurements); + std::vector stdevsPF = { args.m_ampliMaxX /3., args.m_ampliMaxVx /3., args.m_ampliMaxY /3. , args.m_ampliMaxVy /3. }; + int seedPF = args.m_seedPF; + unsigned int nbParticles = args.m_N; + int nbThreads = args.m_nbThreads; + + vpColVector X0(4); + X0[0] = 0.9 * args.m_gt_X_init; // x, i.e. 10% of error with regard to ground truth + X0[1] = 0.9 * args.m_gt_vX_init; // dx/dt, i.e. 10% of error with regard to ground truth + X0[2] = 0.9 * args.m_gt_Y_init; // y, i.e. 10% of error with regard to ground truth + X0[3] = 0.9 * args.m_gt_vY_init; // dy/dt, i.e. 10% of error with regard to ground truth + + vpParticleFilter::vpProcessFunction f = fx; + vpRadarStation radar(args.m_radar_X, args.m_radar_Y, args.m_sigmaRange, args.m_sigmaElevAngle, args.m_maxDistanceForLikelihood); using std::placeholders::_1; using std::placeholders::_2; - vpParticleFilter::vpLikelihoodFunction likelihoodFunc = std::bind(&vpMarkersMeasurements::likelihoodParticle, &markerMeas, _1, _2); + vpParticleFilter::vpLikelihoodFunction likelihoodFunc = std::bind(&vpRadarStation::likelihood, &radar, _1, _2); vpParticleFilter::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter::simpleResamplingCheck; vpParticleFilter::vpResamplingFunction resamplingFunc = vpParticleFilter::simpleImportanceResampling; - //! [Init_functions] - //! [Init_PF] // Initialize the PF - vpParticleFilter filter(nbParticles, stdevsPF, seedPF, nbThread); - filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc); - //! [Init_PF] + vpParticleFilter filter(nbParticles, stdevsPF, seedPF, nbThreads); + filter.init(X0, f, likelihoodFunc, checkResamplingFunc, resamplingFunc); - //! [Init_plot] #ifdef VISP_HAVE_DISPLAY - // Initialize the plot vpPlot *plot = nullptr; if (args.m_useDisplay) { - plot = new vpPlot(1); + // Initialize the plot + plot = new vpPlot(4); plot->initGraph(0, 3); - plot->setTitle(0, "Position of the robot wX"); - plot->setUnitX(0, "Position along x(m)"); - plot->setUnitY(0, "Position along y (m)"); + plot->setTitle(0, "Position along X-axis"); + plot->setUnitX(0, "Time (s)"); + plot->setUnitY(0, "Position (m)"); plot->setLegend(0, 0, "GT"); plot->setLegend(0, 1, "Filtered"); plot->setLegend(0, 2, "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::black); - } -#endif - //! [Init_plot] - //! [Init_renderer] - // Initialize the display - // Depending on the detected third party libraries, we instantiate here the - // first video device which is available -#ifdef VISP_HAVE_DISPLAY - std::shared_ptr d; - vpImage Idisp(800, 800, vpRGBa(static_cast(255))); - if (args.m_useDisplay) { - d = vpDisplayFactory::createDisplay(Idisp, 800, 50, "Projection of the markers"); + plot->initGraph(1, 3); + plot->setTitle(1, "Velocity along X-axis"); + plot->setUnitX(1, "Time (s)"); + plot->setUnitY(1, "Velocity (m/s)"); + plot->setLegend(1, 0, "GT"); + plot->setLegend(1, 1, "Filtered"); + plot->setLegend(1, 2, "Measure"); + plot->setColor(1, 0, vpColor::red); + plot->setColor(1, 1, vpColor::blue); + plot->setColor(1, 2, vpColor::black); + + plot->initGraph(2, 3); + plot->setTitle(2, "Position along Y-axis"); + plot->setUnitX(2, "Time (s)"); + plot->setUnitY(2, "Position (m)"); + plot->setLegend(2, 0, "GT"); + plot->setLegend(2, 1, "Filtered"); + plot->setLegend(2, 2, "Measure"); + plot->setColor(2, 0, vpColor::red); + plot->setColor(2, 1, vpColor::blue); + plot->setColor(2, 2, vpColor::black); + + plot->initGraph(3, 3); + plot->setTitle(3, "Velocity along Y-axis"); + plot->setUnitX(3, "Time (s)"); + plot->setUnitY(3, "Velocity (m/s)"); + plot->setLegend(3, 0, "GT"); + plot->setLegend(3, 1, "Filtered"); + plot->setLegend(3, 2, "Measure"); + plot->setColor(3, 0, vpColor::red); + plot->setColor(3, 1, vpColor::blue); + plot->setColor(3, 2, vpColor::black); } #endif - //! [Init_renderer] - //! [Init_simu] // Initialize the simulation - vpObjectSimulator object(radius, w, phi, wZ); - vpColVector object_pos(4U, 0.); - object_pos[3] = 1.; - //! [Init_simu] - + vpColVector ac_pos(2); + ac_pos[0] = args.m_gt_X_init; + ac_pos[1] = args.m_gt_Y_init; + vpColVector ac_vel(2); + ac_vel[0] = args.m_gt_vX_init; + ac_vel[1] = args.m_gt_vY_init; + vpACSimulator ac(ac_pos, ac_vel, args.m_stdevAircraftVelocity); + vpColVector gt_Xprec = ac_pos; + vpColVector gt_Vprec = ac_vel; double averageFilteringTime = 0.; + double meanErrorFilter = 0., meanErrorNoise = 0.; + double xNoise_prec = 0., yNoise_prec = 0.; - //! [Warmup_loop] + // Warmup loop const unsigned int nbStepsWarmUp = args.m_nbStepsWarmUp; for (unsigned int i = 0; i < nbStepsWarmUp; ++i) { // Update object pose - object_pos = object.move(dt * static_cast(i)); + vpColVector gt_X = ac.update(args.m_dt); // Perform the measurement - vpColVector z = markerMeas.measureWithNoise(object_pos); + vpColVector z = radar.measureWithNoise(gt_X); // Use the UKF to filter the measurement double t0 = vpTime::measureTimeMicros(); - filter.filter(z, dt); + filter.filter(z, args.m_dt); averageFilteringTime += vpTime::measureTimeMicros() - t0; - } - //! [Warmup_loop] - - //! [Simu_loop] - const unsigned int nbSteps = args.m_nbSteps; - const double invNbSteps = 1. / static_cast(nbSteps); - double meanErrorFilter = 0.; - double meanErrorNoise = 0.; + gt_Xprec = gt_X; - for (unsigned int i = 0; i < nbSteps; ++i) { - //! [Update obj pose] - // Update object pose - object_pos = object.move(dt * static_cast(i)); - //! [Update obj pose] + // Save the noisy position + computeCoordinatesFromMeasurement(z, args.m_radar_X, args.m_radar_Y, xNoise_prec, yNoise_prec); + } - //! [Update_measurement] + for (unsigned int i = 0; i < args.m_nbSteps; ++i) { // Perform the measurement - vpColVector z = markerMeas.measureWithNoise(object_pos); - //! [Update_measurement] + vpColVector gt_X = ac.update(args.m_dt); + vpColVector gt_V = (gt_X - gt_Xprec) / args.m_dt; + vpColVector z = radar.measureWithNoise(gt_X); + // Use the PF to filter the measurement double t0 = vpTime::measureTimeMicros(); - //! [Perform_filtering] - // Use the UKF to filter the measurement - filter.filter(z, dt); - //! [Perform_filtering] + filter.filter(z, args.m_dt); averageFilteringTime += vpTime::measureTimeMicros() - t0; - //! [Get_filtered_state] + // Compute the error between GT and filtered state for statistics at the end of the program vpColVector Xest = filter.computeFilteredState(); - //! [Get_filtered_state] - - //! [Noisy_pose] - // Prepare the pose computation: - // the image points corresponding to the noisy markers are needed - std::vector ip; - for (unsigned int id = 0; id < nbMarkers; ++id) { - vpImagePoint markerProjNoisy(z[2*id + 1], z[2*id]); - ip.push_back(markerProjNoisy); - } + vpColVector gtState = vpColVector({ gt_Xprec[0], gt_Vprec[0], gt_Xprec[1], gt_Vprec[1] }); + double normErrorFilter = computeStateError(Xest, gt_X); + meanErrorFilter += normErrorFilter; - // Compute the pose using the noisy markers - vpHomogeneousMatrix cMo_noisy = computePose(markersAsVpPoint, ip, cam); - vpHomogeneousMatrix wMo_noisy = cMw.inverse() * cMo_noisy; - //! [Noisy_pose] + // Compute the error between GT and noisy measurements for statistics at the end of the program + double xNoise = 0., yNoise = 0.; + computeCoordinatesFromMeasurement(z, args.m_radar_X, args.m_radar_Y, xNoise, yNoise); + double normErrorNoise = computeMeasurementsError(z, args.m_radar_X, args.m_radar_Y, gt_X); + meanErrorNoise += normErrorNoise; - //! [Update_displays] #ifdef VISP_HAVE_DISPLAY if (args.m_useDisplay) { - //! [Update_plot] - // Plot the ground truth - plot->plot(0, 0, object_pos[0], object_pos[1]); - - // Plot the filtered state - plot->plot(0, 1, Xest[0], Xest[1]); - - // Plot the noisy pose - double wXnoisy = wMo_noisy[0][3]; - double wYnoisy = wMo_noisy[1][3]; - plot->plot(0, 2, wXnoisy, wYnoisy); - //! [Update_plot] - - //! [Update_renderer] - // Display the projection of the markers - vpDisplay::display(Idisp); - vpColVector zGT = markerMeas.measureGT(object_pos); - vpColVector zFilt = markerMeas.state_to_measurement(Xest); - 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 markerProjFilt(zFilt[2*id + 1], zFilt[2*id]); - vpDisplay::displayCross(Idisp, markerProjFilt, 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"), vpColor::blue); - ipText.set_i(ipText.get_i() + 20); - vpDisplay::displayText(Idisp, ipText, std::string("Measured"), vpColor::black); - vpDisplay::flush(Idisp); - vpTime::wait(40); - //! [Update_renderer] + // Plot the ground truth, measurement and filtered state + plot->plot(0, 0, i, gt_X[0]); + plot->plot(0, 1, i, Xest[0]); + plot->plot(0, 2, i, xNoise); + + double vxNoise = (xNoise - xNoise_prec) / args.m_dt; + plot->plot(1, 0, i, gt_V[0]); + plot->plot(1, 1, i, Xest[1]); + plot->plot(1, 2, i, vxNoise); + + plot->plot(2, 0, i, gt_X[1]); + plot->plot(2, 1, i, Xest[2]); + plot->plot(2, 2, i, yNoise); + + double vyNoise = (yNoise - yNoise_prec) / args.m_dt; + plot->plot(3, 0, i, gt_V[1]); + plot->plot(3, 1, i, Xest[3]); + plot->plot(3, 2, i, vyNoise); } #endif - //! [Update_displays] - - //! [Compute_error] - double error = std::sqrt(std::pow(Xest[0] - object_pos[0], 2) + std::pow(Xest[1] - object_pos[1], 2) + std::pow(Xest[2] - object_pos[2], 2)); - meanErrorFilter += invNbSteps * error; - error = std::sqrt(std::pow(wMo_noisy[0][3] - object_pos[0], 2) + std::pow(wMo_noisy[1][3] - object_pos[1], 2) + std::pow(wMo_noisy[2][3] - object_pos[2], 2)); - meanErrorNoise += invNbSteps * error; - //! [Compute_error] + + gt_Xprec = gt_X; + gt_Vprec = gt_V; + xNoise_prec = xNoise; + yNoise_prec = yNoise; } - //! [Simu_loop] - averageFilteringTime = averageFilteringTime / (static_cast(nbSteps) + static_cast(nbStepsWarmUp)); - std::cout << "Mean error filter = " << meanErrorFilter << std::endl; - std::cout << "Mean error noise = " << meanErrorNoise << std::endl; + // COmpute and display the error statistics and computation time + meanErrorFilter /= static_cast(args.m_nbSteps); + meanErrorNoise /= static_cast(args.m_nbSteps); + averageFilteringTime = averageFilteringTime / (static_cast(args.m_nbSteps) + static_cast(nbStepsWarmUp)); + std::cout << "Mean error filter = " << meanErrorFilter << "m" << std::endl; + std::cout << "Mean error noise = " << meanErrorNoise << "m" << std::endl; std::cout << "Mean filtering time = " << averageFilteringTime << "us" << std::endl; if (args.m_useDisplay) { @@ -754,25 +794,29 @@ int main(const int argc, const char *argv[]) std::cin.get(); } - //! [Delete_displays] #ifdef VISP_HAVE_DISPLAY - // Delete the plot if it was allocated - if (plot != nullptr) { + if (args.m_useDisplay) { delete plot; } #endif - //! [Delete_displays] - if (meanErrorFilter > meanErrorNoise) { - return EXIT_FAILURE; + // For the unit tests that uses this program + const double maxError = 150.; + if (meanErrorFilter > maxError) { + std::cerr << "Error: max tolerated error = " << maxError << ", mean error = " << meanErrorFilter << std::endl; + return -1; + } + else if (meanErrorFilter >= meanErrorNoise) { + std::cerr << "Error: mean error without filter = " << meanErrorNoise << ", mean error with filter = " << meanErrorFilter << std::endl; + return -1; } - return EXIT_SUCCESS; + 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 EXIT_SUCCESS; + return 0; } #endif diff --git a/modules/core/include/visp3/core/vpCannyEdgeDetection.h b/modules/core/include/visp3/core/vpCannyEdgeDetection.h index 30d3989628..e70360f40e 100644 --- a/modules/core/include/visp3/core/vpCannyEdgeDetection.h +++ b/modules/core/include/visp3/core/vpCannyEdgeDetection.h @@ -81,11 +81,14 @@ class VISP_EXPORT vpCannyEdgeDetection * upper threshold. * \param[in] filteringType : The filtering and gradient operators to apply to the image before the edge detection * operation. + * \param[in] storeEdgePoints : If true, the list of edge-points will be available using + * \b vpCannyEdgeDetection::getEdgePointsList(). */ vpCannyEdgeDetection(const int &gaussianKernelSize, const float &gaussianStdev, const unsigned int &sobelAperture, const float &lowerThreshold = -1.f, const float &upperThreshold = -1.f, const float &lowerThresholdRatio = 0.6f, const float &upperThresholdRatio = 0.8f, - const vpImageFilter::vpCannyFilteringAndGradientType &filteringType = vpImageFilter::CANNY_GBLUR_SOBEL_FILTERING); + const vpImageFilter::vpCannyFilteringAndGradientType &filteringType = vpImageFilter::CANNY_GBLUR_SOBEL_FILTERING, + const bool &storeEdgePoints = false); // // Configuration from files #ifdef VISP_HAVE_NLOHMANN_JSON @@ -248,7 +251,7 @@ class VISP_EXPORT vpCannyEdgeDetection * \warning The mask must be reset manually by the user (either for another mask * or set to \b nullptr ) before computing the edge-map of another image. * - * @param p_mask If different of \b nullptr , a mask of booleans where \b true + * \param p_mask If different of \b nullptr , a mask of booleans where \b true * indicates that a pixel must be considered and \b false that the pixel should * be ignored. */ @@ -256,6 +259,33 @@ class VISP_EXPORT vpCannyEdgeDetection { mp_mask = p_mask; } + + /** + * \brief If set to true, the list of the detected edge-points will be available + * calling the method \b vpCannyEdgeDetection::getEdgePointsList(). + * + * \param[in] storeEdgePoints The new desired status. + */ + inline void setStoreEdgePoints(const bool &storeEdgePoints) + { + m_storeListEdgePoints = storeEdgePoints; + } + //@} + + /** @name Getters */ + //@{ + /** + * \brief Get the list of edge-points that have been detected. + * + * \return std::vector The edge-points list. + */ + inline std::vector getEdgePointsList() const + { + if (!m_storeListEdgePoints) { + throw(vpException(vpException::fatalError, "Asking for the edge-points list while not asking to store it")); + } + return m_edgePointsList; + } //@} private: typedef enum EdgeType @@ -295,9 +325,11 @@ class VISP_EXPORT vpCannyEdgeDetection must be lower than the upper threshold \b m_upperThreshold.*/ // // Edge tracking attributes + bool m_storeListEdgePoints; /*!< If true, the vector \b m_edgePointsList will contain the list of the edge points resulting from the whole algorithm.*/ std::map, EdgeType> m_edgePointsCandidates; /*!< Map that contains the strong edge points, i.e. the points for which we know for sure they are edge points, and the weak edge points, i.e. the points for which we still must determine if they are actual edge points.*/ vpImage m_edgeMap; /*!< Final edge map that results from the whole Canny algorithm.*/ + std::vector m_edgePointsList; /*!< List of the edge points that belong to the final edge map.*/ const vpImage *mp_mask; /*!< Mask that permits to consider only the pixels for which the mask is true.*/ /** @name Constructors and initialization */ diff --git a/modules/core/include/visp3/core/vpParticleFilter.h b/modules/core/include/visp3/core/vpParticleFilter.h index 64121a434d..169de74011 100644 --- a/modules/core/include/visp3/core/vpParticleFilter.h +++ b/modules/core/include/visp3/core/vpParticleFilter.h @@ -50,9 +50,47 @@ BEGIN_VISP_NAMESPACE /*! \class vpParticleFilter + \ingroup group_core_math_tools \tparam MeasurementsType The class that corresponds to the measurements used to compute the weights of the Particle Filter \brief The class permits to use a Particle Filter. + + 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$. + + 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$ f(\textbf{x}_i, \Delta t) : \textit{S} \times R \rightarrow \textit{S} \f$ the process function that project the forward in time. + All the particles pass through the function , and some noise \f$ \epsilon \f$ is independently added to each of them to form the new + particles: + + \f[ + \textbf{x}_i(t + \Delta t) = f( \textbf{x}_i(t) , \Delta t ) + \epsilon + \f] + + The second step of the PF is to update the weights \f$ w_i \f$ associated to each particle based on new measurements. + The update is based on the likelihood of a particle based on the measurements \f$ \textbf{z} \in \textit{M} \f$, where + \f$ \textit{M} \f$ is the measurement space. Be \f$ l: \textit{S} \times \textit{M} \rightarrow [0; 1.] \f$ the likelihood function, + we have: + + \f[ + w_i = l(\textbf{x}_i, \textbf{z}) + \f] + + 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. In this class, we implemented the Simple Resampling algorithm + in a dedicated method and let to the user the possibility of writing user-defined check and resampling methods. + + Finally, we can compute the new state estimate \f$ \textbf{x}_{filtered} \f$ by performing a weighted mean of the particles + \f$ \textbf{x}_i \f$. Be \f$ \textbf{w} = (w_0 \dots w_{N-1})^T \in R^N \f$, \f$ \textbf{x} = {\textbf{x}_0 \dots \textbf{x}_{N-1}} \in \textit{S}^N \f$ + and \f$ wm: R^N \times \textit{S}^N \rightarrow \textit{S} \f$ the weighted mean function of the state space + \f$ \textit{S} \f$, we have: + + \f[ + \textbf{x}_{filtered} = wm(\textbf{w}, \textbf{x}) + \f] */ template class vpParticleFilter @@ -404,6 +442,7 @@ vpParticleFilter::vpParticleFilter(const unsigned int &N, cons else { m_nbMaxThreads = nbThreads; } + omp_set_num_threads(m_nbMaxThreads); #endif // Generating the random generators unsigned int sizeState = static_cast(stdev.size()); @@ -434,6 +473,9 @@ void vpParticleFilter::init(const vpColVector &x0, const vpPro const vpResamplingConditionFunction &checkResamplingFunc, const vpResamplingFunction &resamplingFunc, const vpFilterFunction &filterFunc, const vpStateAddFunction &addFunc) { + if (x0.size() != m_noiseGenerators[0].size()) { + throw(vpException(vpException::dimensionError, "X0 does not have the same size than the vector of stdevs used to build the object")); + } m_f = f; m_stateFilterFunc = filterFunc; m_likelihood = l; @@ -453,6 +495,9 @@ void vpParticleFilter::init(const vpColVector &x0, const vpCom const vpResamplingConditionFunction &checkResamplingFunc, const vpResamplingFunction &resamplingFunc, const vpFilterFunction &filterFunc, const vpStateAddFunction &addFunc) { + if (x0.size() != m_noiseGenerators[0].size()) { + throw(vpException(vpException::dimensionError, "X0 does not have the same size than the vector of stdevs used to build the object")); + } m_bx = bx; m_stateFilterFunc = filterFunc; m_likelihood = l; @@ -644,7 +689,7 @@ void vpParticleFilter::predictMultithread(const double &dt, co template double threadLikelihood(const typename vpParticleFilter::vpLikelihoodFunction &likelihood, const std::vector &v_particles, - const vpColVector &z, std::vector &w, const int &istart, const int &ipoints) + const MeasurementsType &z, std::vector &w, const int &istart, const int &ipoints) { double sum(0.0); for (int i = istart; i< istart + ipoints; ++i) { diff --git a/modules/core/include/visp3/core/vpUniRand.h b/modules/core/include/visp3/core/vpUniRand.h index 4c9b0fde0b..5acb7fbea7 100644 --- a/modules/core/include/visp3/core/vpUniRand.h +++ b/modules/core/include/visp3/core/vpUniRand.h @@ -78,6 +78,8 @@ typedef unsigned __int32 uint32_t; #include // std::mt19937 #include // std::iota #else +#include // std::time +#include // std::rand, std::srand #include // std::random_shuffle #endif @@ -136,20 +138,32 @@ class VISP_EXPORT vpUniRand void setSeed(uint64_t initstate, uint64_t initseq); /** - * @brief Create a new vector that is a shuffled version of the \b inputVector. - * - * @tparam T : A class that possesses a copy constructor. - * @param inputVector : The input vector that must be shuffled. It will not be modified. - * @return std::vector A vector containing the same objects than \b inputVector, but that are shuffled. - */ + * @brief Create a new vector that is a shuffled version of the \b inputVector. + * + * @tparam T : A class that possesses a copy constructor. + * @param inputVector : The input vector that must be shuffled. It will not be modified. + * @param seed : The seed value. + * @return std::vector A vector containing the same objects than \b inputVector, but that are shuffled. + */ template - inline static std::vector shuffleVector(const std::vector &inputVector) + inline static std::vector shuffleVector(const std::vector &inputVector, const int32_t &seed = -1) { std::vector shuffled = inputVector; #if (VISP_CXX_STANDARD <= VISP_CXX_STANDARD_11) + if (seed > 0) { + std::srand(seed); + } + else { + std::srand(std::time(0)); + } std::random_shuffle(shuffled.begin(), shuffled.end()); #else - std::shuffle(shuffled.begin(), shuffled.end(), std::mt19937 { std::random_device{}() }); + if (seed < 0) { + std::shuffle(shuffled.begin(), shuffled.end(), std::mt19937 { std::random_device{}() }); + } + else { + std::shuffle(shuffled.begin(), shuffled.end(), std::mt19937 { static_cast(seed) }); + } #endif return shuffled; } diff --git a/modules/core/src/image/vpCannyEdgeDetection.cpp b/modules/core/src/image/vpCannyEdgeDetection.cpp index 4ca3b1083e..310814709b 100644 --- a/modules/core/src/image/vpCannyEdgeDetection.cpp +++ b/modules/core/src/image/vpCannyEdgeDetection.cpp @@ -109,6 +109,7 @@ vpCannyEdgeDetection::vpCannyEdgeDetection(const int &gaussianKernelSize, const , const unsigned int &sobelAperture, const float &lowerThreshold, const float &upperThreshold , const float &lowerThresholdRatio, const float &upperThresholdRatio , const vpImageFilter::vpCannyFilteringAndGradientType &filteringType + , const bool &storeEdgePoints ) : m_filteringAndGradientType(filteringType) , m_gaussianKernelSize(gaussianKernelSize) @@ -119,6 +120,7 @@ vpCannyEdgeDetection::vpCannyEdgeDetection(const int &gaussianKernelSize, const , m_lowerThresholdRatio(lowerThresholdRatio) , m_upperThreshold(upperThreshold) , m_upperThresholdRatio(upperThresholdRatio) + , m_storeListEdgePoints(storeEdgePoints) , mp_mask(nullptr) { initGaussianFilters(); @@ -241,6 +243,7 @@ vpCannyEdgeDetection::detect(const vpImage &I) m_edgeMap.resize(I.getHeight(), I.getWidth(), 0); m_edgeCandidateAndGradient.clear(); m_edgePointsCandidates.clear(); + m_edgePointsList.clear(); // // Step 1 and 2: filter the image and compute the gradient, if not given by the user if (!m_areGradientAvailable) { @@ -493,6 +496,9 @@ vpCannyEdgeDetection::performEdgeTracking() for (it = m_edgePointsCandidates.begin(); it != m_edgePointsCandidates_end; ++it) { if (it->second == STRONG_EDGE) { m_edgeMap[it->first.first][it->first.second] = 255; + if (m_storeListEdgePoints) { + m_edgePointsList.push_back(vpImagePoint(it->first.first, it->first.second)); + } } else if (it->second == WEAK_EDGE) { if (recursiveSearchForStrongEdge(it->first)) { diff --git a/modules/core/test/math/testParticleFilter.cpp b/modules/core/test/math/testParticleFilter.cpp new file mode 100644 index 0000000000..4e0f949b0d --- /dev/null +++ b/modules/core/test/math/testParticleFilter.cpp @@ -0,0 +1,864 @@ +/* + * 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. + * + * Description: + * Test Particle Filter functionalities. + */ + +/*! + \example testParticleFilter.cpp + + Test some vpParticleFilter functionalities. + The aim is to fit a polynomial to input data. +*/ +#include + +#if defined(VISP_HAVE_CATCH2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +#include +#include + +#include +#include +#include +#include + +#ifdef VISP_HAVE_DISPLAY +#include +#include +#endif + +#define CATCH_CONFIG_RUNNER +#include + +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + +namespace +{ +bool opt_display = false; //!< If true, activate debug display + +/*! + * \brief Model of a parabola \f[v = \sum_{i = 0}^N a_i u^i \f] where \f[N\f] is the + * degree of the polynomial. + */ +class vpParabolaModel +{ +public: + inline vpParabolaModel(const unsigned int °ree, const unsigned int &height, const unsigned int &width) + : m_degree(degree) + , m_height(static_cast(height)) + , m_width(static_cast(width)) + , m_coeffs(degree + 1, 0.) + { } + + /** + * \brief Construct a new vpParabolaModel object + * + * \param[in] coeffs The coefficients of the polynomial, where coeffs[0] = offset + * and coeffs[m_degree] is the coefficient applied to the highest degree input. + * \param[in] height The height of the input image. + * \param[in] width The width of the input image. + */ + inline vpParabolaModel(const vpColVector &coeffs, const unsigned int &height, const unsigned int &width) + : m_degree(coeffs.size() - 1) + , m_height(static_cast(height)) + , m_width(static_cast(width)) + , m_coeffs(coeffs) + { } + + /** + * \brief Construct a new vpParabolaModel object + * + * \param[in] coeffs The coefficients of the polynomial, where coeffs[0][0] = offset + * and coeffs[m_degree][0] is the coefficient applied to the highest degree input. + * \param[in] height The height of the input image. + * \param[in] width The width of the input image. + */ + inline vpParabolaModel(const vpMatrix &coeffs, const unsigned int &height, const unsigned int &width) + : m_degree(coeffs.getRows() - 1) + , m_height(static_cast(height)) + , m_width(static_cast(width)) + , m_coeffs(coeffs.getCol(0)) + { } + + inline vpParabolaModel(const vpParabolaModel &other) + { + *this = other; + } + + /** + * @brief Compute \f[v = \sum_{i = 0}^N a_i u^i \f] + * + * \param[in] u Input + * \return float The corresponding v. + */ + inline double eval(const double &u) const + { + double normalizedU = u / m_width; + double v = 0.; + for (unsigned int i = 0; i <= m_degree; ++i) { + v += m_coeffs[i] * std::pow(normalizedU, i); + } + v *= m_height; + return v; + } + + /** + * \brief Cast into a vpColVector + * + * \return coeffs a:=coeffs[0] b:=coeffs[1] c:=coeffs[2] + */ + inline vpColVector toVpColVector() const + { + return m_coeffs; + } + + inline vpParabolaModel &operator=(const vpParabolaModel &other) + { + m_degree = other.m_degree; + m_height = other.m_height; + m_width = other.m_width; + m_coeffs = other.m_coeffs; + return *this; + } + + /** + * @brief Fill the matrices that form the linear system A X = b + * where A contains the different powers of the u-coordinates, + * X contains the model coefficients and b contains the + * v-coordinates. + * + * \param[in] degree The highest degree of the polynomial. + * \param[in] height The height of the input image. + * \param[in] width The width of the input image. + * \param[in] pts The points to use to interpolate the coefficients of the parabola. + * \param[out] A The matrix that contains the different powers of the u-coordinates. + * \param[out] b The matrix that contains the v-coordinates. + * \return Fill + */ + static void fillSystem(const unsigned int °ree, const double &height, const double &width, const std::vector< vpImagePoint> &pts, vpMatrix &A, vpMatrix &b) + { + const unsigned int nbPts = pts.size(); + const unsigned int nbCoeffs = degree + 1; + std::vector normalizedPts; + for (const auto &pt: pts) { + normalizedPts.push_back(vpImagePoint(pt.get_i() / height, pt.get_j() / width)); + } + A.resize(nbPts, nbCoeffs, 1.); + b.resize(nbPts, 1); + for (unsigned int i = 0; i < nbPts; ++i) { + double u = normalizedPts[i].get_u(); + double v = normalizedPts[i].get_v(); + for (unsigned int j = 0; j < nbCoeffs; ++j) { + A[i][j] = std::pow(u, j); + } + b[i][0] = v; + } + } + +#ifdef VISP_HAVE_DISPLAY + /** + * \brief Display the fitted parabola on the image. + * + * \tparam T Either unsigned char or vpRGBa. + * \param[in] I The image on which we want to display the parabola model. + * \param[in] color The color we want to use to display the parabola. + */ + template + void display(const vpImage &I, const vpColor &color, const std::string &legend, + const unsigned int &vertPosLegend, const unsigned int &horPosLegend) + { + unsigned int width = I.getWidth(); + for (unsigned int u = 0; u < width; ++u) { + int v = eval(u); + vpDisplay::displayPoint(I, v, u, color, 1); + vpDisplay::displayText(I, vertPosLegend, horPosLegend, legend, color); + } + } +#endif + +private: + unsigned int m_degree; /*!< The highest degree of the polynomial.*/ + double m_height; /*!< The height of the input image*/ + double m_width; /*!< The width of the input image*/ + vpColVector m_coeffs; /*!< The coefficient of the polynomial, where m_coeffs[0] is the offset and m_coeffs[m_degree] is the coefficient applied to the highest degree.*/ +}; + +/** + * \brief Compute the coefficients of the 2nd degree curve for the simulated data. + * The polynomial is written as y = a x^2 + b x + c. + * + * \param[in] x0 Horizontal coordinate of the inflection point. + * \param[in] y0 Vertical coordinate of the inflection point. + * \param[in] x1 Horizontal coordinate of another point of the curve. + * \param[in] y1 Vertical coordinate of another point of the curve. + * \return vpColVector The coefficients such as v[0] = c ; v[1] = b ; v[2] = a + */ +vpColVector computeABC(const double &x0, const double &y0, const double &x1, const double &y1) +{ + double b = (y1 - y0)/(-0.5*(x1 * x1/x0) + x1 -0.5 * x0); + double a = -b / (2. * x0); + double c = y0 - 0.5 * b * x0; + return vpColVector({ c, b, a }); +} + +/** + * \brief Compute the coefficients of the 2nd degree curve for the simulated data. + * The polynomial is written as y = a x^3 + b x^2 + c x + d. + * + * \param[in] x0 Horizontal coordinate of the inflection point. + * \param[in] y0 Vertical coordinate of the inflection point. + * \param[in] x1 Horizontal coordinate of another point of the curve. + * \param[in] y1 Vertical coordinate of another point of the curve. + * \return vpColVector The coefficients such as v[0] = d ; v[1] = c ; v[2] = b ; v[3] = a + */ +vpColVector computeABCD(const double &x0, const double &y0, const double &x1, const double &y1) +{ + double factorA = -2. / (3. * (x1 + x0)); + double factorC = -1. * ((-2. * std::pow(x0, 2))/(x1 + x0) + 2 * x0); + double b = (y1 - y0)/(factorA * (std::pow(x1, 3) - std::pow(x0, 3)) + (std::pow(x1, 2) - std::pow(x0, 2)) + (x1 - x0) * factorC); + double a = factorA * b; + double c = factorC * b; + double d = y0-(a * std::pow(x0, 3) + b * std::pow(x0, 2) + c * x0); + return vpColVector({ d, c, b, a }); +} + +/** + * \brief Compute the v-coordinate of an image point based on the u-coordinate and the polynomial used + * for the simulation. + * \param[in] x The u-coordinate of the image point. + * \param[in] coeffs The coefficients of the polynomial. + * \return The corresponding v-coordinate. + */ +double computeY(const double &x, const vpColVector &coeffs) +{ + double y = 0.; + unsigned int nbCoeffs = coeffs.size(); + for (unsigned int i = 0; i < nbCoeffs; ++i) { + y += coeffs[i] * std::pow(x, i); + } + return y; +} + +/** + * \brief Generate the polynomial points corresponding to the polynomial. + * + * \param[in] xmin The lowest x-coordinate to use to generate the data. + * \param[in] xmax The highest x-coordinate to use to generate the data. + * \param[in] step The step between the x-coordinates to use. + * \param[in] coeffs The coefficients of the polynomial. + * \return std::vector Vector of points that represent the polynomial. + */ +std::vector generateSimulatedImage(const double &xmin, const double &xmax, const double &step, const vpColVector &coeffs) +{ + std::vector points; + for (double x = xmin; x <= xmax; x += step) { + double y = computeY(x, coeffs); + vpImagePoint pt(y, x); + points.push_back(pt); + } + return points; +} + +#ifdef VISP_HAVE_DISPLAY +template +void displayGeneratedImage(const vpImage &I, const std::vector &pts, const vpColor &color, + const std::string &legend, const unsigned int &vertOffset, const unsigned int &horOffset) +{ + unsigned int nbPts = pts.size(); + for (unsigned int i = 1; i < nbPts; ++i) { + vpDisplay::displayPoint(I, pts[i], color, 1); + } + vpDisplay::displayText(I, vertOffset, horOffset, legend, color); +} +#endif + +/** + * \brief Compute the initial guess of the state for the Particle Filter initialization. + * + * \param[in] pts The points to use for the initialization of the Particle Filter. + * \param[in] degree The degree of the polynomial to fit. + * \param[in] height The maximum y-coordinate. + * \param[in] width The maximum x-coordinate. + * \return vpParabolaModel The fitter model. + */ +vpParabolaModel computeInitialGuess(const std::vector &pts, const unsigned int °ree, const double &height, const double &width) +{ + vpMatrix A; // The matrix that contains the u^2, u and 1s + vpMatrix X; // The matrix we want to estimate, that contains the a, b and c coefficients. + vpMatrix b; // The matrix that contains the v values + + // Fill the matrices that form the system we want to solve + vpParabolaModel::fillSystem(degree, height, width, pts, A, b); + + // Compute the parabola coefficients using the least-mean-square method. + X = A.pseudoInverse() * b; + vpParabolaModel model(X, height, width); + return model; +} + +/** + * \brief Compute the square error between the parabola model and + * the input points \b pts. + * + * \param[in] pts The input points. + * \return double The square error. + */ +double evaluate(const std::vector &pts, const vpParabolaModel &model) +{ + double rmse = 0.; + size_t sizePts = pts.size(); + for (size_t i = 0; i < sizePts; ++i) { + const vpImagePoint &pt = pts[i]; + double u = pt.get_u(); + double v = pt.get_v(); + double v_model = model.eval(u); + double error = v - v_model; + double squareError = error * error; + rmse += squareError; + } + rmse = std::sqrt(rmse / static_cast(pts.size())); + return rmse; +} + +/** + * \brief Process function, we use a constant model. + * + * \param[in] coeffs The particle, that represents the polynomial coefficients. + */ +vpColVector fx(const vpColVector &coeffs, const double &/*dt*/) +{ + vpColVector updatedCoeffs = coeffs; + return updatedCoeffs; +} + +class vpAverageFunctor +{ +public: + vpAverageFunctor(const unsigned int °ree, const unsigned int &height, const unsigned int &width) + : m_degree(degree) + , m_height(height) + , m_width(width) + { } + + vpColVector averagePolynomials(const std::vector &particles, const std::vector &weights, const vpParticleFilter>::vpStateAddFunction &/**/) + { + const unsigned int nbParticles = particles.size(); + const double nbParticlesAsDOuble = static_cast(nbParticles); + const double sumWeight = std::accumulate(weights.begin(), weights.end(), 0.); + const double nbPointsForAverage = 10. * nbParticlesAsDOuble; + std::vector initPoints; + for (unsigned int i = 0; i < nbParticles; ++i) { + double nbPoints = std::floor(weights[i] * nbPointsForAverage / sumWeight); + if (nbPoints > 1.) { + vpParabolaModel curve(particles[i], m_height, m_width); + double widthAsDouble = static_cast(m_width); + double step = widthAsDouble / (nbPoints - 1.); + for (double u = 0.; u < widthAsDouble; u += step) { + double v = curve.eval(u); + vpImagePoint pt(v, u); + initPoints.push_back(pt); + } + } + else if (nbPoints == 1.) { + vpParabolaModel curve(particles[i], m_height, m_width); + double u = static_cast(m_width) / 2.; + double v = curve.eval(u); + vpImagePoint pt(v, u); + initPoints.push_back(pt); + } + } + vpMatrix A, X, b; + vpParabolaModel::fillSystem(m_degree, m_height, m_width, initPoints, A, b); + X = A.pseudoInverse() * b; + return vpParabolaModel(X, m_height, m_width).toVpColVector(); + } + +private: + unsigned int m_degree; //!< The degree of the polynomial. + unsigned int m_height; //!< The height of the input image. + unsigned int m_width; //!< The width of the input image. +}; + +class vpLikelihoodFunctor +{ +public: + /** + * @brief Construct a new vp Likelihood Functor object + * + * \param[in] stdev The standard deviation of the likelihood function. + * \param[in] height The height of the input image. + * \param[in] width The width of the input image. + */ + vpLikelihoodFunctor(const double &stdev, const unsigned int &height, const unsigned int &width) + : m_height(height) + , m_width(width) + { + double sigmaDistanceSquared = stdev * stdev; + m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared); + m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared); + } + + //! [Likelihood_function] + /** + * \brief Compute the likelihood of a particle compared to the measurements. + * The likelihood equals zero if the particle is completely different of + * the measurements and equals one if it matches completely. + * The chosen likelihood is 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. + * + * \param[in] coeffs The particle, which represent the parabola coefficients. + * \param[in] meas The measurement vector. + * \return double The likelihood of the particle. + */ + double likelihood(const vpColVector &coeffs, const std::vector &meas) + { + double likelihood = 0.; + unsigned int nbPoints = meas.size(); + vpParabolaModel model(coeffs, m_height, m_width); + vpColVector residuals(nbPoints); + double rmse = evaluate(meas, model); + likelihood = std::exp(rmse * m_constantExpDenominator) * 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; + } + //! [Likelihood_function] +private: + 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. + unsigned int m_height; //!< The height of the input image. + unsigned int m_width; //!< The width of the input image. +}; +} + +TEST_CASE("2nd-degree", "[vpParticleFilter][Polynomial interpolation]") +{ + /// ----- Simulation parameters ----- + const double width = 600.; //!< The width of the simulated image + const double height = 400.; //!< The height of the simulated image + const unsigned int degree = 2; //!< The degree of the polynomial in the simulated image + const unsigned int nbInitPoints = 10; //!< Number of points to compute the initial guess of the PF state + const uint64_t seedCurve = 4224; //!< The seed to generate the curve + const uint64_t seedInitPoints = 2112; //!< The seed to choose the init points + const unsigned int nbTestRepet = 10; //!< The number of times the test is repeated + const unsigned int nbWarmUpIter = 10; //!< Number of iterations for the warmup loop + const unsigned int nbEvalIter = 20; //!< Number of iterations for the evaluation loop + const double dt = 0.040; //!< Simulated period of acquisition + const int32_t seedShuffle = 4221; //!< The seed to shuffle the curve points + + /// ----- PF parameters ----- + // The maximum amplitude for the likelihood compute. + // A particle whose "distance" with the measurements is greater than this value has a likelihood of 0 + const double ampliMaxLikelihood = 16.; + const double sigmaLikelihood = ampliMaxLikelihood / 3.; //:< The corresponding standard deviation + const unsigned int nbParticles = 300; //!< Number of particles used by the particle filter + const double ratioAmpliMax(0.25); //!< Ratio of the initial guess values to use to add noise to the PF state + const long seedPF = 4221; //!< Seed of the particle filter + const int nbThreads = 1; // curvePoints = generateSimulatedImage(0, width, 1., coeffs); + +#ifdef VISP_HAVE_DISPLAY + vpImage I(height, width); + std::shared_ptr pDisplay; + if (opt_display) { + pDisplay = vpDisplayFactory::createDisplay(I); + } +#endif + + for (unsigned int iter = 0; iter < nbTestRepet; ++iter) { + // Randomly select the initialization points + std::vector suffledVector = vpUniRand::shuffleVector(curvePoints, seedShuffle); + std::vector initPoints; + for (unsigned int j = 0; j < nbInitPoints; ++j) { + initPoints.push_back(suffledVector[j]); + } + + // Compute the initial model + vpParabolaModel modelInitial = computeInitialGuess(initPoints, degree, height, width); + vpColVector X0 = modelInitial.toVpColVector(); + + // Initialize the Particle Filter + std::vector stdevsPF; + for (unsigned int i = 0; i < degree + 1; ++i) { + stdevsPF.push_back(ratioAmpliMax * X0[0] / 3.); + } + vpParticleFilter>::vpProcessFunction processFunc = fx; + vpLikelihoodFunctor likelihoodFtor(sigmaLikelihood, height, width); + using std::placeholders::_1; + using std::placeholders::_2; + vpParticleFilter>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2); + vpParticleFilter>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter>::simpleResamplingCheck; + vpParticleFilter>::vpResamplingFunction resamplingFunc = vpParticleFilter>::simpleImportanceResampling; + vpAverageFunctor averageCpter(degree, height, width); + using std::placeholders::_3; + vpParticleFilter>::vpFilterFunction meanFunc = std::bind(&vpAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3); + vpParticleFilter> filter(nbParticles, stdevsPF, seedPF, nbThreads); + filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc); + + for (unsigned int i = 0; i < nbWarmUpIter; ++i) { + filter.filter(curvePoints, dt); + } + + double meanError = 0.; + for (unsigned int i = 0; i < nbEvalIter; ++i) { + filter.filter(curvePoints, dt); + vpColVector Xest = filter.computeFilteredState(); + vpParabolaModel model(Xest, height, width); + double rmse = evaluate(curvePoints, model); + meanError += rmse; + +#ifdef VISP_HAVE_DISPLAY + if (opt_display) { + vpDisplay::display(I); + displayGeneratedImage(I, curvePoints, vpColor::red, "GT", 20, 20); + model.display(I, vpColor::blue, "Model", 40, 20); + vpDisplay::flush(I); + vpDisplay::getClick(I); + } +#endif + } + meanError /= static_cast(nbEvalIter); + std::cout << "Mean(rmse) = " << meanError << std::endl; + CHECK(meanError <= maxToleratedError); + } + } + + SECTION("Noisy", "Noise is added to the init points") + { + const double maxToleratedError = 12.; + double x0 = rngCurvePoints.uniform(0., width); + double x1 = rngCurvePoints.uniform(0., width); + double y0 = rngCurvePoints.uniform(0., height); + double y1 = rngCurvePoints.uniform(0., height); + vpColVector coeffs = computeABC(x0, y0, x1, y1); + std::vector curvePoints = generateSimulatedImage(0, width, 1., coeffs); + +#ifdef VISP_HAVE_DISPLAY + vpImage I(height, width); + std::shared_ptr pDisplay; + if (opt_display) { + pDisplay = vpDisplayFactory::createDisplay(I); + } +#endif + + const double ampliMaxInitNoise = 24.; + const double stdevInitNoise = ampliMaxInitNoise / 3.; + vpGaussRand rngInitNoise(stdevInitNoise, 0., seedInitPoints); + + for (unsigned int iter = 0; iter < nbTestRepet; ++iter) { + // Randomly select the initialization points + std::vector suffledVector = vpUniRand::shuffleVector(curvePoints, seedShuffle); + std::vector initPoints; + for (unsigned int j = 0; j < nbInitPoints; ++j) { + vpImagePoint noisyPt(suffledVector[j].get_i() + rngInitNoise(), suffledVector[j].get_j() + rngInitNoise()); + initPoints.push_back(noisyPt); + } + + // Compute the initial model + vpParabolaModel modelInitial = computeInitialGuess(initPoints, degree, height, width); + vpColVector X0 = modelInitial.toVpColVector(); + + // Initialize the Particle Filter + std::vector stdevsPF; + for (unsigned int i = 0; i < degree + 1; ++i) { + stdevsPF.push_back(ratioAmpliMax * X0[0] / 3.); + } + vpParticleFilter>::vpProcessFunction processFunc = fx; + vpLikelihoodFunctor likelihoodFtor(sigmaLikelihood, height, width); + using std::placeholders::_1; + using std::placeholders::_2; + vpParticleFilter>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2); + vpParticleFilter>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter>::simpleResamplingCheck; + vpParticleFilter>::vpResamplingFunction resamplingFunc = vpParticleFilter>::simpleImportanceResampling; + vpAverageFunctor averageCpter(degree, height, width); + using std::placeholders::_3; + vpParticleFilter>::vpFilterFunction meanFunc = std::bind(&vpAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3); + vpParticleFilter> filter(nbParticles, stdevsPF, seedPF, nbThreads); + filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc); + + for (unsigned int i = 0; i < nbWarmUpIter; ++i) { + filter.filter(curvePoints, dt); + } + + double meanError = 0.; + for (unsigned int i = 0; i < nbEvalIter; ++i) { + filter.filter(curvePoints, dt); + vpColVector Xest = filter.computeFilteredState(); + vpParabolaModel model(Xest, height, width); + double rmse = evaluate(curvePoints, model); + meanError += rmse; + +#ifdef VISP_HAVE_DISPLAY + if (opt_display) { + vpDisplay::display(I); + displayGeneratedImage(I, curvePoints, vpColor::red, "GT", 20, 20); + model.display(I, vpColor::blue, "Model", 40, 20); + vpDisplay::flush(I); + vpDisplay::getClick(I); + } +#endif + } + meanError /= static_cast(nbEvalIter); + std::cout << "Mean(rmse) = " << meanError << std::endl; + CHECK(meanError <= maxToleratedError); + } + } +} + +TEST_CASE("3rd-degree", "[vpParticleFilter][Polynomial interpolation]") +{ +/// ----- Simulation parameters ----- + const double width = 600.; //!< The width of the simulated image + const double height = 400.; //!< The height of the simulated image + const unsigned int degree = 3; //!< The degree of the polynomial in the simulated image + const unsigned int nbInitPoints = 10; //!< Number of points to compute the initial guess of the PF state + const uint64_t seedCurve = 4224; //!< The seed to generate the curve + const uint64_t seedInitPoints = 2112; //!< The seed to choose the init points + const unsigned int nbTestRepet = 10; //!< The number of times the test is repeated + const unsigned int nbWarmUpIter = 10; //!< Number of iterations for the warmup loop + const unsigned int nbEvalIter = 20; //!< Number of iterations for the evaluation loop + const double dt = 0.040; //!< Simulated period of acquisition + const int32_t seedShuffle = 4221; //!< The seed to shuffle the curve points + + /// ----- PF parameters ----- + // The maximum amplitude for the likelihood compute. + // A particle whose "distance" with the measurements is greater than this value has a likelihood of 0 + const double ampliMaxLikelihood = 6.; + const double sigmaLikelihood = ampliMaxLikelihood / 3.; //:< The corresponding standard deviation + const unsigned int nbParticles = 300; //!< Number of particles used by the particle filter + const double ratioAmpliMax(0.21); //!< Ratio of the initial guess values to use to add noise to the PF state + const long seedPF = 4221; //!< Seed of the particle filter + const int nbThreads = 1; // curvePoints = generateSimulatedImage(0, width, 1., coeffs); + +#ifdef VISP_HAVE_DISPLAY + vpImage I(height, width); + std::shared_ptr pDisplay; + if (opt_display) { + pDisplay = vpDisplayFactory::createDisplay(I); + } +#endif + + for (unsigned int iter = 0; iter < nbTestRepet; ++iter) { + // Randomly select the initialization points + std::vector suffledVector = vpUniRand::shuffleVector(curvePoints, seedShuffle); + std::vector initPoints; + for (unsigned int j = 0; j < nbInitPoints; ++j) { + initPoints.push_back(suffledVector[j]); + } + + // Compute the initial model + vpParabolaModel modelInitial = computeInitialGuess(initPoints, degree, height, width); + vpColVector X0 = modelInitial.toVpColVector(); + + // Initialize the Particle Filter + std::vector stdevsPF; + for (unsigned int i = 0; i < degree + 1; ++i) { + stdevsPF.push_back(ratioAmpliMax * std::pow(0.1, i) * X0[0] / 3.); + } + vpParticleFilter>::vpProcessFunction processFunc = fx; + vpLikelihoodFunctor likelihoodFtor(sigmaLikelihood, height, width); + using std::placeholders::_1; + using std::placeholders::_2; + vpParticleFilter>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2); + vpParticleFilter>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter>::simpleResamplingCheck; + vpParticleFilter>::vpResamplingFunction resamplingFunc = vpParticleFilter>::simpleImportanceResampling; + vpAverageFunctor averageCpter(degree, height, width); + using std::placeholders::_3; + vpParticleFilter>::vpFilterFunction meanFunc = std::bind(&vpAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3); + vpParticleFilter> filter(nbParticles, stdevsPF, seedPF, nbThreads); + filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc); + + for (unsigned int i = 0; i < nbWarmUpIter; ++i) { + filter.filter(curvePoints, dt); + } + + double meanError = 0.; + for (unsigned int i = 0; i < nbEvalIter; ++i) { + filter.filter(curvePoints, dt); + vpColVector Xest = filter.computeFilteredState(); + vpParabolaModel model(Xest, height, width); + double rmse = evaluate(curvePoints, model); + meanError += rmse; + +#ifdef VISP_HAVE_DISPLAY + if (opt_display) { + vpDisplay::display(I); + displayGeneratedImage(I, curvePoints, vpColor::red, "GT", 20, 20); + model.display(I, vpColor::blue, "Model", 40, 20); + vpDisplay::flush(I); + vpDisplay::getClick(I); + } +#endif + } + meanError /= static_cast(nbEvalIter); + std::cout << "Mean(rmse) = " << meanError << std::endl; + CHECK(meanError <= maxToleratedError); + } + } + + + + SECTION("Noisy", "Noise is added to the init points") + { + const double maxToleratedError = 17.; + double x0 = rngCurvePoints.uniform(0., width); + double x1 = rngCurvePoints.uniform(0., width); + double y0 = rngCurvePoints.uniform(0., height); + double y1 = rngCurvePoints.uniform(0., height); + vpColVector coeffs = computeABCD(x0, y0, x1, y1); + std::vector curvePoints = generateSimulatedImage(0, width, 1., coeffs); + +#ifdef VISP_HAVE_DISPLAY + vpImage I(height, width); + std::shared_ptr pDisplay; + if (opt_display) { + pDisplay = vpDisplayFactory::createDisplay(I); + } +#endif + + const double ampliMaxInitNoise = 1.5; + const double stdevInitNoise = ampliMaxInitNoise / 3.; + vpGaussRand rngInitNoise(stdevInitNoise, 0., seedInitPoints); + + for (unsigned int iter = 0; iter < nbTestRepet; ++iter) { + // Randomly select the initialization points + std::vector suffledVector = vpUniRand::shuffleVector(curvePoints, seedShuffle); + std::vector initPoints; + for (unsigned int j = 0; j < nbInitPoints * 4; ++j) { + vpImagePoint noisyPt(suffledVector[j].get_i() + rngInitNoise(), suffledVector[j].get_j() + rngInitNoise()); + initPoints.push_back(noisyPt); + } + + // Compute the initial model + vpParabolaModel modelInitial = computeInitialGuess(initPoints, degree, height, width); + vpColVector X0 = modelInitial.toVpColVector(); + + // Initialize the Particle Filter + std::vector stdevsPF; + for (unsigned int i = 0; i < degree + 1; ++i) { + stdevsPF.push_back(ratioAmpliMax * std::pow(.05, i) * X0[0] / 3.); + } + vpParticleFilter>::vpProcessFunction processFunc = fx; + vpLikelihoodFunctor likelihoodFtor(sigmaLikelihood * 2., height, width); + using std::placeholders::_1; + using std::placeholders::_2; + vpParticleFilter>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2); + vpParticleFilter>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter>::simpleResamplingCheck; + vpParticleFilter>::vpResamplingFunction resamplingFunc = vpParticleFilter>::simpleImportanceResampling; + vpAverageFunctor averageCpter(degree, height, width); + using std::placeholders::_3; + vpParticleFilter>::vpFilterFunction meanFunc = std::bind(&vpAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3); + vpParticleFilter> filter(nbParticles, stdevsPF, seedPF, nbThreads); + filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc); + + for (unsigned int i = 0; i < nbWarmUpIter * 5; ++i) { + filter.filter(curvePoints, dt); + } + + double meanError = 0.; + for (unsigned int i = 0; i < nbEvalIter; ++i) { + filter.filter(curvePoints, dt); + vpColVector Xest = filter.computeFilteredState(); + vpParabolaModel model(Xest, height, width); + double rmse = evaluate(curvePoints, model); + meanError += rmse; + +#ifdef VISP_HAVE_DISPLAY + if (opt_display) { + vpDisplay::display(I); + displayGeneratedImage(I, curvePoints, vpColor::red, "GT", 20, 20); + model.display(I, vpColor::blue, "Model", 40, 20); + vpDisplay::flush(I); + vpDisplay::getClick(I); + } +#endif + } + meanError /= static_cast(nbEvalIter); + std::cout << "Mean(rmse) = " << meanError << std::endl; + CHECK(meanError <= maxToleratedError); + } + } +} + +int main(int argc, char *argv[]) +{ + Catch::Session session; // There must be exactly one instance + + // Build a new parser on top of Catch's + using namespace Catch::clara; + auto cli = session.cli() // Get Catch's composite command line parser + | Opt(opt_display) // bind variable to a new option, with a hint string + ["--display"] // the option names it will respond to + ("Activate debug display"); // description string for the help output + + // Now pass the new composite back to Catch so it uses that + session.cli(cli); + + // Let Catch (using Clara) parse the command line + session.applyCommandLine(argc, argv); + + int numFailed = session.run(); + + // numFailed is clamped to 255 as some unices only use the lower 8 bits. + // This clamping has already been applied, so just return it here + // You can also do any post run clean-up here + return numFailed; +} + +#else +#include + +int main() { return EXIT_SUCCESS; } +#endif diff --git a/modules/gui/include/visp3/gui/vpDisplayFactory.h b/modules/gui/include/visp3/gui/vpDisplayFactory.h index 59585ed1c6..a43322fefa 100644 --- a/modules/gui/include/visp3/gui/vpDisplayFactory.h +++ b/modules/gui/include/visp3/gui/vpDisplayFactory.h @@ -60,7 +60,7 @@ namespace vpDisplayFactory * \return A newly allocated vpDisplay specialization * if a GUI library is available or nullptr otherwise. */ -vpDisplay *allocateDisplay() +inline vpDisplay *allocateDisplay() { #if defined(VISP_HAVE_DISPLAY) #ifdef VISP_HAVE_X11 @@ -134,7 +134,7 @@ vpDisplay *allocateDisplay(vpImage &I, const int winx = -1, const int winy = * \return A smart pointer pointing to a vpDisplay specialization * if a GUI library is available or nullptr otherwise. */ -std::shared_ptr createDisplay() +inline std::shared_ptr createDisplay() { #if defined(VISP_HAVE_DISPLAY) #ifdef VISP_HAVE_X11 @@ -213,10 +213,10 @@ struct GridSettings unsigned int paddingY; }; -void makeDisplayGridHelper(std::vector> &res, const GridSettings &settings, - unsigned int currRow, unsigned int currCol, - unsigned int currentPixelX, unsigned int currentPixelY, - unsigned int maxRowHeightPixel) +inline void makeDisplayGridHelper(std::vector> &res, const GridSettings &settings, + unsigned int currRow, unsigned int currCol, + unsigned int currentPixelX, unsigned int currentPixelY, + unsigned int maxRowHeightPixel) { if (currRow != (settings.rows - 1) && (currCol != settings.cols - 1)) { throw vpException(vpException::dimensionError, "Too few images for the grid size"); diff --git a/modules/imgproc/src/vpCircleHoughTransform_centers.cpp b/modules/imgproc/src/vpCircleHoughTransform_centers.cpp index 62a09d7c79..52b5b3e0f6 100644 --- a/modules/imgproc/src/vpCircleHoughTransform_centers.cpp +++ b/modules/imgproc/src/vpCircleHoughTransform_centers.cpp @@ -35,6 +35,7 @@ BEGIN_VISP_NAMESPACE +#ifndef DOXYGEN_SHOULD_SKIP_THIS namespace { /** @@ -204,7 +205,7 @@ updateAccumAlongGradientDir(const vpDataForAccumLoop &data, float &sx, float &sy float dy = (coord.y_orig - static_cast(coord.y)); accum[coord.y - data.offsetY][coord.x - data.offsetX] += std::abs(dx) + std::abs(dy); } - }; + }; #endif vpCoordinatesForAccumStep coords; coords.x_orig = x1; @@ -216,14 +217,15 @@ updateAccumAlongGradientDir(const vpDataForAccumLoop &data, float &sx, float &sy coords.x = x_high; coords.y = y_high; updateAccumulator(coords, data, centersAccum, hasToStopLoop); + } } } - } sx = -sx; sy = -sy; + } } } -} +#endif // Static variables @@ -525,8 +527,8 @@ vpCircleHoughTransform::filterCenterCandidates(const std::vector m_centerCandidatesList.push_back(merged_peaks_position_votes[i].m_position); m_centerVotes.push_back(static_cast(merged_peaks_position_votes[i].m_votes)); } - } - } + } +} vpCircleHoughTransform::vpCentersBarycenter vpCircleHoughTransform::mergeSimilarCenters(const unsigned int &idPeak, const unsigned int &nbPeaks, const float &squared_distance_max, const std::vector &peak_positions_votes, std::vector &has_been_merged) diff --git a/modules/imgproc/src/vpCircleHoughTransform_circles.cpp b/modules/imgproc/src/vpCircleHoughTransform_circles.cpp index d54782b89d..ee328335c6 100644 --- a/modules/imgproc/src/vpCircleHoughTransform_circles.cpp +++ b/modules/imgproc/src/vpCircleHoughTransform_circles.cpp @@ -35,6 +35,7 @@ BEGIN_VISP_NAMESPACE +#ifndef DOXYGEN_SHOULD_SKIP_THIS namespace { #if (VISP_CXX_STANDARD == VISP_CXX_STANDARD_98) @@ -128,6 +129,7 @@ updateRadiusAccumulator(const vpDataUpdateRadAccum &data, std::vector &ra } } } +#endif void vpCircleHoughTransform::computeCircleCandidates() diff --git a/tutorial/CMakeLists.txt b/tutorial/CMakeLists.txt index e80833ef2c..38d101ae34 100644 --- a/tutorial/CMakeLists.txt +++ b/tutorial/CMakeLists.txt @@ -49,6 +49,8 @@ visp_add_subdirectory(kalman REQUIRED_DEPS visp_co visp_add_subdirectory(mean-drift REQUIRED_DEPS visp_core visp_gui) visp_add_subdirectory(misc/npz REQUIRED_DEPS visp_core visp_gui visp_io) visp_add_subdirectory(munkres REQUIRED_DEPS visp_core visp_gui) +visp_add_subdirectory(particle-filter REQUIRED_DEPS visp_core visp_io visp_gui) +visp_add_subdirectory(particle-filter-curve-fitting REQUIRED_DEPS visp_core visp_io visp_gui) visp_add_subdirectory(robot/flir-ptu REQUIRED_DEPS visp_core visp_robot visp_sensor visp_vision visp_gui visp_vs visp_visual_features visp_detection) visp_add_subdirectory(robot/pioneer REQUIRED_DEPS visp_core visp_robot visp_vs visp_gui) visp_add_subdirectory(robot/mbot/raspberry/visp REQUIRED_DEPS visp_core visp_detection visp_io visp_gui visp_sensor visp_vs) diff --git a/tutorial/image/tutorial-canny.cpp b/tutorial/image/tutorial-canny.cpp index 586fdf3b59..2ea2f73fac 100644 --- a/tutorial/image/tutorial-canny.cpp +++ b/tutorial/image/tutorial-canny.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ //! \example tutorial-canny.cpp diff --git a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageConversion.h b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageConversion.h index 7c3abf2c58..323fd65e0c 100644 --- a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageConversion.h +++ b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageConversion.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * This file is part of the ViSP software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #import #import @@ -41,12 +39,11 @@ @interface ImageConversion : NSObject -+ (vpImage)vpImageColorFromUIImage:(UIImage *)image; -+ (vpImage)vpImageGrayFromUIImage:(UIImage *)image; -+ (UIImage *)UIImageFromVpImageColor:(const vpImage &)I; -+ (UIImage *)UIImageFromVpImageGray:(const vpImage &)I; ++ (vpImage)vpImageColorFromUIImage : (UIImage *)image; ++(vpImage)vpImageGrayFromUIImage:(UIImage *)image; ++(UIImage *)UIImageFromVpImageColor:(const vpImage &)I; ++(UIImage *)UIImageFromVpImageGray:(const vpImage &)I; @end #endif - diff --git a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageConversion.mm b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageConversion.mm index 3eab39980f..8d55fdce28 100644 --- a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageConversion.mm +++ b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageConversion.mm @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * This file is part of the ViSP software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #ifndef DOXYGEN_SHOULD_SKIP_THIS diff --git a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplay.h b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplay.h index 30cc1f2328..de0c09a37b 100644 --- a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplay.h +++ b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplay.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * This file is part of the ViSP software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #import #ifdef __cplusplus @@ -41,11 +39,10 @@ @interface ImageDisplay : NSObject -+ (UIImage *)displayLine:(UIImage *)image :(vpImagePoint &)ip1 :(vpImagePoint &)ip2 :(UIColor*)color :(int)tickness; -+ (UIImage *)displayFrame:(UIImage *)image :(const vpHomogeneousMatrix &)cMo :(const vpCameraParameters &)cam - :(double) size :(int)tickness; ++ (UIImage *)displayLine : (UIImage *)image : (vpImagePoint &)ip1 : (vpImagePoint &)ip2 : (UIColor *)color : (int)tickness; ++(UIImage *)displayFrame:(UIImage *)image : (const vpHomogeneousMatrix &)cMo : (const vpCameraParameters &)cam + : (double)size : (int)tickness; @end #endif - diff --git a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplay.mm b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplay.mm index 1b3a369ea4..f94cc3b5be 100644 --- a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplay.mm +++ b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplay.mm @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #ifndef DOXYGEN_SHOULD_SKIP_THIS @@ -137,4 +135,3 @@ + (UIImage *)displayFrame:(UIImage *)image :(const vpHomogeneousMatrix &)cMo :(c @end #endif - diff --git a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplayWithContext.h b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplayWithContext.h index 1b41a85db4..361847e95d 100644 --- a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplayWithContext.h +++ b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplayWithContext.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * This file is part of the ViSP software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #import #ifdef __cplusplus @@ -39,13 +37,13 @@ NS_ASSUME_NONnullptr_BEGIN -@interface ImageDisplay (withContext) +@interface ImageDisplay(withContext) -+ (void)displayLineWithContext:(CGContextRef)context :(std::vector)polygon :(UIColor*)color :(int)tickness; ++ (void)displayLineWithContext :(CGContextRef)context : (std::vector)polygon : (UIColor *)color : (int)tickness; -+ (void)displayFrameWithContext:(CGContextRef)context :(const vpHomogeneousMatrix &)cMo :(const vpCameraParameters &)cam :(double) size :(int)tickness; ++(void)displayFrameWithContext:(CGContextRef)context : (const vpHomogeneousMatrix &)cMo : (const vpCameraParameters &)cam : (double)size : (int)tickness; -+ (void)displayText:(NSString*)text :(double)x :(double)y :(int)width :(int)height :(UIColor*)color :(UIColor*)bgColor; ++(void)displayText:(NSString *)text : (double)x : (double)y : (int)width : (int)height : (UIColor *)color : (UIColor *)bgColor; @end diff --git a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplayWithContext.mm b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplayWithContext.mm index 9c1dc3b57a..151955c4b8 100644 --- a/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplayWithContext.mm +++ b/tutorial/ios/AprilTagLiveCamera/VispHelper/ImageDisplayWithContext.mm @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * This file is part of the ViSP software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #import "ImageDisplayWithContext.h" diff --git a/tutorial/ios/GettingStarted/GettingStarted/ViewController.mm b/tutorial/ios/GettingStarted/GettingStarted/ViewController.mm index 1a390eae40..c59cd350c4 100644 --- a/tutorial/ios/GettingStarted/GettingStarted/ViewController.mm +++ b/tutorial/ios/GettingStarted/GettingStarted/ViewController.mm @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #ifndef DOXYGEN_SHOULD_SKIP_THIS @@ -113,4 +111,3 @@ - (void)didReceiveMemoryWarning { @end #endif - diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageConversion.h b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageConversion.h index 9767439702..7514db6c0d 100644 --- a/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageConversion.h +++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageConversion.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #import #import @@ -41,12 +39,11 @@ @interface ImageConversion : NSObject -+ (vpImage)vpImageColorFromUIImage:(UIImage *)image; -+ (vpImage)vpImageGrayFromUIImage:(UIImage *)image; -+ (UIImage *)UIImageFromVpImageColor:(const vpImage &)I; -+ (UIImage *)UIImageFromVpImageGray:(const vpImage &)I; ++ (vpImage)vpImageColorFromUIImage : (UIImage *)image; ++(vpImage)vpImageGrayFromUIImage:(UIImage *)image; ++(UIImage *)UIImageFromVpImageColor:(const vpImage &)I; ++(UIImage *)UIImageFromVpImageGray:(const vpImage &)I; @end #endif - diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageConversion.mm b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageConversion.mm index 156710a531..431348b513 100644 --- a/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageConversion.mm +++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageConversion.mm @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #ifndef DOXYGEN_SHOULD_SKIP_THIS diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageDisplay.h b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageDisplay.h index b552c513ec..657250a336 100644 --- a/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageDisplay.h +++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageDisplay.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #import #ifdef __cplusplus @@ -39,11 +37,10 @@ @interface ImageDisplay : NSObject -+ (UIImage *)displayLine:(UIImage *)image :(vpImagePoint &)ip1 :(vpImagePoint &)ip2 :(UIColor*)color :(int)tickness; -+ (UIImage *)displayFrame:(UIImage *)image :(const vpHomogeneousMatrix &)cMo :(const vpCameraParameters &)cam - :(double) size :(int)tickness; ++ (UIImage *)displayLine : (UIImage *)image : (vpImagePoint &)ip1 : (vpImagePoint &)ip2 : (UIColor *)color : (int)tickness; ++(UIImage *)displayFrame:(UIImage *)image : (const vpHomogeneousMatrix &)cMo : (const vpCameraParameters &)cam + : (double)size : (int)tickness; @end #endif - diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageDisplay.mm b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageDisplay.mm index 1b3a369ea4..f94cc3b5be 100644 --- a/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageDisplay.mm +++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageDisplay.mm @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #ifndef DOXYGEN_SHOULD_SKIP_THIS @@ -137,4 +135,3 @@ + (UIImage *)displayFrame:(UIImage *)image :(const vpHomogeneousMatrix &)cMo :(c @end #endif - diff --git a/tutorial/ios/StartedImageProc/StartedImageProc/ImageConversion.h b/tutorial/ios/StartedImageProc/StartedImageProc/ImageConversion.h index 9767439702..7514db6c0d 100644 --- a/tutorial/ios/StartedImageProc/StartedImageProc/ImageConversion.h +++ b/tutorial/ios/StartedImageProc/StartedImageProc/ImageConversion.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #import #import @@ -41,12 +39,11 @@ @interface ImageConversion : NSObject -+ (vpImage)vpImageColorFromUIImage:(UIImage *)image; -+ (vpImage)vpImageGrayFromUIImage:(UIImage *)image; -+ (UIImage *)UIImageFromVpImageColor:(const vpImage &)I; -+ (UIImage *)UIImageFromVpImageGray:(const vpImage &)I; ++ (vpImage)vpImageColorFromUIImage : (UIImage *)image; ++(vpImage)vpImageGrayFromUIImage:(UIImage *)image; ++(UIImage *)UIImageFromVpImageColor:(const vpImage &)I; ++(UIImage *)UIImageFromVpImageGray:(const vpImage &)I; @end #endif - diff --git a/tutorial/ios/StartedImageProc/StartedImageProc/ImageConversion.mm b/tutorial/ios/StartedImageProc/StartedImageProc/ImageConversion.mm index 156710a531..431348b513 100644 --- a/tutorial/ios/StartedImageProc/StartedImageProc/ImageConversion.mm +++ b/tutorial/ios/StartedImageProc/StartedImageProc/ImageConversion.mm @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #ifndef DOXYGEN_SHOULD_SKIP_THIS diff --git a/tutorial/ios/StartedImageProc/StartedImageProc/ViewController.mm b/tutorial/ios/StartedImageProc/StartedImageProc/ViewController.mm index 07b7544acf..39d850872d 100644 --- a/tutorial/ios/StartedImageProc/StartedImageProc/ViewController.mm +++ b/tutorial/ios/StartedImageProc/StartedImageProc/ViewController.mm @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #ifndef DOXYGEN_SHOULD_SKIP_THIS diff --git a/tutorial/kalman/tutorial-ukf.cpp b/tutorial/kalman/tutorial-ukf.cpp index 8bcfa470f6..318c53fd83 100644 --- a/tutorial/kalman/tutorial-ukf.cpp +++ b/tutorial/kalman/tutorial-ukf.cpp @@ -39,11 +39,7 @@ //! [Display_includes] #ifdef VISP_HAVE_DISPLAY #include -#include -#include -#include -#include -#include +#include #endif //! [Display_includes] #include @@ -263,10 +259,12 @@ int main(/*const int argc, const char *argv[]*/) 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 - const std::vector 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.}) }; // Vector of the markers sticked on the object + + // Vector of the markers sticked on the object + const std::vector 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(markers.size()); std::vector markersAsVpPoint; for (unsigned int i = 0; i < nbMarkers; ++i) { @@ -370,22 +368,8 @@ int main(/*const int argc, const char *argv[]*/) // Depending on the detected third party libraries, we instantiate here the // first video device which is available #ifdef VISP_HAVE_DISPLAY - vpDisplay *d = nullptr; -#if defined(VISP_HAVE_X11) - d = new vpDisplayX; -#elif defined(VISP_HAVE_GTK) - d = new vpDisplayGTK; -#elif defined(VISP_HAVE_GDI) - d = new vpDisplayGDI; -#elif defined(VISP_HAVE_D3D9) - d = new vpDisplayD3D; -#elif defined(HAVE_OPENCV_HIGHGUI) - d = new vpDisplayOpenCV; -#endif - vpImage Idisp(800, 800, vpRGBa(255)); - if (d != nullptr) { - d->init(Idisp, 800, 50, "Projection of the markers"); - } + vpImage Idisp(700, 700, vpRGBa(255)); + std::shared_ptr d = vpDisplayFactory::createDisplay(Idisp, 800, -1, "Projection of the markers"); #endif //! [Init_renderer] @@ -475,14 +459,6 @@ int main(/*const int argc, const char *argv[]*/) std::cout << "Press Enter to quit..." << std::endl; std::cin.get(); - //! [Delete_renderer] - // Delete the renderer if it was allocated -#ifdef VISP_HAVE_DISPLAY - if (d != nullptr) { - delete d; - } -#endif -//! [Delete_renderer] return 0; } #else diff --git a/tutorial/mean-drift/tutorial-meandrift.cpp b/tutorial/mean-drift/tutorial-meandrift.cpp index 5d9758ae76..9e457c151f 100644 --- a/tutorial/mean-drift/tutorial-meandrift.cpp +++ b/tutorial/mean-drift/tutorial-meandrift.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ //! \example tutorial-meandrift.cpp diff --git a/tutorial/misc/npz/tutorial-npz.cpp b/tutorial/misc/npz/tutorial-npz.cpp index 29957ee770..a8415cfa4d 100644 --- a/tutorial/misc/npz/tutorial-npz.cpp +++ b/tutorial/misc/npz/tutorial-npz.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ //! \example tutorial-npz.cpp diff --git a/tutorial/particle-filter-curve-fitting/CMakeLists.txt b/tutorial/particle-filter-curve-fitting/CMakeLists.txt new file mode 100644 index 0000000000..f078da7cbb --- /dev/null +++ b/tutorial/particle-filter-curve-fitting/CMakeLists.txt @@ -0,0 +1,54 @@ +############################################################################# +# +# 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. +# +# Description: +# ViSP overall configuration file. +# +############################################################################# + +cmake_minimum_required(VERSION 3.5) + +project(tutorial-pf) + +find_package(VISP REQUIRED visp_core visp_gui) + +set(tutorial_cpp) + +list(APPEND tutorial_cpp tutorial-pf-curve-fitting-lms.cpp tutorial-pf-curve-fitting-pf.cpp tutorial-pf-curve-fitting-all.cpp) + +foreach(cpp ${tutorial_cpp}) + visp_add_target(${cpp} vpTutoCommonData.h vpTutoMeanSquareFitting.h vpTutoMeanSquareFitting.cpp vpTutoParabolaModel.h vpTutoSegmentation.h vpTutoSegmentation.cpp) + if(COMMAND visp_add_dependency) + visp_add_dependency(${cpp} "tutorials") + endif() +endforeach() + +visp_copy_dir(tutorial-pf-curve-fitting-all "${CMAKE_CURRENT_SOURCE_DIR}" data) +visp_copy_dir(tutorial-pf-curve-fitting-all "${CMAKE_CURRENT_SOURCE_DIR}" calib) diff --git a/tutorial/particle-filter-curve-fitting/calib/hsv-thresholds.yml b/tutorial/particle-filter-curve-fitting/calib/hsv-thresholds.yml new file mode 100644 index 0000000000..163ef42143 --- /dev/null +++ b/tutorial/particle-filter-curve-fitting/calib/hsv-thresholds.yml @@ -0,0 +1,10 @@ +# File created 2024/07/25 09:49:49 +rows: 6 +cols: 1 +data: + - [29] + - [89] + - [115] + - [255] + - [96] + - [255] diff --git a/tutorial/particle-filter-curve-fitting/data/camera.xml b/tutorial/particle-filter-curve-fitting/data/camera.xml new file mode 100644 index 0000000000..c9d3e1966f --- /dev/null +++ b/tutorial/particle-filter-curve-fitting/data/camera.xml @@ -0,0 +1,74 @@ + + + + + + color_camera + + 640 + 480 + + + + perspectiveProjWithDistortion + + 608.26318359375 + 608.282470703125 + + 324.86920166015625 + 246.94593811035156 + + -0 + + 0 + + + + + depth_camera + + 640 + 480 + + + + perspectiveProjWithDistortion + + 608.26318359375 + 608.282470703125 + + 324.86920166015625 + 246.94593811035156 + + -0 + + 0 + + + + + infrared_camera + + 640 + 480 + + + + perspectiveProjWithDistortion + + 380.10546875 + 380.10546875 + + 325.81158447265625 + 233.01167297363281 + + -0 + + 0 + + + diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0000.png b/tutorial/particle-filter-curve-fitting/data/color_image_0000.png new file mode 100644 index 0000000000..236efa670a Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0000.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0001.png b/tutorial/particle-filter-curve-fitting/data/color_image_0001.png new file mode 100644 index 0000000000..74c984cff2 Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0001.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0002.png b/tutorial/particle-filter-curve-fitting/data/color_image_0002.png new file mode 100644 index 0000000000..bb4477d565 Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0002.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0003.png b/tutorial/particle-filter-curve-fitting/data/color_image_0003.png new file mode 100644 index 0000000000..94f653f65f Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0003.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0004.png b/tutorial/particle-filter-curve-fitting/data/color_image_0004.png new file mode 100644 index 0000000000..91446e16f5 Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0004.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0005.png b/tutorial/particle-filter-curve-fitting/data/color_image_0005.png new file mode 100644 index 0000000000..872b66591d Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0005.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0006.png b/tutorial/particle-filter-curve-fitting/data/color_image_0006.png new file mode 100644 index 0000000000..737d755047 Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0006.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0007.png b/tutorial/particle-filter-curve-fitting/data/color_image_0007.png new file mode 100644 index 0000000000..3c63ee6f42 Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0007.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0008.png b/tutorial/particle-filter-curve-fitting/data/color_image_0008.png new file mode 100644 index 0000000000..a506efb3bb Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0008.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0009.png b/tutorial/particle-filter-curve-fitting/data/color_image_0009.png new file mode 100644 index 0000000000..4057fa1f28 Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0009.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0010.png b/tutorial/particle-filter-curve-fitting/data/color_image_0010.png new file mode 100644 index 0000000000..4c71b1278a Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0010.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0011.png b/tutorial/particle-filter-curve-fitting/data/color_image_0011.png new file mode 100644 index 0000000000..9dd1046a7a Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0011.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0012.png b/tutorial/particle-filter-curve-fitting/data/color_image_0012.png new file mode 100644 index 0000000000..bda1583900 Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0012.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0013.png b/tutorial/particle-filter-curve-fitting/data/color_image_0013.png new file mode 100644 index 0000000000..d45e16b938 Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0013.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0014.png b/tutorial/particle-filter-curve-fitting/data/color_image_0014.png new file mode 100644 index 0000000000..30d00b573a Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0014.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0015.png b/tutorial/particle-filter-curve-fitting/data/color_image_0015.png new file mode 100644 index 0000000000..b2a2703180 Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0015.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0016.png b/tutorial/particle-filter-curve-fitting/data/color_image_0016.png new file mode 100644 index 0000000000..aab3951143 Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0016.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0017.png b/tutorial/particle-filter-curve-fitting/data/color_image_0017.png new file mode 100644 index 0000000000..e603d735d5 Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0017.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0018.png b/tutorial/particle-filter-curve-fitting/data/color_image_0018.png new file mode 100644 index 0000000000..5248b7a7df Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0018.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0019.png b/tutorial/particle-filter-curve-fitting/data/color_image_0019.png new file mode 100644 index 0000000000..98c17e4c24 Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0019.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0020.png b/tutorial/particle-filter-curve-fitting/data/color_image_0020.png new file mode 100644 index 0000000000..2ea704d3bc Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0020.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0021.png b/tutorial/particle-filter-curve-fitting/data/color_image_0021.png new file mode 100644 index 0000000000..6547614853 Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0021.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0022.png b/tutorial/particle-filter-curve-fitting/data/color_image_0022.png new file mode 100644 index 0000000000..ee9c62811e Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0022.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0023.png b/tutorial/particle-filter-curve-fitting/data/color_image_0023.png new file mode 100644 index 0000000000..9326496730 Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0023.png differ diff --git a/tutorial/particle-filter-curve-fitting/data/color_image_0024.png b/tutorial/particle-filter-curve-fitting/data/color_image_0024.png new file mode 100644 index 0000000000..8341c046ae Binary files /dev/null and b/tutorial/particle-filter-curve-fitting/data/color_image_0024.png differ diff --git a/tutorial/particle-filter-curve-fitting/tutorial-pf-curve-fitting-all.cpp b/tutorial/particle-filter-curve-fitting/tutorial-pf-curve-fitting-all.cpp new file mode 100644 index 0000000000..9528d18de5 --- /dev/null +++ b/tutorial/particle-filter-curve-fitting/tutorial-pf-curve-fitting-all.cpp @@ -0,0 +1,672 @@ +/* + * 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. + */ + +//! \example tutorial-pf-curve-fitting-all.cpp + +// System includes +#include +#include + +// ViSP includes +#include +#include +#include +#include + +#ifdef VISP_HAVE_DISPLAY +#include +#endif + +//! [Include_PF] +#include +//! [Include_PF] + +#include "vpTutoCommonData.h" +#include "vpTutoMeanSquareFitting.h" +#include "vpTutoParabolaModel.h" +#include "vpTutoSegmentation.h" + +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY) +#ifndef DOXYGEN_SHOULD_SKIP_THIS +namespace tutorial +{ +//! [Evaluation_functions] +/** + * \brief Compute the square error between the parabola model and + * the input point \b pt. + * + * \param[in] pt The input point. + * \return double The square error. + */ +double evaluate(const vpImagePoint &pt, const vpTutoParabolaModel &model) +{ + double u = pt.get_u(); + double v = pt.get_v(); + double v_model = model.eval(u); + double error = v - v_model; + double squareError = error * error; + return squareError; +} + +/** + * \brief Compute the mean-square error between the parabola model and + * the input points \b pts. + * + * \param[in] coeffs The coefficients of the polynomial. + * \param[in] height The height of the input image. + * \param[in] width The width of the input image. + * \param[in] pts The input points. + * \return double The root mean square error. + */ +double evaluate(const vpColVector &coeffs, const unsigned int &height, const unsigned int &width, const std::vector &pts) +{ + unsigned int nbPts = pts.size(); + vpColVector residuals(nbPts); + vpColVector weights(nbPts, 1.); + vpTutoParabolaModel model(coeffs, height, width); + // Compute the residuals + for (unsigned int i = 0; i < nbPts; ++i) { + double squareError = evaluate(pts[i], model); + residuals[i] = squareError; + } + double meanSquareError = residuals.sum() / static_cast(nbPts); + return std::sqrt(meanSquareError); +} +//! [Evaluation_functions] + +//! [Display_function] +/** + * \brief Display the fitted parabola on the image. + * + * \tparam T Either unsigned char or vpRGBa. + * \param[in] coeffs The coefficients of the parabola, such as coeffs[0] = a coeffs[1] = b coeffs[2] = c + * \param[in] I The image on which we want to display the parabola model. + * \param[in] color The color we want to use to display the parabola. + */ +template +void display(const vpColVector &coeffs, const vpImage &I, const vpColor &color, + const unsigned int &vertPosLegend, const unsigned int &horPosLegend) +{ +#if defined(VISP_HAVE_DISPLAY) + unsigned int width = I.getWidth(); + vpTutoParabolaModel model(coeffs, I.getHeight(), I.getWidth()); + for (unsigned int u = 0; u < width; ++u) { + float v = model.eval(u); + vpDisplay::displayPoint(I, v, u, color, 1); + vpDisplay::displayText(I, vertPosLegend, horPosLegend, "Particle Filter model", color); + } +#else + (void)coeffs; + (void)I; + (void)color; + (void)vertPosLegend; + (void)horPosLegend; +#endif +} +//! [Display_function] + +//! [Initialization_function] +/** + * \brief Select automatically the init points from the segmented image. + * + * \param[in] data The data common to the whole program. + * \return std::vector The vector of image points to use to initialize + * the Particle Filter using a Least Mean Square minimization. + */ +std::vector automaticInitialization(tutorial::vpTutoCommonData &data) +{ + // Initialization-related variables + const unsigned int minNbPts = data.m_degree + 1; + const unsigned int nbPtsToUse = 10 * minNbPts; + std::vector initPoints; + + // Perform HSV segmentation + tutorial::performSegmentationHSV(data); + + // Extracting the skeleton of the mask + std::vector edgePoints = tutorial::extractSkeleton(data); + unsigned int nbEdgePoints = edgePoints.size(); + + if (nbEdgePoints < nbPtsToUse) { + return edgePoints; + } + + // Uniformly extract init points + auto ptHasLowerU = [](const vpImagePoint &ptA, const vpImagePoint &ptB) { + return ptA.get_u() < ptB.get_u(); + }; + std::sort(edgePoints.begin(), edgePoints.end(), ptHasLowerU); + + unsigned int idStart, idStop; + if (nbEdgePoints > nbPtsToUse + 20) { + // Avoid extreme points in case it's noise + idStart = 10; + idStop = edgePoints.size() - 10; + } + else { + // We need to take all the points because we don't have enough + idStart = 0; + idStop = edgePoints.size(); + } + + // Sample uniformly the points starting from the left of the image to the right + unsigned int sizeWindow = idStop - idStart + 1; + unsigned int step = sizeWindow / (nbPtsToUse - 1); + for (unsigned int id = idStart; id <= idStop; id += step) { + initPoints.push_back(edgePoints[id]); + } + return initPoints; +} + +/** + * \brief Get the init points by user-interaction. + * + * \param[in] data The data common to the whole program. + * \return std::vector The vector that contains the init points. + */ +std::vector manualInitialization(const tutorial::vpTutoCommonData &data) +{ + // Interaction variables + const bool waitForClick = true; + vpImagePoint ipClick; + vpMouseButton::vpMouseButtonType button; + + // Display variables + const unsigned int sizeCross = 10; + const unsigned int thicknessCross = 2; + const vpColor colorCross = vpColor::red; + + // Initialization-related variables + const unsigned int minNbPts = data.m_degree + 1; + std::vector initPoints; + + bool notEnoughPoints = true; + while (notEnoughPoints) { + // Initial display of the images + vpDisplay::display(data.m_I_orig); + + // Display the how-to + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "Left click to add init point (min.: " + std::to_string(minNbPts) + "), right click to estimate the initial coefficients of the Particle Filter.", data.m_colorLegend); + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend + data.m_legendOffset, "A middle click reinitialize the list of init points.", data.m_colorLegend); + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend + data.m_legendOffset + data.m_legendOffset, "If not enough points have been selected, a right click has no effect.", data.m_colorLegend); + + // Display the already selected points + unsigned int nbInitPoints = initPoints.size(); + for (unsigned int i = 0; i < nbInitPoints; ++i) { + vpDisplay::displayCross(data.m_I_orig, initPoints[i], sizeCross, colorCross, thicknessCross); + } + + // Update the display + vpDisplay::flush(data.m_I_orig); + + // Get the user input + vpDisplay::getClick(data.m_I_orig, ipClick, button, waitForClick); + + // Either add the clicked point to the list of initial points or stop the loop if enough points are available + switch (button) { + case vpMouseButton::vpMouseButtonType::button1: + initPoints.push_back(ipClick); + break; + case vpMouseButton::vpMouseButtonType::button2: + initPoints.clear(); + break; + case vpMouseButton::vpMouseButtonType::button3: + (initPoints.size() >= minNbPts ? notEnoughPoints = false : notEnoughPoints = true); + break; + default: + break; + } + } + + return initPoints; +} + +/** + * \brief Compute the initial guess of the state for the Particle Filter + * using Least-Mean-Square minimization. + * + * \param[in] data The data used in the tutorial. + * \return vpColVector The vector containing the coefficients, used as initial guess, + * of the parabola. + */ +vpColVector computeInitialGuess(tutorial::vpTutoCommonData &data) +{ + // Vector that contains the init points + std::vector initPoints; + +#ifdef VISP_HAVE_DISPLAY + // Interaction variables + const bool waitForClick = true; + vpImagePoint ipClick; + vpMouseButton::vpMouseButtonType button; + + // Display variables + const unsigned int sizeCross = 10; + const unsigned int thicknessCross = 2; + const vpColor colorCross = vpColor::red; + + bool automaticInit = false; + + // Initial display of the images + vpDisplay::display(data.m_I_orig); + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "Left click to manually select the init points, right click to automatically initialize the PF", data.m_colorLegend); + + // Update the display + vpDisplay::flush(data.m_I_orig); + + // Get the user input + vpDisplay::getClick(data.m_I_orig, ipClick, button, waitForClick); + + // Either use the automatic initialization or the manual one depending on the user input + switch (button) { + case vpMouseButton::vpMouseButtonType::button1: + automaticInit = false; + break; + case vpMouseButton::vpMouseButtonType::button3: + automaticInit = true; + break; + default: + break; + } + + if (automaticInit) { + // Get automatically the init points from the segmented image + initPoints = tutorial::automaticInitialization(data); + } + else { + // Get manually the init points from the original image + initPoints = tutorial::manualInitialization(data); + } + +#else + // Get the init points from the segmented image + initPoints = tutorial::automaticInitialization(data); +#endif + + // Compute the coefficients of the parabola using Least-Mean-Square minimization. + tutorial::vpTutoMeanSquareFitting lmsFitter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth()); + lmsFitter.fit(initPoints); + vpColVector X0 = lmsFitter.getCoeffs(); + std::cout << "---[Initial fit]---" << std::endl; + std::cout << lmsFitter.getModel(); + std::cout << "---[Initial fit]---" << std::endl; + + // Display info about the initialization + vpDisplay::display(data.m_I_orig); + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "Here are the points selected for the initialization.", data.m_colorLegend); + unsigned int nbInitPoints = initPoints.size(); + for (unsigned int i = 0; i < nbInitPoints; ++i) { + const vpImagePoint &ip = initPoints[i]; + vpDisplay::displayCross(data.m_I_orig, ip, sizeCross, colorCross, thicknessCross); + } + + // Update display and wait for click + lmsFitter.display(data.m_I_orig, vpColor::red, data.m_ipLegend.get_v() + 2 * data.m_legendOffset.get_v(), data.m_ipLegend.get_u()); + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend + data.m_legendOffset, "A click to continue.", data.m_colorLegend); + vpDisplay::flush(data.m_I_orig); + vpDisplay::getClick(data.m_I_orig, waitForClick); + + return X0; +} +//! [Initialization_function] + +//! [Process_function] +vpColVector fx(const vpColVector &coeffs, const double &/*dt*/) +{ + vpColVector updatedCoeffs = coeffs; // We use a constant position model + return updatedCoeffs; +} +//! [Process_function] + +//! [Average_functor] +class vpTutoAverageFunctor +{ +public: + vpTutoAverageFunctor(const unsigned int °ree, const unsigned int &height, const unsigned int &width) + : m_degree(degree) + , m_height(height) + , m_width(width) + { } + + /** + * \brief Compute the "weighted average" of polynomial models, by sampling control points and + * then performing Least-Mean Square minimization to best fit the control points. + * + * \param[in] particles The vector containing the particles of the PF. + * \param[in] weights Their associated weights + * + * \return vpColVector The coefficients of the polynomial model that best fits the control points. + */ + vpColVector averagePolynomials(const std::vector &particles, const std::vector &weights, const vpParticleFilter>::vpStateAddFunction &/**/) + { + const unsigned int nbParticles = particles.size(); + const double nbParticlesAsDOuble = static_cast(nbParticles); + // Compute the sum of the weights to be able to determine the "importance" of a particle with regard to the whole set + const double sumWeight = std::accumulate(weights.begin(), weights.end(), 0.); + + // Defining the total number of control points we want to generate + const double nbPointsForAverage = 10. * nbParticlesAsDOuble; + std::vector initPoints; + + // Creating control points by each particle + for (unsigned int i = 0; i < nbParticles; ++i) { + // The number of control points a particle can generate is proportional to the ratio of its weight w.r.t. the sum of the weights + double nbPoints = std::floor(weights[i] * nbPointsForAverage / sumWeight); + if (nbPoints > 1.) { + // The particle has a weight high enough to deserve more than one points + vpTutoParabolaModel curve(particles[i], m_height, m_width); + double widthAsDouble = static_cast(m_width); + // Uniform sampling of the control points along the polynomial model + double step = widthAsDouble / (nbPoints - 1.); + for (double u = 0.; u < widthAsDouble; u += step) { + double v = curve.eval(u); + vpImagePoint pt(v, u); + initPoints.push_back(pt); + } + } + else if (nbPoints == 1.) { + // The weight of the particle make it have only one control point + // We sample it at the middle of the image + vpTutoParabolaModel curve(particles[i], m_height, m_width); + double u = static_cast(m_width) / 2.; + double v = curve.eval(u); + vpImagePoint pt(v, u); + initPoints.push_back(pt); + } + } + // We use Least-Mean Square minimization to compute the polynomial model that best fits all the control points + vpTutoMeanSquareFitting lms(m_degree, m_height, m_width); + lms.fit(initPoints); + return lms.getCoeffs(); + } + +private: + unsigned int m_degree; //!< The degree of the polynomial. + unsigned int m_height; //!< The height of the input image. + unsigned int m_width; //!< The width of the input image. +}; +//! [Average_functor] + +//! [Likelihood_functor] +class vpTutoLikelihoodFunctor +{ +public: + /** + * @brief Construct a new vp Likelihood Functor object + * + * \param[in] stdev The standard deviation of the likelihood function. + * \param[in] height The height of the input image. + * \param[in] width The width of the input image. + */ + vpTutoLikelihoodFunctor(const double &stdev, const unsigned int &height, const unsigned int &width) + : m_height(height) + , m_width(width) + { + double sigmaDistanceSquared = stdev * stdev; + m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared); + m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared); + } + + //! [Likelihood_function] + /** + * \brief Compute the likelihood of a particle compared to the measurements. + * The likelihood equals zero if the particle is completely different of + * the measurements and equals one if it matches completely. + * The chosen likelihood is 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. + * + * \param[in] coeffs The particle, which represent the parabola coefficients. + * \param[in] meas The measurement vector. + * \return double The likelihood of the particle. + */ + double likelihood(const vpColVector &coeffs, const std::vector &meas) + { + double likelihood = 0.; + unsigned int nbPoints = meas.size(); + + // Generate a model from the coefficients stored in the particle state + vpTutoParabolaModel model(coeffs, m_height, m_width); + + // Compute the residual between each measurement point and its equivalent in the model + vpColVector residuals(nbPoints); + for (unsigned int i = 0; i < nbPoints; ++i) { + double squareError = tutorial::evaluate(meas[i], model); + residuals[i] = squareError; + } + + // Use Tukey M-estimator to be robust against outliers + vpRobust Mestimator; + vpColVector w(nbPoints, 1.); + Mestimator.MEstimator(vpRobust::TUKEY, residuals, w); + double sumError = w.hadamard(residuals).sum(); + + // Compute the likelihood as a Gaussian function + likelihood = std::exp(m_constantExpDenominator * sumError / w.sum()) * 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; + } + //! [Likelihood_function] +private: + 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. + unsigned int m_height; //!< The height of the input image. + unsigned int m_width; //!< The width of the input image. +}; +//! [Likelihood_functor] +} +#endif + +int main(const int argc, const char *argv[]) +{ + tutorial::vpTutoCommonData data; + int returnCode = data.init(argc, argv); + if (returnCode != tutorial::vpTutoCommonData::SOFTWARE_CONTINUE) { + return returnCode; + } + tutorial::vpTutoMeanSquareFitting lmsFitter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth()); + const unsigned int vertOffset = data.m_legendOffset.get_i(); + const unsigned int horOffset = data.m_ipLegend.get_j(); + const unsigned int legendLmsVert = data.m_I_orig.getHeight() - 3 * vertOffset; + const unsigned int legendLmsHor = horOffset; + const unsigned int legendPFVert = data.m_I_orig.getHeight() - 2 * vertOffset, legendPFHor = horOffset; + + // Initialize the attributes of the PF + //! [Initial_estimates] + vpColVector X0 = tutorial::computeInitialGuess(data); + //! [Initial_estimates] + + //! [Constants_for_the_PF] + const double maxDistanceForLikelihood = data.m_pfMaxDistanceForLikelihood; // 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 = data.m_pfN; // Number of particles to use + std::vector stdevsPF; // Standard deviation for each state component + for (unsigned int i = 0; i < data.m_degree + 1; ++i) { + double ampliMax = data.m_pfRatiosAmpliMax[i] * X0[i]; + stdevsPF.push_back(ampliMax / 3.); + } + unsigned long seedPF; // Seed for the random generators of the PF + const float period = 33.3; // 33.3ms i.e. 30Hz + if (data.m_pfSeed < 0) { + seedPF = vpTime::measureTimeMicros(); + } + else { + seedPF = data.m_pfSeed; + } + const int nbThread = data.m_pfNbThreads; + //! [Constants_for_the_PF] + + //! [Init_functions] + vpParticleFilter>::vpProcessFunction processFunc = tutorial::fx; + tutorial::vpTutoLikelihoodFunctor likelihoodFtor(sigmaLikelihood, data.m_I_orig.getHeight(), data.m_I_orig.getWidth()); + using std::placeholders::_1; + using std::placeholders::_2; + vpParticleFilter>::vpLikelihoodFunction likelihoodFunc = std::bind(&tutorial::vpTutoLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2); + vpParticleFilter>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter>::simpleResamplingCheck; + vpParticleFilter>::vpResamplingFunction resamplingFunc = vpParticleFilter>::simpleImportanceResampling; + tutorial::vpTutoAverageFunctor averageCpter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth()); + using std::placeholders::_3; + vpParticleFilter>::vpFilterFunction meanFunc = std::bind(&tutorial::vpTutoAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3); + //! [Init_functions] + + //! [Init_PF] + // Initialize the PF + vpParticleFilter> filter(nbParticles, stdevsPF, seedPF, nbThread); + filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc); + //! [Init_PF] + + //! [Init_plot] +#ifdef VISP_HAVE_DISPLAY + unsigned int plotHeight = 350, plotWidth = 350; + int plotXpos = data.m_legendOffset.get_u(); + int plotYpos = data.m_I_orig.getHeight() + 4. * data.m_legendOffset.get_v(); + vpPlot plot(1, plotHeight, plotWidth, plotXpos, plotYpos, "Root mean-square error"); + plot.initGraph(0, 2); + plot.setLegend(0, 0, "LMS estimator"); + plot.setColor(0, 0, vpColor::gray); + plot.setLegend(0, 1, "PF estimator"); + plot.setColor(0, 1, vpColor::red); +#endif +//! [Init_plot] + + bool run = true; + unsigned int nbIter = 0; + double meanDtLMS = 0., meanDtPF = 0.; + double meanRootMeanSquareErrorLMS = 0., meanRootMeanSquareErrorPF = 0.; + while (!data.m_grabber.end() && run) { + std::cout << "Iter " << nbIter << std::endl; + data.m_grabber.acquire(data.m_I_orig); + + tutorial::performSegmentationHSV(data); + + /// Extracting the skeleton of the mask + std::vector edgePoints = tutorial::extractSkeleton(data); + + /// Simulate sensor noise + std::vector noisyEdgePoints = tutorial::addSaltAndPepperNoise(edgePoints, data); + +#ifdef VISP_HAVE_DISPLAY + /// Initial display of the images + vpDisplay::display(data.m_I_orig); + vpDisplay::display(data.m_I_segmented); + vpDisplay::display(data.m_IskeletonNoisy); +#endif + + /// Fit using least-square + double tLms = vpTime::measureTimeMs(); + lmsFitter.fit(noisyEdgePoints); + double dtLms = vpTime::measureTimeMs() - tLms; + float lmsRootMeanSquareError = lmsFitter.evaluate(edgePoints); + std::cout << " [Least-Mean Square method] " << std::endl; + std::cout << " Coeffs = [" << lmsFitter.getCoeffs().transpose() << " ]" << std::endl; + std::cout << " Root Mean Square Error = " << lmsRootMeanSquareError << " pixels" << std::endl; + std::cout << " Fitting duration = " << dtLms << " ms" << std::endl; + meanDtLMS += dtLms; + meanRootMeanSquareErrorLMS += lmsRootMeanSquareError; + + /// Use the PF to filter the measurement + double tPF = vpTime::measureTimeMs(); + //! [Perform_filtering] + filter.filter(noisyEdgePoints, period); + //! [Perform_filtering] + double dtPF = vpTime::measureTimeMs() - tPF; + + //! [Get_filtered_state] + vpColVector Xest = filter.computeFilteredState(); + //! [Get_filtered_state] + + //! [Evaluate_performances] + float pfError = tutorial::evaluate(Xest, data.m_I_orig.getHeight(), data.m_I_orig.getWidth(), edgePoints); + //! [Evaluate_performances] + std::cout << " [Particle Filter method] " << std::endl; + std::cout << " Coeffs = [" << Xest.transpose() << " ]" << std::endl; + std::cout << " Root Mean Square Error = " << pfError << " pixels" << std::endl; + std::cout << " Fitting duration = " << dtPF << " ms" << std::endl; + meanDtPF += dtPF; + meanRootMeanSquareErrorPF += pfError; + +#ifdef VISP_HAVE_DISPLAY + // Update image overlay + lmsFitter.display(data.m_IskeletonNoisy, vpColor::gray, legendLmsVert, legendLmsHor); + tutorial::display(Xest, data.m_IskeletonNoisy, vpColor::red, legendPFVert, legendPFHor); + + // Update plot + plot.plot(0, 0, nbIter, lmsRootMeanSquareError); + plot.plot(0, 1, nbIter, pfError); + // Display the images with overlayed info + data.displayLegend(data.m_I_orig); + vpDisplay::flush(data.m_I_orig); + vpDisplay::flush(data.m_I_segmented); + vpDisplay::flush(data.m_IskeletonNoisy); + run = data.manageClicks(data.m_I_orig, data.m_stepbystep); +#endif + ++nbIter; + } + + double iterAsDouble = static_cast(nbIter); + + std::cout << std::endl << std::endl << "-----[Statistics summary]-----" << std::endl; + + std::cout << " [LMS method] " << std::endl; + std::cout << " Average Root Mean Square Error = " << meanRootMeanSquareErrorLMS / iterAsDouble << " pixels" << std::endl; + std::cout << " Average fitting duration = " << meanDtLMS / iterAsDouble << " ms" << std::endl; + + std::cout << " [Particle Filter method] " << std::endl; + std::cout << " Average Root Mean Square Error = " << meanRootMeanSquareErrorPF / iterAsDouble << " pixels" << std::endl; + std::cout << " Average fitting duration = " << meanDtPF / iterAsDouble << " ms" << std::endl; + +#ifdef VISP_HAVE_DISPLAY + if (data.m_grabber.end() && (!data.m_stepbystep)) { + /// Initial display of the images + vpDisplay::display(data.m_I_orig); + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "End of sequence reached. Click to exit.", data.m_colorLegend); + + /// Update the display + vpDisplay::flush(data.m_I_orig); + + /// Get the user input + vpDisplay::getClick(data.m_I_orig, true); + } +#endif + return 0; +} +#else +int main() +{ + std::cerr << "ViSP must be compiled with C++ standard >= C++11 to use this tutorial." << std::endl; + std::cerr << "ViSP must also have a 3rd party enabling display features, such as X11 or OpenCV." << std::endl; + return EXIT_FAILURE; +} +#endif diff --git a/tutorial/particle-filter-curve-fitting/tutorial-pf-curve-fitting-lms.cpp b/tutorial/particle-filter-curve-fitting/tutorial-pf-curve-fitting-lms.cpp new file mode 100644 index 0000000000..381256e595 --- /dev/null +++ b/tutorial/particle-filter-curve-fitting/tutorial-pf-curve-fitting-lms.cpp @@ -0,0 +1,161 @@ +/* + * 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. + */ + +//! \example tutorial-pf-curve-fitting-lms.cpp + +#include +#include +#include +#include + +#ifdef VISP_HAVE_DISPLAY +#include +#endif + +#include "vpTutoCommonData.h" +#include "vpTutoMeanSquareFitting.h" +#include "vpTutoParabolaModel.h" +#include "vpTutoSegmentation.h" + +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY) +int main(const int argc, const char *argv[]) +{ + tutorial::vpTutoCommonData data; + int returnCode = data.init(argc, argv); + if (returnCode != tutorial::vpTutoCommonData::SOFTWARE_CONTINUE) { + return returnCode; + } + tutorial::vpTutoMeanSquareFitting lmsFitter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth()); + const unsigned int vertOffset = data.m_legendOffset.get_i(); + const unsigned int horOffset = data.m_ipLegend.get_j(); + const unsigned int legendLmsVert = data.m_I_orig.getHeight() - 2 * vertOffset; + const unsigned int legendLmsHor = horOffset; + + //! [Init_plot] +#ifdef VISP_HAVE_DISPLAY + unsigned int plotHeight = 350, plotWidth = 350; + int plotXpos = data.m_legendOffset.get_u(); + int plotYpos = data.m_I_orig.getHeight() + 4. * data.m_legendOffset.get_v(); + vpPlot plot(1, plotHeight, plotWidth, plotXpos, plotYpos, "Root mean-square error"); + plot.initGraph(0, 1); + plot.setLegend(0, 0, "LMS estimator"); + plot.setColor(0, 0, vpColor::gray); +#endif +//! [Init_plot] + + bool run = true; + unsigned int nbIter = 0; + double meanDtLMS = 0.; + double meanRootMeanSquareErrorLMS = 0.; + while (!data.m_grabber.end() && run) { + //! [Measurements_extraction] + std::cout << "Iter " << nbIter << std::endl; + data.m_grabber.acquire(data.m_I_orig); + + // Perform color segmentation + tutorial::performSegmentationHSV(data); + + /// Extracting the skeleton of the mask + std::vector edgePoints = tutorial::extractSkeleton(data); + + /// Simulate sensor noise + std::vector noisyEdgePoints = tutorial::addSaltAndPepperNoise(edgePoints, data); + //! [Measurements_extraction] + +#ifdef VISP_HAVE_DISPLAY + /// Initial display of the images + vpDisplay::display(data.m_I_orig); + vpDisplay::display(data.m_I_segmented); + vpDisplay::display(data.m_IskeletonNoisy); +#endif + + /// Fit using least-square + double tLms = vpTime::measureTimeMs(); + //! [LMS_interpolation] + lmsFitter.fit(noisyEdgePoints); + //! [LMS_interpolation] + double dtLms = vpTime::measureTimeMs() - tLms; + float lmsRootMeanSquareError = lmsFitter.evaluate(edgePoints); + std::cout << " [Least-Mean Square method] " << std::endl; + std::cout << " Coeffs = [" << lmsFitter.getCoeffs().transpose() << " ]" << std::endl; + std::cout << " Root Mean Square Error = " << lmsRootMeanSquareError << " pixels" << std::endl; + std::cout << " Fitting duration = " << dtLms << " ms" << std::endl; + meanDtLMS += dtLms; + meanRootMeanSquareErrorLMS += lmsRootMeanSquareError; + +#ifdef VISP_HAVE_DISPLAY + // Update image overlay + lmsFitter.display(data.m_IskeletonNoisy, vpColor::gray, legendLmsVert, legendLmsHor); + + // Update plot + plot.plot(0, 0, nbIter, lmsRootMeanSquareError); + // Display the images with overlayed info + data.displayLegend(data.m_I_orig); + vpDisplay::flush(data.m_I_orig); + vpDisplay::flush(data.m_I_segmented); + vpDisplay::flush(data.m_IskeletonNoisy); + run = data.manageClicks(data.m_I_orig, data.m_stepbystep); +#endif + ++nbIter; + } + + double iterAsDouble = static_cast(nbIter); + std::cout << std::endl << std::endl << "-----[Statistics summary]-----" << std::endl; + std::cout << " [LMS method] " << std::endl; + std::cout << " Average Root Mean Square Error = " << meanRootMeanSquareErrorLMS / iterAsDouble << " pixels" << std::endl; + std::cout << " Average fitting duration = " << meanDtLMS / iterAsDouble << " ms" << std::endl; + +#ifdef VISP_HAVE_DISPLAY + if (data.m_grabber.end() && (!data.m_stepbystep)) { + /// Initial display of the images + vpDisplay::display(data.m_I_orig); + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "End of sequence reached. Click to exit.", data.m_colorLegend); + + /// Update the display + vpDisplay::flush(data.m_I_orig); + + /// Get the user input + vpDisplay::getClick(data.m_I_orig, true); + } +#endif + return 0; +} +#else +int main() +{ + std::cerr << "ViSP must be compiled with C++ standard >= C++11 to use this tutorial." << std::endl; + std::cerr << "ViSP must also have a 3rd party enabling display features, such as X11 or OpenCV." << std::endl; + return EXIT_FAILURE; +} +#endif diff --git a/tutorial/particle-filter-curve-fitting/tutorial-pf-curve-fitting-pf.cpp b/tutorial/particle-filter-curve-fitting/tutorial-pf-curve-fitting-pf.cpp new file mode 100644 index 0000000000..9f605abf89 --- /dev/null +++ b/tutorial/particle-filter-curve-fitting/tutorial-pf-curve-fitting-pf.cpp @@ -0,0 +1,643 @@ +/* + * 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. + */ + +//! \example tutorial-pf-curve-fitting-pf.cpp + +#include +#include +#include +#include +#include + +#ifdef VISP_HAVE_DISPLAY +#include +#endif + +//! [Include_PF] +#include +//! [Include_PF] + +#include "vpTutoCommonData.h" +#include "vpTutoMeanSquareFitting.h" +#include "vpTutoParabolaModel.h" +#include "vpTutoSegmentation.h" + +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY) +#ifndef DOXYGEN_SHOULD_SKIP_THIS +namespace tutorial +{ +//! [Evaluation_functions] +/** + * \brief Compute the square error between the parabola model and + * the input point \b pt. + * + * \param[in] pt The input point. + * \return double The square error. + */ +double evaluate(const vpImagePoint &pt, const vpTutoParabolaModel &model) +{ + double u = pt.get_u(); + double v = pt.get_v(); + double v_model = model.eval(u); + double error = v - v_model; + double squareError = error * error; + return squareError; +} + +/** + * \brief Compute the mean-square error between the parabola model and + * the input points \b pts. + * + * \param[in] coeffs The coefficients of the polynomial. + * \param[in] height The height of the input image. + * \param[in] width The width of the input image. + * \param[in] pts The input points. + * \return double The root mean square error. + */ +double evaluate(const vpColVector &coeffs, const unsigned int &height, const unsigned int &width, const std::vector &pts) +{ + unsigned int nbPts = pts.size(); + vpColVector residuals(nbPts); + vpColVector weights(nbPts, 1.); + vpTutoParabolaModel model(coeffs, height, width); + // Compute the residuals + for (unsigned int i = 0; i < nbPts; ++i) { + double squareError = evaluate(pts[i], model); + residuals[i] = squareError; + } + double meanSquareError = residuals.sum() / static_cast(nbPts); + return std::sqrt(meanSquareError); +} +//! [Evaluation_functions] + +//! [Display_function] +/** + * \brief Display the fitted parabola on the image. + * + * \tparam T Either unsigned char or vpRGBa. + * \param[in] coeffs The coefficients of the parabola, such as coeffs[0] = a coeffs[1] = b coeffs[2] = c + * \param[in] I The image on which we want to display the parabola model. + * \param[in] color The color we want to use to display the parabola. + */ +template +void display(const vpColVector &coeffs, const vpImage &I, const vpColor &color, + const unsigned int &vertPosLegend, const unsigned int &horPosLegend) +{ +#if defined(VISP_HAVE_DISPLAY) + unsigned int width = I.getWidth(); + vpTutoParabolaModel model(coeffs, I.getHeight(), I.getWidth()); + for (unsigned int u = 0; u < width; ++u) { + float v = model.eval(u); + vpDisplay::displayPoint(I, v, u, color, 1); + vpDisplay::displayText(I, vertPosLegend, horPosLegend, "Particle Filter model", color); + } +#else + (void)coeffs; + (void)I; + (void)color; + (void)vertPosLegend; + (void)horPosLegend; +#endif +} +//! [Display_function] + +//! [Initialization_function] +/** + * \brief Select automatically the init points from the segmented image. + * + * \param[in] data The data common to the whole program. + * \return std::vector The vector of image points to use to initialize + * the Particle Filter using a Least Mean Square minimization. + */ +std::vector automaticInitialization(tutorial::vpTutoCommonData &data) +{ + // Initialization-related variables + const unsigned int minNbPts = data.m_degree + 1; + const unsigned int nbPtsToUse = 10 * minNbPts; + std::vector initPoints; + + // Perform HSV segmentation + tutorial::performSegmentationHSV(data); + + // Extracting the skeleton of the mask + std::vector edgePoints = tutorial::extractSkeleton(data); + unsigned int nbEdgePoints = edgePoints.size(); + + if (nbEdgePoints < nbPtsToUse) { + return edgePoints; + } + + // Uniformly extract init points + auto ptHasLowerU = [](const vpImagePoint &ptA, const vpImagePoint &ptB) { + return ptA.get_u() < ptB.get_u(); + }; + std::sort(edgePoints.begin(), edgePoints.end(), ptHasLowerU); + + unsigned int idStart, idStop; + if (nbEdgePoints > nbPtsToUse + 20) { + // Avoid extreme points in case it's noise + idStart = 10; + idStop = edgePoints.size() - 10; + } + else { + // We need to take all the points because we don't have enough + idStart = 0; + idStop = edgePoints.size(); + } + + // Sample uniformly the points starting from the left of the image to the right + unsigned int sizeWindow = idStop - idStart + 1; + unsigned int step = sizeWindow / (nbPtsToUse - 1); + for (unsigned int id = idStart; id <= idStop; id += step) { + initPoints.push_back(edgePoints[id]); + } + return initPoints; +} + +/** + * \brief Get the init points by user-interaction. + * + * \param[in] data The data common to the whole program. + * \return std::vector The vector that contains the init points. + */ +std::vector manualInitialization(const tutorial::vpTutoCommonData &data) +{ + // Interaction variables + const bool waitForClick = true; + vpImagePoint ipClick; + vpMouseButton::vpMouseButtonType button; + + // Display variables + const unsigned int sizeCross = 10; + const unsigned int thicknessCross = 2; + const vpColor colorCross = vpColor::red; + + // Initialization-related variables + const unsigned int minNbPts = data.m_degree + 1; + std::vector initPoints; + + bool notEnoughPoints = true; + while (notEnoughPoints) { + // Initial display of the images + vpDisplay::display(data.m_I_orig); + + // Display the how-to + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "Left click to add init point (min.: " + std::to_string(minNbPts) + "), right click to estimate the initial coefficients of the Particle Filter.", data.m_colorLegend); + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend + data.m_legendOffset, "A middle click reinitialize the list of init points.", data.m_colorLegend); + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend + data.m_legendOffset + data.m_legendOffset, "If not enough points have been selected, a right click has no effect.", data.m_colorLegend); + + // Display the already selected points + unsigned int nbInitPoints = initPoints.size(); + for (unsigned int i = 0; i < nbInitPoints; ++i) { + vpDisplay::displayCross(data.m_I_orig, initPoints[i], sizeCross, colorCross, thicknessCross); + } + + // Update the display + vpDisplay::flush(data.m_I_orig); + + // Get the user input + vpDisplay::getClick(data.m_I_orig, ipClick, button, waitForClick); + + // Either add the clicked point to the list of initial points or stop the loop if enough points are available + switch (button) { + case vpMouseButton::vpMouseButtonType::button1: + initPoints.push_back(ipClick); + break; + case vpMouseButton::vpMouseButtonType::button2: + initPoints.clear(); + break; + case vpMouseButton::vpMouseButtonType::button3: + (initPoints.size() >= minNbPts ? notEnoughPoints = false : notEnoughPoints = true); + break; + default: + break; + } + } + + return initPoints; +} + +/** + * \brief Compute the initial guess of the state for the Particle Filter + * using Least-Mean-Square minimization. + * + * \param[in] data The data used in the tutorial. + * \return vpColVector The vector containing the coefficients, used as initial guess, + * of the parabola. + */ +vpColVector computeInitialGuess(tutorial::vpTutoCommonData &data) +{ + // Vector that contains the init points + std::vector initPoints; + +#ifdef VISP_HAVE_DISPLAY + // Interaction variables + const bool waitForClick = true; + vpImagePoint ipClick; + vpMouseButton::vpMouseButtonType button; + + // Display variables + const unsigned int sizeCross = 10; + const unsigned int thicknessCross = 2; + const vpColor colorCross = vpColor::red; + + bool automaticInit = false; + + // Initial display of the images + vpDisplay::display(data.m_I_orig); + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "Left click to manually select the init points, right click to automatically initialize the PF", data.m_colorLegend); + + // Update the display + vpDisplay::flush(data.m_I_orig); + + // Get the user input + vpDisplay::getClick(data.m_I_orig, ipClick, button, waitForClick); + + // Either use the automatic initialization or the manual one depending on the user input + switch (button) { + case vpMouseButton::vpMouseButtonType::button1: + automaticInit = false; + break; + case vpMouseButton::vpMouseButtonType::button3: + automaticInit = true; + break; + default: + break; + } + + if (automaticInit) { + // Get automatically the init points from the segmented image + initPoints = tutorial::automaticInitialization(data); + } + else { + // Get manually the init points from the original image + initPoints = tutorial::manualInitialization(data); + } + +#else + // Get the init points from the segmented image + initPoints = tutorial::automaticInitialization(data); +#endif + + // Compute the coefficients of the parabola using Least-Mean-Square minimization. + tutorial::vpTutoMeanSquareFitting lmsFitter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth()); + lmsFitter.fit(initPoints); + vpColVector X0 = lmsFitter.getCoeffs(); + std::cout << "---[Initial fit]---" << std::endl; + std::cout << lmsFitter.getModel(); + std::cout << "---[Initial fit]---" << std::endl; + + // Display info about the initialization + vpDisplay::display(data.m_I_orig); + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "Here are the points selected for the initialization.", data.m_colorLegend); + unsigned int nbInitPoints = initPoints.size(); + for (unsigned int i = 0; i < nbInitPoints; ++i) { + const vpImagePoint &ip = initPoints[i]; + vpDisplay::displayCross(data.m_I_orig, ip, sizeCross, colorCross, thicknessCross); + } + + // Update display and wait for click + lmsFitter.display(data.m_I_orig, vpColor::red, data.m_ipLegend.get_v() + 2 * data.m_legendOffset.get_v(), data.m_ipLegend.get_u()); + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend + data.m_legendOffset, "A click to continue.", data.m_colorLegend); + vpDisplay::flush(data.m_I_orig); + vpDisplay::getClick(data.m_I_orig, waitForClick); + + return X0; +} +//! [Initialization_function] + +//! [Process_function] +vpColVector fx(const vpColVector &coeffs, const double &/*dt*/) +{ + vpColVector updatedCoeffs = coeffs; // We use a constant position model + return updatedCoeffs; +} +//! [Process_function] + +//! [Average_functor] +class vpTutoAverageFunctor +{ +public: + vpTutoAverageFunctor(const unsigned int °ree, const unsigned int &height, const unsigned int &width) + : m_degree(degree) + , m_height(height) + , m_width(width) + { } + + /** + * \brief Compute the "weighted average" of polynomial models, by sampling control points and + * then performing Least-Mean Square minimization to best fit the control points. + * + * \param[in] particles The vector containing the particles of the PF. + * \param[in] weights Their associated weights + * + * \return vpColVector The coefficients of the polynomial model that best fits the control points. + */ + vpColVector averagePolynomials(const std::vector &particles, const std::vector &weights, const vpParticleFilter>::vpStateAddFunction &/**/) + { + const unsigned int nbParticles = particles.size(); + const double nbParticlesAsDOuble = static_cast(nbParticles); + // Compute the sum of the weights to be able to determine the "importance" of a particle with regard to the whole set + const double sumWeight = std::accumulate(weights.begin(), weights.end(), 0.); + + // Defining the total number of control points we want to generate + const double nbPointsForAverage = 10. * nbParticlesAsDOuble; + std::vector initPoints; + + // Creating control points by each particle + for (unsigned int i = 0; i < nbParticles; ++i) { + // The number of control points a particle can generate is proportional to the ratio of its weight w.r.t. the sum of the weights + double nbPoints = std::floor(weights[i] * nbPointsForAverage / sumWeight); + if (nbPoints > 1.) { + // The particle has a weight high enough to deserve more than one points + vpTutoParabolaModel curve(particles[i], m_height, m_width); + double widthAsDouble = static_cast(m_width); + // Uniform sampling of the control points along the polynomial model + double step = widthAsDouble / (nbPoints - 1.); + for (double u = 0.; u < widthAsDouble; u += step) { + double v = curve.eval(u); + vpImagePoint pt(v, u); + initPoints.push_back(pt); + } + } + else if (nbPoints == 1.) { + // The weight of the particle make it have only one control point + // We sample it at the middle of the image + vpTutoParabolaModel curve(particles[i], m_height, m_width); + double u = static_cast(m_width) / 2.; + double v = curve.eval(u); + vpImagePoint pt(v, u); + initPoints.push_back(pt); + } + } + // We use Least-Mean Square minimization to compute the polynomial model that best fits all the control points + vpTutoMeanSquareFitting lms(m_degree, m_height, m_width); + lms.fit(initPoints); + return lms.getCoeffs(); + } + +private: + unsigned int m_degree; //!< The degree of the polynomial. + unsigned int m_height; //!< The height of the input image. + unsigned int m_width; //!< The width of the input image. +}; +//! [Average_functor] + +//! [Likelihood_functor] +class vpTutoLikelihoodFunctor +{ +public: + /** + * @brief Construct a new vp Likelihood Functor object + * + * \param[in] stdev The standard deviation of the likelihood function. + * \param[in] height The height of the input image. + * \param[in] width The width of the input image. + */ + vpTutoLikelihoodFunctor(const double &stdev, const unsigned int &height, const unsigned int &width) + : m_height(height) + , m_width(width) + { + double sigmaDistanceSquared = stdev * stdev; + m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared); + m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared); + } + + //! [Likelihood_function] + /** + * \brief Compute the likelihood of a particle compared to the measurements. + * The likelihood equals zero if the particle is completely different of + * the measurements and equals one if it matches completely. + * The chosen likelihood is 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. + * + * \param[in] coeffs The particle, which represent the parabola coefficients. + * \param[in] meas The measurement vector. + * \return double The likelihood of the particle. + */ + double likelihood(const vpColVector &coeffs, const std::vector &meas) + { + double likelihood = 0.; + unsigned int nbPoints = meas.size(); + + // Generate a model from the coefficients stored in the particle state + vpTutoParabolaModel model(coeffs, m_height, m_width); + + // Compute the residual between each measurement point and its equivalent in the model + vpColVector residuals(nbPoints); + for (unsigned int i = 0; i < nbPoints; ++i) { + double squareError = tutorial::evaluate(meas[i], model); + residuals[i] = squareError; + } + + // Use Tukey M-estimator to be robust against outliers + vpRobust Mestimator; + vpColVector w(nbPoints, 1.); + Mestimator.MEstimator(vpRobust::TUKEY, residuals, w); + double sumError = w.hadamard(residuals).sum(); + + // Compute the likelihood as a Gaussian function + likelihood = std::exp(m_constantExpDenominator * sumError / w.sum()) * 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; + } + //! [Likelihood_function] +private: + 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. + unsigned int m_height; //!< The height of the input image. + unsigned int m_width; //!< The width of the input image. +}; +//! [Likelihood_functor] +} +#endif + +int main(const int argc, const char *argv[]) +{ + tutorial::vpTutoCommonData data; + int returnCode = data.init(argc, argv); + if (returnCode != tutorial::vpTutoCommonData::SOFTWARE_CONTINUE) { + return returnCode; + } + const unsigned int vertOffset = data.m_legendOffset.get_i(); + const unsigned int horOffset = data.m_ipLegend.get_j(); + const unsigned int legendPFVert = data.m_I_orig.getHeight() - 2 * vertOffset, legendPFHor = horOffset; + + // Initialize the attributes of the PF + //! [Initial_estimates] + vpColVector X0 = tutorial::computeInitialGuess(data); + //! [Initial_estimates] + + //! [Constants_for_the_PF] + const double maxDistanceForLikelihood = data.m_pfMaxDistanceForLikelihood; // 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 = data.m_pfN; // Number of particles to use + std::vector stdevsPF; // Standard deviation for each state component + for (unsigned int i = 0; i < data.m_degree + 1; ++i) { + double ampliMax = data.m_pfRatiosAmpliMax[i] * X0[i]; + stdevsPF.push_back(ampliMax / 3.); + } + unsigned long seedPF; // Seed for the random generators of the PF + const float period = 33.3; // 33.3ms i.e. 30Hz + if (data.m_pfSeed < 0) { + seedPF = vpTime::measureTimeMicros(); + } + else { + seedPF = data.m_pfSeed; + } + const int nbThread = data.m_pfNbThreads; + //! [Constants_for_the_PF] + + //! [Init_functions] + vpParticleFilter>::vpProcessFunction processFunc = tutorial::fx; + tutorial::vpTutoLikelihoodFunctor likelihoodFtor(sigmaLikelihood, data.m_I_orig.getHeight(), data.m_I_orig.getWidth()); + using std::placeholders::_1; + using std::placeholders::_2; + vpParticleFilter>::vpLikelihoodFunction likelihoodFunc = std::bind(&tutorial::vpTutoLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2); + vpParticleFilter>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter>::simpleResamplingCheck; + vpParticleFilter>::vpResamplingFunction resamplingFunc = vpParticleFilter>::simpleImportanceResampling; + tutorial::vpTutoAverageFunctor averageCpter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth()); + using std::placeholders::_3; + vpParticleFilter>::vpFilterFunction meanFunc = std::bind(&tutorial::vpTutoAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3); + //! [Init_functions] + + //! [Init_PF] + // Initialize the PF + vpParticleFilter> filter(nbParticles, stdevsPF, seedPF, nbThread); + filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc); + //! [Init_PF] + + //! [Init_plot] +#ifdef VISP_HAVE_DISPLAY + unsigned int plotHeight = 350, plotWidth = 350; + int plotXpos = data.m_legendOffset.get_u(); + int plotYpos = data.m_I_orig.getHeight() + 4. * data.m_legendOffset.get_v(); + vpPlot plot(1, plotHeight, plotWidth, plotXpos, plotYpos, "Root mean-square error"); + plot.initGraph(0, 1); + plot.setLegend(0, 0, "PF estimator"); + plot.setColor(0, 0, vpColor::red); +#endif +//! [Init_plot] + + bool run = true; + unsigned int nbIter = 0; + double meanDtPF = 0.; + double meanRootMeanSquareErrorPF = 0.; + while (!data.m_grabber.end() && run) { + std::cout << "Iter " << nbIter << std::endl; + data.m_grabber.acquire(data.m_I_orig); + + tutorial::performSegmentationHSV(data); + + /// Extracting the skeleton of the mask + std::vector edgePoints = tutorial::extractSkeleton(data); + + /// Simulate sensor noise + std::vector noisyEdgePoints = tutorial::addSaltAndPepperNoise(edgePoints, data); + +#ifdef VISP_HAVE_DISPLAY + /// Initial display of the images + vpDisplay::display(data.m_I_orig); + vpDisplay::display(data.m_I_segmented); + vpDisplay::display(data.m_IskeletonNoisy); +#endif + + /// Use the PF to filter the measurement + double tPF = vpTime::measureTimeMs(); + //! [Perform_filtering] + filter.filter(noisyEdgePoints, period); + //! [Perform_filtering] + double dtPF = vpTime::measureTimeMs() - tPF; + + //! [Get_filtered_state] + vpColVector Xest = filter.computeFilteredState(); + //! [Get_filtered_state] + + //! [Evaluate_performances] + float pfError = tutorial::evaluate(Xest, data.m_I_orig.getHeight(), data.m_I_orig.getWidth(), edgePoints); + //! [Evaluate_performances] + std::cout << " [Particle Filter method] " << std::endl; + std::cout << " Coeffs = [" << Xest.transpose() << " ]" << std::endl; + std::cout << " Root Mean Square Error = " << pfError << " pixels" << std::endl; + std::cout << " Fitting duration = " << dtPF << " ms" << std::endl; + meanDtPF += dtPF; + meanRootMeanSquareErrorPF += pfError; + +#ifdef VISP_HAVE_DISPLAY + // Update image overlay + tutorial::display(Xest, data.m_IskeletonNoisy, vpColor::red, legendPFVert, legendPFHor); + + // Update plot + plot.plot(0, 0, nbIter, pfError); + // Display the images with overlayed info + data.displayLegend(data.m_I_orig); + vpDisplay::flush(data.m_I_orig); + vpDisplay::flush(data.m_I_segmented); + vpDisplay::flush(data.m_IskeletonNoisy); + run = data.manageClicks(data.m_I_orig, data.m_stepbystep); +#endif + ++nbIter; + } + + double iterAsDouble = static_cast(nbIter); + std::cout << std::endl << std::endl << "-----[Statistics summary]-----" << std::endl; + std::cout << " [Particle Filter method] " << std::endl; + std::cout << " Average Root Mean Square Error = " << meanRootMeanSquareErrorPF / iterAsDouble << " pixels" << std::endl; + std::cout << " Average fitting duration = " << meanDtPF / iterAsDouble << " ms" << std::endl; + +#ifdef VISP_HAVE_DISPLAY + if (data.m_grabber.end() && (!data.m_stepbystep)) { + /// Initial display of the images + vpDisplay::display(data.m_I_orig); + vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "End of sequence reached. Click to exit.", data.m_colorLegend); + + /// Update the display + vpDisplay::flush(data.m_I_orig); + + /// Get the user input + vpDisplay::getClick(data.m_I_orig, true); + } +#endif + return 0; +} +#else +int main() +{ + std::cerr << "ViSP must be compiled with C++ standard >= C++11 to use this tutorial." << std::endl; + std::cerr << "ViSP must also have a 3rd party enabling display features, such as X11 or OpenCV." << std::endl; + return EXIT_FAILURE; +} +#endif diff --git a/tutorial/particle-filter-curve-fitting/vpTutoCommonData.h b/tutorial/particle-filter-curve-fitting/vpTutoCommonData.h new file mode 100644 index 0000000000..f6d6757681 --- /dev/null +++ b/tutorial/particle-filter-curve-fitting/vpTutoCommonData.h @@ -0,0 +1,342 @@ +/* + * 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. + */ +#ifndef VP_COMMMON_DATA_H +#define VP_COMMMON_DATA_H +#include +#include + +#include +#include +#include +#include +#include +#include + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +#ifndef DOXYGEN_SHOULD_SKIP_THIS +namespace tutorial +{ +inline void log(std::ostream &os, const std::string &filename, const std::string &funName, const std::string &arrayName, const VISP_NAMESPACE_ADDRESSING vpArray2D &array, const unsigned int &level = 0) +{ + os << "[" << filename << "::" << funName << "] "; + for (unsigned int i = 0; i < level; ++i) { + os << "\t"; + } + os << arrayName << ":=" << std::endl; + for (unsigned int r = 0; r < array.getRows(); ++r) { + for (unsigned int i = 0; i < level; ++i) { + os << "\t"; + } + os << "["; + for (unsigned int c = 0; c < array.getCols() - 1; ++c) { + os << std::setprecision(3) << std::scientific << array[r][c] << "\t; "; + } + os << array[r][array.getCols() - 1] << "]\n"; + } + os << std::flush; +} + +typedef struct vpTutoCommonData +{ + static const int SOFTWARE_CONTINUE = 4221; + const VISP_NAMESPACE_ADDRESSING vpColor m_colorLegend = VISP_NAMESPACE_ADDRESSING vpColor::red; + const VISP_NAMESPACE_ADDRESSING vpImagePoint m_ipLegend = VISP_NAMESPACE_ADDRESSING vpImagePoint(20, 20); + const VISP_NAMESPACE_ADDRESSING vpImagePoint m_legendOffset = VISP_NAMESPACE_ADDRESSING vpImagePoint(20, 0); + std::string m_seqFilename; /*!< Sequence filenames, such as I%04d.png*/ + VISP_NAMESPACE_ADDRESSING vpVideoReader m_grabber; /*!< Video grabber from stored files.*/ + std::string m_hsvFilename; /*!< Filename of the YAML file that contains the HSV thresholds.*/ + VISP_NAMESPACE_ADDRESSING vpColVector m_hsv_values; /*!< Vector that contains the lower and upper limits of the HSV thresholds.*/ + bool m_stepbystep; /*!< If true, the frames are treated in a step by step mode, otherwise the frames are treated as a video.*/ + double m_ratioSaltPepperNoise; /*!< Ratio of noise points to introduce in the addSaltAndPepperNoise function.*/ + unsigned int m_degree; //!< Degree for the polynomials. + + /// Images and displays parameters + VISP_NAMESPACE_ADDRESSING vpImage m_I_orig; /*!< The color image read from the file.*/ + VISP_NAMESPACE_ADDRESSING vpImage m_I_segmented; /*!< The segmented color image resulting from HSV segmentation.*/ + VISP_NAMESPACE_ADDRESSING vpImage m_mask; /*!< A binary mask where 255 means that a pixel belongs to the HSV range delimited by the HSV thresholds.*/ + VISP_NAMESPACE_ADDRESSING vpImage m_Iskeleton; /*!< The image resulting from the skeletonization of the mask.*/ + VISP_NAMESPACE_ADDRESSING vpImage m_IskeletonNoisy; /*!< The image resulting from the skeletonization of the mask, to which is added some salt and pepper noise.*/ +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY) + std::shared_ptr m_displayOrig; + std::shared_ptr m_displaySegmented; + std::shared_ptr m_displayNoisy; +#elif defined(VISP_HAVE_DISPLAY) + VISP_NAMESPACE_ADDRESSING vpDisplay *m_displayOrig; + VISP_NAMESPACE_ADDRESSING vpDisplay *m_displaySegmented; + VISP_NAMESPACE_ADDRESSING vpDisplay *m_displayNoisy; +#endif + + /// Particle filter parameters + double m_pfMaxDistanceForLikelihood; /*!< Maximum tolerated distance for the likelihood evaluation.*/ + unsigned int m_pfN; /*!< Number of particles for the particle filter.*/ + std::vector m_pfRatiosAmpliMax; /*!< The ratio of the initial guess the maximum amplitude of noise on each coefficient of the parabola.*/ + long m_pfSeed; /*!< The seed for the particle filter. A negative value will use the current timestamp.*/ + int m_pfNbThreads; /*!< Number of threads the Particle filter should use.*/ + + vpTutoCommonData() + : m_seqFilename(VISP_NAMESPACE_ADDRESSING vpIoTools::createFilePath("data", "color_image_%04d.png")) + , m_hsvFilename(VISP_NAMESPACE_ADDRESSING vpIoTools::createFilePath("calib", "hsv-thresholds.yml")) + , m_stepbystep(true) + , m_ratioSaltPepperNoise(0.15) + , m_degree(2) +#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY) + , m_displayOrig(nullptr) + , m_displaySegmented(nullptr) + , m_displayNoisy(nullptr) +#endif + , m_pfMaxDistanceForLikelihood(40) + , m_pfN(300) + , m_pfRatiosAmpliMax({ 0.25, 0.25, 0.25 }) + , m_pfSeed(4221) + , m_pfNbThreads(-1) + { } + +#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY) + ~vpTutoCommonData() + { + if (m_displayOrig != nullptr) { + delete m_displayOrig; + m_displayOrig = nullptr; + } + if (m_displaySegmented != nullptr) { + delete m_displaySegmented; + m_displaySegmented = nullptr; + } + + if (m_displayNoisy != nullptr) { + delete m_displayNoisy; + m_displayNoisy = nullptr; + } + } +#endif + + /** + * \brief Print the help about the program optional parameters. + * + * \param[in] softName The name of the program. + */ + inline void printHelp(const char *softName) + { + std::cout << "\nSYNOPSIS " << std::endl + << softName + << " [--video ] [--hsv-thresholds ] [--noise ]" << std::endl + << " [--degree ]" << std::endl + << " [--max-distance-likelihood ] [-N, --nb-particles ] [--seed ] [--nb-threads ] [--state-noise-ratio ]" << std::endl + << " [--help,-h]" + << std::endl; + std::cout << "\nOPTIONS " << std::endl + << " [General params]" << std::endl + << " --video " << std::endl + << " Name of the input video filename." << std::endl + << " If name is set to \"generate-simulated\" a simulated image is generated." << std::endl + << " Example: --video " << this->m_seqFilename << std::endl + << std::endl + << " --hsv-thresholds " << std::endl + << " Path to a yaml filename that contains H , S , V threshold values." << std::endl + << " For an example, have a look to the file \"" << this->m_hsvFilename << "\"" << std::endl + << std::endl + << " --noise " << std::endl + << " Ratio of noisy points added to the image resulting from the skeletonization of the segmented image, to simulate sensor noise." << std::endl + << " Default = " << this->m_ratioSaltPepperNoise << std::endl + << std::endl + << " --degree " << std::endl + << " Choose the degree of the polynomials to use." << std::endl + << " Default = " << this->m_degree << std::endl + << std::endl + << std::endl + << " [PF params]" << std::endl + << " --max-distance-likelihood" << std::endl + << " Maximum mean square distance between a particle with the measurements." << std::endl + << " Above this value, the likelihood of the particle is 0." << std::endl + << " NOTE: M-estimation is used to make the likelihood function robust against outliers." << std::endl + << " Default: " << m_pfMaxDistanceForLikelihood << std::endl + << std::endl + << " -N, --nb-particles" << std::endl + << " Number of particles of the Particle Filter." << std::endl + << " Default: " << m_pfN << std::endl + << std::endl + << " --seed" << std::endl + << " Seed to initialize the Particle Filter." << std::endl + << " Use a negative value makes to use the current timestamp instead." << std::endl + << " Default: " << m_pfSeed << std::endl + << std::endl + << " --nb-threads" << std::endl + << " Set the number of threads to use in the Particle Filter (only if OpenMP is available)." << std::endl + << " Use a negative value to use the maximum number of threads instead." << std::endl + << " Default: " << m_pfNbThreads << std::endl + << std::endl + << " --state-noise-ratio " << std::endl + << " Ratio of the initial guess of the curve coefficients to use as maximal amplitude of the noise added to the particles." << std::endl + << " Default: " << m_pfRatiosAmpliMax[0] << std::endl + << " --help, -h" << std::endl + << " Display this helper message." << std::endl + << std::endl; + } + + /** + * \brief Initialize the program data from the command line arguments. + * + * \param[in] argc The number of optional parameters. + * \param[in] argv The values of the optional parameters. + * \return int Initialization status. EXIT_FAILURE if there was a problem, EXIT_SUCCESS if printing the program + * help was asked and SOFTWARE_CONTINUE if the initialization went well. + */ + inline int init(const int &argc, const char *argv[]) + { + // Parse the input arguments + int i = 1; + while (i < argc) { + std::string argname(argv[i]); + if ((argname == std::string("--video")) && ((i + 1) < argc)) { + ++i; + m_seqFilename = std::string(argv[i]); + } + else if ((argname == std::string("--hsv-thresholds")) && ((i + 1) < argc)) { + ++i; + m_hsvFilename = std::string(argv[i]); + } + else if ((argname == "--noise") && ((i + 1) < argc)) { + ++i; + m_ratioSaltPepperNoise = std::atof(argv[i]); + } + else if ((argname == std::string("--degree")) && ((i + 1) < argc)) { + ++i; + m_degree = std::atoi(argv[i]); + } + else if ((argname == "--max-distance-likelihood") && ((i+1) < argc)) { + ++i; + m_pfMaxDistanceForLikelihood = std::atof(argv[i]); + } + else if (((argname == "-N") || (argname == "--nb-particles")) && ((i+1) < argc)) { + ++i; + m_pfN = std::atoi(argv[i]); + } + else if ((argname == "--seed") && ((i+1) < argc)) { + ++i; + m_pfSeed = std::atoi(argv[i]); + } + else if ((argname == "--nb-threads") && ((i+1) < argc)) { + ++i; + m_pfNbThreads = std::atoi(argv[i]); + } + else if ((argname == "--state-noise-ratio") && ((i+1) < argc)) { + ++i; + m_pfRatiosAmpliMax[0] = std::atof(argv[i]); + } + else if ((argname == std::string("-h")) || (argname == std::string("--help"))) { + vpTutoCommonData helpPrinter; + helpPrinter.printHelp(argv[0]); + return EXIT_SUCCESS; + } + else { + std::cerr << "Unknown argument \"" << argname << "\"" << std::endl; + return EXIT_FAILURE; + } + ++i; + } + + // Ensure that the maximal amplitude vector is of correct size and values + m_pfRatiosAmpliMax.resize(m_degree, m_pfRatiosAmpliMax[0]); + + // Load the HSV thresholds + if (VISP_NAMESPACE_ADDRESSING vpColVector::loadYAML(m_hsvFilename, m_hsv_values)) { + std::cout << "Load HSV threshold values from " << m_hsvFilename << std::endl; + std::cout << "HSV low/high values: " << m_hsv_values.t() << std::endl; + } + else { + std::cout << "ERROR: unable to load HSV thresholds values from " << m_hsvFilename << std::endl; + return EXIT_FAILURE; + } + + // Open the sequence of images + try { + m_grabber.setFileName(m_seqFilename); + m_grabber.open(m_I_orig); + } + catch (const VISP_NAMESPACE_ADDRESSING vpException &e) { + std::cout << e.getStringMessage() << std::endl; + return EXIT_FAILURE; + } + + m_I_segmented.resize(m_I_orig.getHeight(), m_I_orig.getWidth()); // Resize the segmented image to match the original image + m_mask.resize(m_I_orig.getHeight(), m_I_orig.getWidth()); // Resize the binary mask that indicates which pixels are in the allowed HSV range. + m_Iskeleton.resize(m_I_orig.getHeight(), m_I_orig.getWidth()); // Resize the edge-map. + m_IskeletonNoisy.resize(m_I_orig.getHeight(), m_I_orig.getWidth()); // Resize the edge-map. + + // Init the displays + const int horOffset = 20, vertOffset = 25; + std::string skeletonTitle("Skeletonized image ("); + skeletonTitle += (m_ratioSaltPepperNoise == 0 ? "without" : std::to_string(static_cast(m_ratioSaltPepperNoise * 100.)) + "%"); + skeletonTitle += " noise)"; +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY) + m_displayOrig = VISP_NAMESPACE_ADDRESSING vpDisplayFactory::createDisplay(m_I_orig, horOffset, vertOffset, "Original image"); + m_displaySegmented = VISP_NAMESPACE_ADDRESSING vpDisplayFactory::createDisplay(m_I_segmented, 2 * horOffset + m_I_orig.getWidth(), vertOffset, "Segmented image"); + m_displayNoisy = VISP_NAMESPACE_ADDRESSING vpDisplayFactory::createDisplay(m_IskeletonNoisy, 2 * horOffset + m_I_orig.getWidth(), 2 * vertOffset + m_I_orig.getHeight(), skeletonTitle); +#elif defined(VISP_HAVE_DISPLAY) + m_displayOrig = VISP_NAMESPACE_ADDRESSING vpDisplayFactory::allocateDisplay(m_I_orig, horOffset, vertOffset, "Original image"); + m_displaySegmented = VISP_NAMESPACE_ADDRESSING vpDisplayFactory::allocateDisplay(m_I_segmented, 2 * horOffset + m_I_orig.getWidth(), vertOffset, "Segmented image"); + m_displayNoisy = VISP_NAMESPACE_ADDRESSING vpDisplayFactory::allocateDisplay(m_IskeletonNoisy, 2 * horOffset + m_I_orig.getWidth(), 2 * vertOffset + m_I_orig.getHeight(), skeletonTitle); +#endif + return SOFTWARE_CONTINUE; + } + +#ifdef VISP_HAVE_DISPLAY + template + void displayLegend(const VISP_NAMESPACE_ADDRESSING vpImage &I) + { + VISP_NAMESPACE_ADDRESSING vpImagePoint ip(20, 20); + VISP_NAMESPACE_ADDRESSING vpImagePoint offset(20, 0); + if (m_stepbystep) { + VISP_NAMESPACE_ADDRESSING vpDisplay::displayText(I, ip, std::string("Left click to switch to next image"), VISP_NAMESPACE_ADDRESSING vpColor::red); + } + VISP_NAMESPACE_ADDRESSING vpDisplay::displayText(I, ip + offset, std::string("Middle click to switch to ") + (m_stepbystep ? std::string("video mode") : std::string("step-by-step mode")), VISP_NAMESPACE_ADDRESSING vpColor::red); + VISP_NAMESPACE_ADDRESSING vpDisplay::displayText(I, ip + offset + offset, std::string("Right click to quit"), VISP_NAMESPACE_ADDRESSING vpColor::red); + } + + template + bool manageClicks(const VISP_NAMESPACE_ADDRESSING vpImage &I, bool &stepbystep) + { + VISP_NAMESPACE_ADDRESSING vpImagePoint ip; + VISP_NAMESPACE_ADDRESSING vpMouseButton::vpMouseButtonType button; + VISP_NAMESPACE_ADDRESSING vpDisplay::getClick(I, ip, button, stepbystep); + if (button == VISP_NAMESPACE_ADDRESSING vpMouseButton::vpMouseButtonType::button3) { + return false; + } + if (button == VISP_NAMESPACE_ADDRESSING vpMouseButton::vpMouseButtonType::button2) { + stepbystep = stepbystep ^ true; + } + return true; + } +#endif +}vpTutoCommonData; +} +#endif +#endif +#endif diff --git a/tutorial/particle-filter-curve-fitting/vpTutoMeanSquareFitting.cpp b/tutorial/particle-filter-curve-fitting/vpTutoMeanSquareFitting.cpp new file mode 100644 index 0000000000..a357909b52 --- /dev/null +++ b/tutorial/particle-filter-curve-fitting/vpTutoMeanSquareFitting.cpp @@ -0,0 +1,108 @@ +/* + * 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 "vpTutoMeanSquareFitting.h" + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +#ifndef DOXYGEN_SHOULD_SKIP_THIS +namespace tutorial +{ +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + +vpTutoMeanSquareFitting::vpTutoMeanSquareFitting(const unsigned int °ree, const unsigned int &height, const unsigned int &width) + : m_degree(degree) + , m_height(static_cast(height)) + , m_width(static_cast(width)) + , m_model(degree, height, width) + , m_isFitted(false) +{ } + +//! [Solve_LMS_system] +void vpTutoMeanSquareFitting::fit(const std::vector &pts) +{ + vpMatrix A; // The matrix that contains the u^i + vpMatrix X; // The matrix we want to estimate, that contains the polynomial coefficients. + vpMatrix b; // The matrix that contains the v values + + // Fill the matrices that form the system we want to solve + vpTutoParabolaModel::fillSystem(m_degree, m_height, m_width, pts, A, b); + + // Compute the parabola coefficients using the least-mean-square method. + X = A.pseudoInverse() * b; + m_model = vpTutoParabolaModel(X, m_height, m_width); + m_isFitted = true; +} +//! [Solve_LMS_system] + +double vpTutoMeanSquareFitting::evaluate(const std::vector &pts) +{ + if (!m_isFitted) { + throw(vpException(vpException::notInitialized, "fit() has not been called.")); + } + unsigned int nbPts = pts.size(); + + // Compute the mean absolute error + double meanSquareError = 0.f; + for (unsigned int i = 0; i < nbPts; ++i) { + double squareError = evaluate(pts[i]); + meanSquareError += squareError; + } + meanSquareError /= static_cast(nbPts); + return std::sqrt(meanSquareError); +} + +double vpTutoMeanSquareFitting::evaluate(const vpImagePoint &pt) +{ + if (!m_isFitted) { + throw(vpException(vpException::notInitialized, "fit() has not been called.")); + } + double u = pt.get_u(); + double v = pt.get_v(); + double v_model = model(u); + double error = v - v_model; + double squareError = error * error; + return squareError; +} + +double vpTutoMeanSquareFitting::model(const float &u) +{ + if (!m_isFitted) { + throw(vpException(vpException::notInitialized, "fit() has not been called.")); + } + double v = m_model.eval(u); + return v; +} +} +#endif +#else +void dummy_vpTutoMeanSquareFitting() { } +#endif diff --git a/tutorial/particle-filter-curve-fitting/vpTutoMeanSquareFitting.h b/tutorial/particle-filter-curve-fitting/vpTutoMeanSquareFitting.h new file mode 100644 index 0000000000..66b771ce83 --- /dev/null +++ b/tutorial/particle-filter-curve-fitting/vpTutoMeanSquareFitting.h @@ -0,0 +1,162 @@ +/* + * 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. + */ + +#ifndef VP_TUTO_MEAN_SQUARE_FITTING_H +#define VP_TUTO_MEAN_SQUARE_FITTING_H + +#include +#include +#include +#include +#include + +#include "vpTutoParabolaModel.h" + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +#ifndef DOXYGEN_SHOULD_SKIP_THIS +namespace tutorial +{ +/** + * \brief Estimates the coefficients of a parabola v = a u^2 + b u + c + * using the least-mean-square method. + */ +class vpTutoMeanSquareFitting +{ +public: + /** + * \brief Construct a new vp Tuto Mean Square Fitting object + * + * \param[in] degree The desired degree of the polynomial to fit. + * \param[in] height The height of the input image. + * \param[in] width The width of the input image. + */ + vpTutoMeanSquareFitting(const unsigned int °ree, const unsigned int &height, const unsigned int &width); + + /** + * \brief Estimate the parabola coefficients that fits the best + * the input points \b pts. + * + * \param[in] pts The input points for which we want to fit a parabola model. + */ + void fit(const std::vector &pts); + + /** + * \brief Compute the Root Mean Square Error between the model and the given points. + * + * \param[in] pts The Ground Truth points. + * \return double The Root Mean Square Error \f[ RMSE = \sqrt{\frac{\sum_{i = 0}^{N-1} e^2}{N}} \f] + */ + double evaluate(const std::vector &pts); + + /** + * \brief Compute the square error between the model and a Ground Truth point. + * + * \param[in] pt The Ground Truth point. + * \return double The square error. + */ + double evaluate(const VISP_NAMESPACE_ADDRESSING vpImagePoint &pt); + + /** + * \brief Compute the v-coordinate that corresponds to the given u-coordinate + * based on the underlying polynomial model. + * + * \param[in] u The u-coordinate for which we want to know the v-coordinate computed using + * the underlying polynomial model. + * \return double The v-ccordinate that corresponds to the model. + */ + double model(const float &u); + +#ifdef VISP_HAVE_DISPLAY + /** + * \brief Display the fitted parabola on the image. + * + * \tparam T Either unsigned char or vpRGBa. + * \param[in] I The image on which we want to display the parabola model. + * \param[in] color The color we want to use to display the parabola. + */ + template + void display(const VISP_NAMESPACE_ADDRESSING vpImage &I, const VISP_NAMESPACE_ADDRESSING vpColor &color, + const unsigned int &vertPosLegend, const unsigned int &horPosLegend) + { + unsigned int width = I.getWidth(); + for (unsigned int u = 0; u < width; ++u) { + int v = model(u); + VISP_NAMESPACE_ADDRESSING vpDisplay::displayPoint(I, v, u, color, 1); + VISP_NAMESPACE_ADDRESSING vpDisplay::displayText(I, vertPosLegend, horPosLegend, "Least-mean square model", color); + } + } +#endif + + /** + * \brief Permits to reinitialize the least-mean square fitter. + */ + inline void reinit() + { + m_isFitted = false; + } + + inline vpTutoMeanSquareFitting &operator=(const vpTutoMeanSquareFitting &other) + { + m_model = other.m_model; + m_isFitted = other.m_isFitted; + return *this; + } + + /** + * \brief Get the coefficients of the parabola model. + * + * \return vpColVector The coefficients of the parabola, as described in \b vpTutoParabolaModel::toVpColVector() + */ + inline VISP_NAMESPACE_ADDRESSING vpColVector getCoeffs() const + { + return m_model.toVpColVector(); + } + + /** + * \brief Get the parabola model that is fitted. + * + * \return vpTutoParabolaModel The parabola model. + */ + inline const vpTutoParabolaModel &getModel() const + { + return m_model; + } + +protected: + unsigned int m_degree; /*!< The degree of the curve that is estimated*/ + double m_height; /*!< The height of the input image*/ + double m_width; /*!< The width of the input image*/ + vpTutoParabolaModel m_model; /*!< The model of the curve we try to fit.*/ + bool m_isFitted; /*!< Set to true if the fit method has been called.*/ +}; +} +#endif +#endif +#endif diff --git a/tutorial/particle-filter-curve-fitting/vpTutoParabolaModel.h b/tutorial/particle-filter-curve-fitting/vpTutoParabolaModel.h new file mode 100644 index 0000000000..ba71e2cf9f --- /dev/null +++ b/tutorial/particle-filter-curve-fitting/vpTutoParabolaModel.h @@ -0,0 +1,175 @@ +/* + * 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. + */ + +#ifndef VP_PARABOLA_MODEL_H +#define VP_PARABOLA_MODEL_H + +#include +#include +#include + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +#ifndef DOXYGEN_SHOULD_SKIP_THIS +namespace tutorial +{ +/*! + * \brief Model of a parabola \f[v = \sum_{i = 0}^N a_i u^i \f] where \f[N\f] is the + * degree of the polynomial. + */ +class vpTutoParabolaModel +{ +public: + inline vpTutoParabolaModel(const unsigned int °ree, const unsigned int &height, const unsigned int &width) + : m_degree(degree) + , m_height(static_cast(height)) + , m_width(static_cast(width)) + , m_coeffs(degree + 1, 0.) + { } + + /** + * \brief Construct a new vpTutoParabolaModel object + * + * \param[in] coeffs The coefficients of the polynomial, where coeffs[0] = offset + * and coeffs[m_degree] is the coefficient applied to the highest degree input. + * \param[in] height The height of the input image. + * \param[in] width The width of the input image. + */ + inline vpTutoParabolaModel(const VISP_NAMESPACE_ADDRESSING vpColVector &coeffs, const unsigned int &height, const unsigned int &width) + : m_degree(coeffs.size() - 1) + , m_height(static_cast(height)) + , m_width(static_cast(width)) + , m_coeffs(coeffs) + { } + + /** + * \brief Construct a new vpTutoParabolaModel object + * + * \param[in] coeffs The coefficients of the polynomial, where coeffs[0][0] = offset + * and coeffs[m_degree][0] is the coefficient applied to the highest degree input. + * \param[in] height The height of the input image. + * \param[in] width The width of the input image. + */ + inline vpTutoParabolaModel(const VISP_NAMESPACE_ADDRESSING vpMatrix &coeffs, const unsigned int &height, const unsigned int &width) + : m_degree(coeffs.getRows() - 1) + , m_height(static_cast(height)) + , m_width(static_cast(width)) + , m_coeffs(coeffs.getCol(0)) + { } + + /** + * @brief Compute \f[v = \sum_{i = 0}^N a_i u^i \f] + * + * \param[in] u Input + * \return float The corresponding v. + */ + inline double eval(const double &u) const + { + double normalizedU = u / m_width; + double v = 0.; + for (unsigned int i = 0; i <= m_degree; ++i) { + v += m_coeffs[i] * std::pow(normalizedU, i); + } + v *= m_height; + return v; + } + + /** + * \brief Cast into a vpColVector + * + * \return coeffs a:=coeffs[0] b:=coeffs[1] c:=coeffs[2] + */ + inline VISP_NAMESPACE_ADDRESSING vpColVector toVpColVector() const + { + return m_coeffs; + } + + inline vpTutoParabolaModel &operator=(const vpTutoParabolaModel &other) + { + m_degree = other.m_degree; + m_height = other.m_height; + m_width = other.m_width; + m_coeffs = other.m_coeffs; + return *this; + } + + /** + * @brief Fill the matrices that form the linear system A X = b + * where A contains the different powers of the u-coordinates, + * X contains the model coefficients and b contains the + * v-coordinates. + * + * \param[in] degree The highest degree of the polynomial. + * \param[in] height The height of the input image. + * \param[in] width The width of the input image. + * \param[in] pts The points to use to interpolate the coefficients of the parabola. + * \param[out] A The matrix that contains the different powers of the u-coordinates. + * \param[out] b The matrix that contains the v-coordinates. + * \return Fill + */ + //! [Fill_LMS_system] + static void fillSystem(const unsigned int °ree, const double &height, const double &width, const std::vector &pts, VISP_NAMESPACE_ADDRESSING vpMatrix &A, VISP_NAMESPACE_ADDRESSING vpMatrix &b) + { + const unsigned int nbPts = pts.size(); + const unsigned int nbCoeffs = degree + 1; + std::vector normalizedPts; + // Normalization to avoid numerical instability + for (const auto &pt: pts) { + normalizedPts.push_back(VISP_NAMESPACE_ADDRESSING vpImagePoint(pt.get_i() / height, pt.get_j() / width)); + } + A.resize(nbPts, nbCoeffs, 1.); // Contains the u^i + b.resize(nbPts, 1); // Contains the v coordinates + for (unsigned int i = 0; i < nbPts; ++i) { + double u = normalizedPts[i].get_u(); + double v = normalizedPts[i].get_v(); + for (unsigned int j = 0; j < nbCoeffs; ++j) { + A[i][j] = std::pow(u, j); + } + b[i][0] = v; + } + } + //! [Fill_LMS_system] + + friend std::ostream &operator<<(std::ostream &os, const vpTutoParabolaModel &model) + { + os << "Highest degree = " << model.m_degree << std::endl; + os << "Coeffs = [ " << model.m_coeffs.transpose() << " ]" << std::endl; + return os; + } + +private: + unsigned int m_degree; /*!< The highest degree of the polynomial.*/ + double m_height; /*!< The height of the input image*/ + double m_width; /*!< The width of the input image*/ + VISP_NAMESPACE_ADDRESSING vpColVector m_coeffs; /*!< The coefficient of the polynomial, where m_coeffs[0] is the offset and m_coeffs[m_degree] is the coefficient applied to the highest degree.*/ +}; +} +#endif +#endif +#endif diff --git a/tutorial/particle-filter-curve-fitting/vpTutoSegmentation.cpp b/tutorial/particle-filter-curve-fitting/vpTutoSegmentation.cpp new file mode 100644 index 0000000000..d533494be3 --- /dev/null +++ b/tutorial/particle-filter-curve-fitting/vpTutoSegmentation.cpp @@ -0,0 +1,152 @@ +/* + * 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 "vpTutoSegmentation.h" + +#include + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +#ifndef DOXYGEN_SHOULD_SKIP_THIS +namespace tutorial +{ +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + +void performSegmentationHSV(vpTutoCommonData &data) +{ + const unsigned int height = data.m_I_orig.getHeight(), width = data.m_I_orig.getWidth(); + vpImage H(height, width); + vpImage S(height, width); + vpImage V(height, width); + vpImageConvert::RGBaToHSV(reinterpret_cast(data.m_I_orig.bitmap), + H.bitmap, + S.bitmap, + V.bitmap, + data.m_I_orig.getSize()); + + vpImageTools::inRange(H.bitmap, + S.bitmap, + V.bitmap, + data.m_hsv_values, + data.m_mask.bitmap, + data.m_mask.getSize()); + + vpImageTools::inMask(data.m_I_orig, data.m_mask, data.m_I_segmented); +} + +std::vector< VISP_NAMESPACE_ADDRESSING vpImagePoint > extractSkeleton(vpTutoCommonData &data) +{ + const int height = data.m_mask.getHeight(); + const int width = data.m_mask.getWidth(); + data.m_Iskeleton.resize(height, width, 0); + std::vector points; + // Edge thinning along the horizontal direction + for (int y = 0; y < height; ++y) { + int left = -1; + for (int x = 0; x < width - 1; ++x) { + if ((data.m_mask[y][x] > 0) && (data.m_mask[y][x + 1] > 0)) { + if (left < 0) { + left = x; + } + } + else if (data.m_mask[y][x] > 0) { + int cx = x; // Case 1 pix wide + if (left >= 0) { + // Case more than 1 pix wide + cx = static_cast(((left + x) - 1) * 0.5f); + } + vpImagePoint pt(y, cx); + points.push_back(pt); + data.m_Iskeleton[y][cx] = 255; + left = -1; + } + } + } + + // Edge thinning along the vertical direction + for (int x = 0; x < width; ++x) { + int top = -1; + for (int y = 0; y < height - 1; ++y) { + if ((data.m_mask[y][x] > 0) && (data.m_mask[y + 1][x] > 0)) { + if (top < 0) { + top = y; + } + } + else if (data.m_mask[y][x] > 0) { + int cy = y; // Case 1 pix wide + if (top >= 0) { + cy = static_cast(((top + y) - 1) * 0.5f); // Case more than 1 pix wide + } + if (data.m_Iskeleton[cy][x] == 0) { + vpImagePoint pt(cy, x); + points.push_back(pt); + data.m_Iskeleton[cy][x] = 255; + } + top = -1; + } + } + } + return points; +} + +std::vector< vpImagePoint > addSaltAndPepperNoise(const std::vector< vpImagePoint > &noisefreePts, vpTutoCommonData &data) +{ + const unsigned int nbNoiseFreePts = noisefreePts.size(); + const unsigned int nbPtsToAdd = data.m_ratioSaltPepperNoise * nbNoiseFreePts; + const double width = data.m_Iskeleton.getWidth(); + const double height = data.m_Iskeleton.getHeight(); + data.m_IskeletonNoisy = data.m_Iskeleton; + vpGaussRand rngX(0.1666, 0.5, vpTime::measureTimeMicros()); + vpGaussRand rngY(0.1666, 0.5, vpTime::measureTimeMicros() + 4224); + std::vector noisyPts = noisefreePts; + for (unsigned int i = 0; i < nbPtsToAdd + 1; ++i) { + double uNormalized = rngX(); + double vNormalized = rngY(); + // Clamp to interval[0, 1[ + uNormalized = std::max(uNormalized, 0.); + uNormalized = std::min(uNormalized, 0.99999); + vNormalized = std::max(vNormalized, 0.); + vNormalized = std::min(vNormalized, 0.99999); + // Scale to image size + double u = uNormalized * width; + double v = vNormalized * height; + // Create corresponding image point + vpImagePoint pt(v, u); + noisyPts.push_back(pt); + data.m_IskeletonNoisy[static_cast(v)][static_cast(u)] = 255; + } + return noisyPts; +} +} +#endif +#else +void dummy_vpTutoSegmentation() { } +#endif diff --git a/tutorial/particle-filter-curve-fitting/vpTutoSegmentation.h b/tutorial/particle-filter-curve-fitting/vpTutoSegmentation.h new file mode 100644 index 0000000000..ce489fa734 --- /dev/null +++ b/tutorial/particle-filter-curve-fitting/vpTutoSegmentation.h @@ -0,0 +1,72 @@ +/* + * 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. + */ +#ifndef VP_TUTO_SEGMENTATION_H +#define VP_TUTO_SEGMENTATION_H + +#include +#include +#include +#include +#include +#include + +#include "vpTutoCommonData.h" + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +#ifndef DOXYGEN_SHOULD_SKIP_THIS +namespace tutorial +{ +/** + * \brief Perform a segmentation of the original image based on the HSV color encoding. + * + * \param[in] data Data common to the whole program. + */ +void performSegmentationHSV(vpTutoCommonData &data); + +/** + * \brief Extract the skeleton of the segmented image. + * + * \param[in] data Data common to the whole program. + * \return std::vector< VISP_NAMESPACE_ADDRESSING vpImagePoint > List of the noise-free skeletonized image points. + */ +std::vector< VISP_NAMESPACE_ADDRESSING vpImagePoint > extractSkeleton(vpTutoCommonData &data); + +/** + * \brief Add salt and pepper noise to the skeletonized image. + * + * \param[in] noisefreePts List of the noise-free skeletonized image points. + * \param[in] data Data common to the whole program. + * \return std::vector< VISP_NAMESPACE_ADDRESSING vpImagePoint > List of the noise-free points + additional noisy points. + */ +std::vector< VISP_NAMESPACE_ADDRESSING vpImagePoint > addSaltAndPepperNoise(const std::vector< VISP_NAMESPACE_ADDRESSING vpImagePoint > &noisefreePts, vpTutoCommonData &data); +} +#endif +#endif +#endif diff --git a/tutorial/particle-filter/CMakeLists.txt b/tutorial/particle-filter/CMakeLists.txt new file mode 100644 index 0000000000..51d0108929 --- /dev/null +++ b/tutorial/particle-filter/CMakeLists.txt @@ -0,0 +1,51 @@ +############################################################################# +# +# 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. +# +# Description: +# ViSP overall configuration file. +# +############################################################################# + +cmake_minimum_required(VERSION 3.5) + +project(tutorial-pf) + +find_package(VISP REQUIRED visp_core visp_gui) + +set(tutorial_cpp) + +list(APPEND tutorial_cpp tutorial-pf.cpp) + +foreach(cpp ${tutorial_cpp}) + visp_add_target(${cpp}) + if(COMMAND visp_add_dependency) + visp_add_dependency(${cpp} "tutorials") + endif() +endforeach() diff --git a/tutorial/particle-filter/tutorial-pf.cpp b/tutorial/particle-filter/tutorial-pf.cpp new file mode 100644 index 0000000000..737ea9e21b --- /dev/null +++ b/tutorial/particle-filter/tutorial-pf.cpp @@ -0,0 +1,853 @@ +/* + * + * 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. + * +*/ + +/*! \example tutorial-pf.cpp + Tutorial on how to use the Particle Filter (PF) on a complex non-linear use-case. + The system is an object, whose coordinate frame origin is the point O, on which are sticked four markers. + The object revolves in a plane parallel to the ground around a fixed point W whose coordinate frame is the world frame. + The scene is observed by a pinhole camera whose coordinate frame has the origin C and which is + fixed to the ceiling. + + The state vector of the PF is: + \f[ + \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} + \f] + + The measurement \f$ \textbf{z} \f$ corresponds to the coordinates in pixels of the different markers. + 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}[2i] &=& u_i \\ + \textbf{z}[2i+1] &=& v_i + \end{array} + \f] + + Some noise is added to the measurement vector to simulate measurements which are + not perfect. +*/ + +// ViSP includes +#include +#include +#include +#include +#include +#include +//! [Display_includes] +#ifdef VISP_HAVE_DISPLAY +#include +#include +#endif +//! [Display_includes] +#include + +//! [UKF_includes] +#include +#include +//! [UKF_includes] + +//! [PF_includes] +#include +//! [PF_includes] + +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +//! [Process_function] +/** + * \brief Process function that makes evolve the state model {\f$ {}^WX_x \f$, \f$ {}^WX_y \f$, \f$ {}^WX_z \f$, \f$ C = \omega \Delta t \f$} + * over time. + * + * \param[in] x The state vector + * \return vpColVector The state vector at the next iteration. + */ +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; +} +//! [Process_function] + +//! [Pose_for_display] +/** + * \brief Compute the pose from the 3D coordinates of the markers and their coordinates in pixels + * in the image. + * + * \param[in] point The 3D coordinates of the markers in the object frame. + * \param[in] ip The pixel coordinates of the markers in the image. + * \param[in] cam The camera parameters used to acquire the image. + * \return vpHomogeneousMatrix The pose of the object in the camera frame. + */ +vpHomogeneousMatrix computePose(std::vector &point, const std::vector &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; +} +//! [Pose_for_display] + +//! [Object_simulator] +/** + * \brief Class that simulates the moving object. + * Random noise is added in order to make vary the velocity of the object. + */ +class vpObjectSimulator +{ +public: + /** + * \brief Construct a new vpObjectSimulator object. + * + * \param[in] R The radius of the revolution around the world frame origin. + * \param[in] w The pulsation of the motion. + * \param[in] phi The phase of the motion. + * \param[in] wZ The y-coordinate of the object in the world frame. + * \param[in] stdevRng The standard deviation of the noise generator that will make vary the velocity of the object. + */ + 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.) + { } + + /** + * \brief Move the object to its new position, expressed in the world frame. + * + * \param[in] t The current time. + * \return vpColVector The new position of the object in the world frame, expressed as homogeneous coordinates. + */ + 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; //!< Radius of the revolution around the world frame origin. + double m_w; //!< Pulsation of the motion. + double m_phi; //!< Phase of the motion. + const double m_wZ; //!< The z-coordinate of the object in the world frame. + vpGaussRand m_rng; //!< Noise generator to make vary the velocity of the object. +}; +//! [Object_simulator] + +//! [Markers_class] +/** + * \brief Class that permits to convert the 3D position of the object into measurements. + */ +class vpMarkersMeasurements +{ +public: + /** + * \brief Construct a new vpMarkersMeasurements object. + * + * \param[in] cam The camera parameters. + * \param[in] cMw The pose of the world frame with regard to the camera frame. + * \param[in] wRo The rotation matrix expressing the rotation between the world frame and object frame. + * \param[in] markers The position of the markers in the object frame. + * \param[in] noise_stdev The standard deviation for the noise generator + * \param[in] seed The seed for the noise generator + * \param[in] likelihood_stdev The standard deviation for the likelihood computation. A particle that is + * 3. * likelihood_stdev further than the measurements will have a weight of 0. + */ + vpMarkersMeasurements(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMw, const vpRotationMatrix &wRo, + const std::vector &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(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])); + } + } + + //! [Measurement_function] + /** + * \brief Convert the prior of the UKF into the measurement space. + * + * \param[in] x The prior. + * \return vpColVector The prior expressed in the measurement space. + */ + vpColVector state_to_measurement(const vpColVector &x) + { + unsigned int nbMarkers = static_cast(m_markers.size()); + vpColVector meas(2*nbMarkers); + vpHomogeneousMatrix wMo; + vpTranslationVector wTo(x[0], x[1], x[2]); + wMo.build(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; + } + //! [Measurement_function] + + //! [GT_measurements] + /** + * \brief Perfect measurement of the projection of the markers in the image when the object + * is located at \b wX. + * + * \param[in] wX The actual position of the robot (wX[0]: x, wX[1]: y, wX[2] = z). + * \return vpColVector [2*i] u_i [2*i + 1] v_i where i is the index of the marker. + */ + vpColVector measureGT(const vpColVector &wX) + { + unsigned int nbMarkers = static_cast(m_markers.size()); + vpColVector meas(2*nbMarkers); + vpHomogeneousMatrix wMo; + vpTranslationVector wTo(wX[0], wX[1], wX[2]); + wMo.build(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; + } + //! [GT_measurements] + + //! [Noisy_measurements] + /** + * \brief Noisy measurement of the projection of the markers in the image when the object + * is located at \b wX. + * + * \param[in] wX The actual position of the robot (wX[0]: x, wX[1]: y, wX[2] = z). + * \return vpColVector [2*i] u_i [2*i + 1] v_i where i is the index of the marker. + */ + 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; + } + //! [Noisy_measurements] + + //! [Likelihood_function] + /** + * \brief Compute the likelihood of a particle compared to the measurements. + * The likelihood equals zero if the particle is completely different of + * the measurements and equals one if it matches completely. + * The chosen likelihood is 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. + * + * \param[in] x The particle. + * \param[in] meas The measurement vector. meas[2i] = u_i meas[2i + 1] = v_i . + * \return double The likelihood of the particle. + */ + double likelihood(const vpColVector &x, const vpColVector &meas) + { + unsigned int nbMarkers = static_cast(m_markers.size()); + double likelihood = 0.; + vpHomogeneousMatrix wMo; + vpTranslationVector wTo(x[0], x[1], x[2]); + wMo.build(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(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; + } + //! [Likelihood_function] +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 m_markers; // The position of the markers in the object frame. + std::vector 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. +}; +//! [Markers_class] + +//! [CLI] +struct SoftwareArguments +{ + // --- Main loop parameters--- + static const int SOFTWARE_CONTINUE = 42; + bool m_useDisplay; //!< If true, activate the plot and the renderer if VISP_HAVE_DISPLAY is defined. + unsigned int m_nbStepsWarmUp; //!< Number of steps for the warmup phase. + unsigned int m_nbSteps; //!< Number of steps for the main loop. + // --- PF parameters--- + 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 0.. + double m_ampliMaxX; //!< Amplitude max of the noise for the state component corresponding to the X coordinate. + double m_ampliMaxY; //!< Amplitude max of the noise for the state component corresponding to the Y coordinate. + double m_ampliMaxZ; //!< Amplitude max of the noise for the state component corresponding to the Z coordinate. + double m_ampliMaxOmega; //!< Amplitude max of the noise for the state component corresponding to the pulsation. + long m_seedPF; //!< Seed for the random generators of the PF. + int m_nbThreads; //!< Number of thread to use in the Particle Filter. + + 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 ] [--nb-steps-warmup ]" << std::endl; + std::cout << " [--max-distance-likelihood ] [-N, --nb-particles ] [--seed ] [--nb-threads ]" << std::endl; + std::cout << " [--ampli-max-X ] [--ampli-max-Y ] [--ampli-max-Z ] [--ampli-max-omega ]" << 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; + } +}; +//! [CLI] + +int main(const int argc, const char *argv[]) +{ + SoftwareArguments args; + int returnCode = args.parseArgs(argc, argv); + if (returnCode != SoftwareArguments::SOFTWARE_CONTINUE) { + return returnCode; + } + + //! [Constants_for_simulation] + 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 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(markers.size()); + std::vector 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]; + //! [Constants_for_simulation] + + //! [Camera_for_measurements] + // 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); + //! [Camera_for_measurements] + + //! [Initial_estimates] + 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 + X0[2] = 0.95 * wZ; // Wrong estimation of the position along the Z-axis: error of 5% + X0[3] = 0.95 * w * dt; // Wrong estimation of the pulsation: error of 25% + //! [Initial_estimates] + + //! [Constants_for_the_PF] + 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 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 = vpTime::measureTimeMicros(); + } + //! [Constants_for_the_PF] + + // Object that converts the pose of the object into measurements + vpMarkersMeasurements markerMeas(cam, cMw, wRo, markers, sigmaMeasurements, seed, sigmaLikelihood); + + //! [Init_functions_pf] + vpParticleFilter::vpProcessFunction processFunc = fx; + using std::placeholders::_1; + using std::placeholders::_2; + vpParticleFilter::vpLikelihoodFunction likelihoodFunc = std::bind(&vpMarkersMeasurements::likelihood, &markerMeas, _1, _2); + vpParticleFilter::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter::simpleResamplingCheck; + vpParticleFilter::vpResamplingFunction resamplingFunc = vpParticleFilter::simpleImportanceResampling; + //! [Init_functions_pf] + + //! [Init_PF] + // Initialize the PF + vpParticleFilter pfFilter(nbParticles, stdevsPF, seedPF, nbThread); + 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 + 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 + //! [Init_plot] + + //! [Init_renderer] + // 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 Idisp(700, 700, vpRGBa(255)); + std::shared_ptr d = vpDisplayFactory::createDisplay(Idisp, 800, -1, "Projection of the markers"); +#endif + //! [Init_renderer] + + //! [Init_simu] + // Initialize the simulation + vpObjectSimulator object(radius, w, phi, wZ, ampliMaxOmega / 3.); + vpColVector object_pos(4, 0.); + object_pos[3] = 1.; + //! [Init_simu] + + //! [Simu_loop] + for (unsigned int i = 0; i < nbIter; ++i) { + double t = dt * static_cast(i); + std::cout << "[Timestep" << i << ", t = " << t << "]" << std::endl; + //! [Update obj pose] + // Update object pose + object_pos = object.move(t); + //! [Update obj pose] + + //! [Update_measurement] + // Perform the measurement + vpColVector z = markerMeas.measureWithNoise(object_pos); + //! [Update_measurement] + + //! [PF_filtering] + /// Use the PF to filter the measurement + double tPF = vpTime::measureTimeMs(); + pfFilter.filter(z, dt); + double dtPF = vpTime::measureTimeMs() - tPF; + //! [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 + //! [Noisy_pose] + // Prepare the pose computation: + // the image points corresponding to the noisy markers are needed + std::vector 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]; + //! [Noisy_pose] + + //! [Update_plot] + // Plot the ground truth + plot.plot(0, 0, object_pos[0], object_pos[1]); + + // Plot the PF filtered state + vpColVector XestPF = pfFilter.computeFilteredState(); + 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); + + 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; + + 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; + + // 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()); + //! [Update_plot] + + //! [Update_renderer] + // 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); + //! [Update_renderer] +#endif + //! [Update_displays] + } + //! [Simu_loop] + std::cout << "Press Enter to quit..." << std::endl; + std::cin.get(); + + 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 diff --git a/tutorial/robot/flir-ptu/tutorial-flir-ptu-ibvs.cpp b/tutorial/robot/flir-ptu/tutorial-flir-ptu-ibvs.cpp index b4d2699d27..13e89d0015 100644 --- a/tutorial/robot/flir-ptu/tutorial-flir-ptu-ibvs.cpp +++ b/tutorial/robot/flir-ptu/tutorial-flir-ptu-ibvs.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -32,8 +31,7 @@ * tests the control law * eye-in-hand control * velocity computed in the camera frame - * -*****************************************************************************/ + */ /*! \example tutorial-flir-ptu-ibvs.cpp