diff --git a/.github/workflows/ubuntu-dep-src.yml b/.github/workflows/ubuntu-dep-src.yml index aa382ffd69..4f85386dcf 100644 --- a/.github/workflows/ubuntu-dep-src.yml +++ b/.github/workflows/ubuntu-dep-src.yml @@ -68,8 +68,12 @@ jobs: echo $OpenBLAS_HOME - name: Build VTK from source + # We set GIT_CLONE_PROTECTION_ACTIVE=false to avoid the issue described in + # https://github.com/git-lfs/git-lfs/issues/5749 run: | pwd + echo "GIT_CLONE_PROTECTION_ACTIVE=false" >> $GITHUB_ENV + export GIT_CLONE_PROTECTION_ACTIVE=false git clone --recursive --depth 1 https://github.com/Kitware/VTK.git ${HOME}/VTK cd ${HOME}/VTK mkdir build && cd build && mkdir install diff --git a/doc/biblio/references.bib b/doc/biblio/references.bib index ca3e3af548..13c3f81ca0 100644 --- a/doc/biblio/references.bib +++ b/doc/biblio/references.bib @@ -689,3 +689,13 @@ @InProceedings{Oliva22c Month = {December}, Year = {2022} } + +@article{Merwe00, + author = {Wan, Eric A., and Rudolph Van Der Merwe}, + url = {https://groups.seas.harvard.edu/courses/cs281/papers/unscented.pdf}, + title = {The unscented Kalman filter for nonlinear estimation.}, + booktitle = {Proceedings of the IEEE adaptive systems for signal processing, communications, and control symposium}, + pages = {153--158}, + year = 2000, + month = oct +} diff --git a/doc/image/tutorial/misc/img-tutorial-ukf-illustration.jpg b/doc/image/tutorial/misc/img-tutorial-ukf-illustration.jpg new file mode 100644 index 0000000000..9a0e004909 Binary files /dev/null and b/doc/image/tutorial/misc/img-tutorial-ukf-illustration.jpg differ diff --git a/doc/image/tutorial/misc/img-tutorial-ukf-run.jpg b/doc/image/tutorial/misc/img-tutorial-ukf-run.jpg new file mode 100644 index 0000000000..f6169f79d6 Binary files /dev/null and b/doc/image/tutorial/misc/img-tutorial-ukf-run.jpg differ diff --git a/doc/tutorial/misc/tutorial-ukf.dox b/doc/tutorial/misc/tutorial-ukf.dox new file mode 100644 index 0000000000..6ade91180e --- /dev/null +++ b/doc/tutorial/misc/tutorial-ukf.dox @@ -0,0 +1,412 @@ +/** + \page tutorial-ukf Tutorial: Using Unscented Kalman Filter to filter your data + \tableofcontents + +\section tuto-ukf-intro Introduction + +The Unscented Kalman Filter (UKF) is a version of the Kalman filter that handles non-linearities. + +In this tutorial, we will use a UKF 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{eqnarray*}{ + {}^W \textbf{X}_x &=& R cos(\omega t + \phi) \\ + {}^W \textbf{X}_y &=& R sin(\omega t + \phi) \\ + {}^W \textbf{X}_z &=& constant +\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-ukf-intro-methods The maths beyond the Unscented Kalman Filter + +The maths beyond the Unscented Kalman Filter are explained in the documentation of the vpUnscentedKalman class. +We will recall briefly the important steps of the UKF. + +Be \f$ \textbf{x} \in {R}^n \f$ the internal state of the UKF and \f$ \textbf{P} \in {R}^{n\text{ x }n} \f$ the +associated covariance matrix. + +The first step of the UKF is the prediction step. During this step, some particular points called sigma points, denoted +\f$ \chi \f$, are drawn along with associated weights \f$ \textbf{w}^m \f$ for the computation of the mean and +\f$ \textbf{w}^c \f$ for the computation of the covariance: + +\f{eqnarray*}{ + \chi &=& sigma-function(\textbf{x}, \textbf{P}) \\ + \textbf{w}^m, \textbf{w}^c &=& weight-function(n, parameters) +\f} + +There are different ways of drawing the sigma points and associated weights in the litterature, such as the one +proposed by Julier or the one proposed by E. A. Wan and R. van der Merwe in \cite Merwe00. + +Be \f$ \textbf{u} \f$ the vector containing the known commands sent to the system, if any. Then, we pass each sigma point through the process function \f$ f(\chi, \Delta t) \f$, the command function +\f$b( \textbf{u}, \Delta t )\f$ and the command function depending on the state \f$bx( \textbf{u}, \chi, \Delta t )\f$ +to project them forward in time, forming the new prior: + +\f$ {Y} = f( \chi , \Delta t ) + b( \textbf{u}, \Delta t ) + bx( \textbf{u}, \chi, \Delta t ) \f$ + +Finally, we apply the Unscented Transform to compute the mean \f$ \boldsymbol{\mu} \f$ +and covariance \f$ \overline{\textbf{P}} \f$ of the prior: + +\f{eqnarray*}{ + \boldsymbol{\mu}, \overline{\textbf{P}} &=& UT({Y}, \textbf{w}^m, \textbf{w}^c, \textbf{Q}) \\ + \boldsymbol{\mu} &=& \sum_{i=0}^{2n} w_i^m {Y}_i \\ + \overline{\textbf{P}} &=& \sum_{i=0}^{2n} ( w_i^c ({Y}_i - \boldsymbol{\mu}) ({Y}_i - \boldsymbol{\mu})^T ) + \textbf{Q} + \f} + +where \f$ \textbf{Q} \f$ is the covariance of the error introduced by the process function. + +The second step of the UKF is to update the internal state based on new measurements. It is performed in the measurement space, so we must convert the sigma points of +the prior into measurements using the measurement function \f$ h: {R}^n \rightarrow {R}^m \f$: + +\f$ {Z} = h({Y}) \f$ + +Then, we use once again the Unscented Transform to compute the mean \f$ \boldsymbol{\mu}_z \in {R}^m \f$ and the +covariance \f$ \textbf{P}_z \in {R}^{m\text{ x }m} \f$ of these points: + +\f{eqnarray*}{ + \boldsymbol{\mu}_z, \textbf{P}_z &=& UT({Z}, \textbf{w}^m, \textbf{w}^c, \textbf{R}) \\ + \boldsymbol{\mu}_z &=& \sum_{i=0}^{2n} w_i^m {Z}_i \\ + \textbf{P}_z &=& \sum_{i=0}^{2n} ( w_i^c ({Z}_i - \boldsymbol{\mu}_z) ({Z}_i - \boldsymbol{\mu}_z)^T ) + \textbf{R} +\f} + +where \f$ \textbf{R} \f$ is the measurement covariance matrix. + +Then, we compute the residual \f$ \textbf{y} \f$ of the measurement \f$ \textbf{z} \f$: + +\f$ \textbf{y} = \textbf{z} - \boldsymbol{\mu}_z \f$ + +To compute the Kalman's gain, we first need to compute the cross covariance of the state and the measurements: + +\f$ \textbf{P}_{xy} = \sum_{i=0}^{2n} w_i^c ({Y}_i - \boldsymbol{\mu})({Z}_i - \boldsymbol{\mu}_z)^T \f$ + +The Kalman's gain is then defined as: + +\f$ \textbf{K} = \textbf{P}_{xz} \textbf{P}_z^{-1} \f$ + +Finally, we can compute the new state estimate \f$ \textbf{x} \f$ and the new covariance \f$ \textbf{P} \f$: + +\f{eqnarray*}{ + \textbf{x} &=& \boldsymbol{\mu} + \textbf{K} \textbf{y} \\ + \textbf{P} &=& \overline{\textbf{P}} - \textbf{K} \textbf{P}_z \textbf{K}^T +\f} + +\section tuto-ukf-tutorial Explanations about the tutorial + +\subsection tuto-ukf-tutorial-howtorun How to run the tutorial + +To run the tutorial, please run the following commands: + +``` +$ cd $VISP_WS/visp-build/tutorial/kalman +$ ./tutorial-ukf +``` + +The program does not take any argument. You should see something similar to the following image: + +\htmlonly +\endhtmlonly +\image html img-tutorial-ukf-run.jpg "Screenshot of the tutorial Graphical User Interface" + +Press `Return` to leave the program. + +\subsection tuto-ukf-tutorial-explained Detailed explanations about the UKF tutorial + +For this tutorial, we use the main program tutorial-ukf.cpp . The internal state of 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{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 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{eqnarray*}{ + \textbf{z}[2i] &=& u_i \\ + \textbf{z}[2i+1] &=& v_i +\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-ukf-tutorial-explained-includes Details on the includes + +To have a Graphical User Interface (GUI), we include the following files. + +\snippet tutorial-ukf.cpp Display_includes + +To be able to use the UKF, we use the following includes: + +\snippet tutorial-ukf.cpp UKF_includes + +\subsubsection tuto-ukf-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-ukf.cpp Object_simulator + +\subsubsection tuto-ukf-tutorial-explained-fx Details on the process function + +As mentionned in \ref tuto-ukf-intro-methods, the UKF relies on a process function which project in time the internal state of the UKF. + +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-ukf-intro, the equations of motion of the object are the following: + +\f{eqnarray*}{ + {}^W X_x(t) &=& R cos(\omega t + \phi) \\ + {}^W X_y(t) &=& R sin(\omega t + \phi) \\ + {}^W X_z(t) &=& constant +\f} + +Thus, we have: + +\f{eqnarray*}{ + {}^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 +\f} + +Which can be rewritten: +\f{eqnarray*}{ + {}^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 +\f} + +And can be finally written as: +\f{eqnarray*}{ + {}^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 +\f} + +This motivates us to choose the following non-linear process function: + +\f{eqnarray*}{ + \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} +\f} + +As the process function is pretty simple, a simple function called here `fx()` is enough: + +\snippet tutorial-ukf.cpp Process_function + +\subsubsection tuto-ukf-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-ukf.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. + +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-ukf.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-ukf.cpp GT_measurements + +The method measureWithNoise adds noise to the ground truth measurements in order to simulate a noisy +measurement process: + +\snippet tutorial-ukf.cpp Noisy_measurements + +\subsubsection tuto-ukf-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 UKF with regard to the 3D position we would have if we computed the pose directly +from the noisy measurements. + +\snippet tutorial-ukf.cpp Pose_for_display + +\subsubsection tuto-ukf-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-ukf.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-ukf.cpp Camera_for_measurements + +\subsubsection tuto-ukf-tutorial-explained-initukf Details on the initialization of the UKF + +To initialize the UKF, we need an object that will be able to compute the sigma points and their associated weights. To +do so, we must create an instance of a class inheriting from the class vpUKSigmaDrawerAbstract. In our case, we decided +to use the method proposed by E. A. Wan and R. van der Merwe in \cite Merwe00 and implemented in the vpUKSigmaDrawerMerwe class: + +\snippet tutorial-ukf.cpp Sigma_points_drawer + +The first parameter is the dimension of the state of the UKF. The second parameter is \f$ \alpha \f$: the greater it is +the further of the mean the sigma points are; it is a real and its value must be between 0 and 1. The third parameter is +\f$ \beta \f$: it is a real whose value is set to two for Gaussian problems. Finally, the last parameter is \f$ \kappa \f$: +it is a real whose value must be set to \f$ 3 - n \f$ for most problems. + +The UKF needs the covariance matrix of the measurements \f$ \textbf{R} \f$ that represents the uncertainty of the +measurements: + +\snippet tutorial-ukf.cpp Covariance_measurements + +The UKF needs the covariance matrix of the process \f$ \textbf{Q} \f$ that represents the uncertainty induced during the +prediction step: + +\snippet tutorial-ukf.cpp Covariance_process + +The UKF needs an estimate of the intial state \f$ \textbf{x}_0 \f$ and of its covariance \f$ \textbf{P}_0 \f$: + +\snippet tutorial-ukf.cpp Initial_estimates + +Next, we initialize the process function and the measurement function: + +\snippet tutorial-ukf.cpp Init_functions + +Finally, we create the UKF and initialize its state: + +\snippet tutorial-ukf.cpp Init_UKF + +If the internal state cannot use the standard addition and substraction, it would be necessary to write other methods: +- a method permitting to add two state vectors (see vpUnscentedKalman::setStateAddFunction), +- a method permitting to substract two state vectors (see vpUnscentedKalman::setStateResidualFunction), +- and a method permitting to compute the mean of several state vectors (see vpUnscentedKalman::setStateMeanFunction). + +If some commands are known to have an effect on the internal state, it would be necessary to write other methods: +- a method for the effects of the commands on the state, without knowing the state (see vpUnscentedKalman::setCommandOnlyFunction), +- and/or a method for the effects of the commands on the state, knowing the state (see vpUnscentedKalman::setCommandStateFunction). + +If the measurement space cannot use the standard addition and substraction, it would be necessary to write other methods: +- a method permitting to substract two measurement vectors (see vpUnscentedKalman::setMeasurementResidualFunction), +- and a method permitting to compute the mean of several measurement vectors (see vpUnscentedKalman::setMeasurementMeanFunction). + +\subsubsection tuto-ukf-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: + +\snippet tutorial-ukf.cpp Init_plot + +Then, we initialize the simple renderer that displays what the camera sees: + +\snippet tutorial-ukf.cpp Init_renderer + +\subsubsection tuto-ukf-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 state of the UKF is expressed, as a null homogeneous coordinates +vector. + +\snippet tutorial-ukf.cpp Init_simu + +\subsubsection tuto-ukf-tutorial-explained-loop Details on the loop + +The main loop of the program is the following: + +\snippet tutorial-ukf.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-ukf.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-ukf.cpp Update_measurement + +Then, we use the Unscented Kalman Filter to filter the noisy measurements: + +\snippet tutorial-ukf.cpp Perform_filtering + +Finally, we update the plot and renderer: + +\snippet tutorial-ukf.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-ukf.cpp Noisy_pose + +Then, we update the plot by plotting the new ground truth, filtered and noisy 3D positions: + +\snippet tutorial-ukf.cpp Update_plot + +Finally, we update the renderer that displays the projection in the image of the markers: + +\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: + +\snippet tutorial-ukf.cpp Delete_renderer + +The program stops once the `Return` key is pressed. +*/ diff --git a/doc/tutorial/python/tutorial-install-python-bindings.dox b/doc/tutorial/python/tutorial-install-python-bindings.dox index 36f1006b4c..c9ab382832 100644 --- a/doc/tutorial/python/tutorial-install-python-bindings.dox +++ b/doc/tutorial/python/tutorial-install-python-bindings.dox @@ -672,26 +672,50 @@ If you have an issue that looks like: from /usr/include/c++/11/bits/specfun.h:45, from /usr/include/c++/11/cmath:1935, from /usr/include/c++/11/math.h:36, - from /home/sfelton/miniconda3/envs/wrapper3.9/include/python3.9/pyport.h:205, - from /home/sfelton/miniconda3/envs/wrapper3.9/include/python3.9/Python.h:50, - from /home/sfelton/.local/include/pybind11/detail/common.h:266, - from /home/sfelton/.local/include/pybind11/attr.h:13, - from /home/sfelton/.local/include/pybind11/detail/class.h:12, - from /home/sfelton/.local/include/pybind11/pybind11.h:13, - from /home/sfelton/software/visp_build/modules/python/bindings/src/robot.cpp:3: + from /home/user/miniconda3/envs/wrapper3.9/include/python3.9/pyport.h:205, + from /home/user/miniconda3/envs/wrapper3.9/include/python3.9/Python.h:50, + from /home/user/.local/include/pybind11/detail/common.h:266, + from /home/user/.local/include/pybind11/attr.h:13, + from /home/user/.local/include/pybind11/detail/class.h:12, + from /home/user/.local/include/pybind11/pybind11.h:13, + from /home/user/visp_ws/visp_build/modules/python/bindings/src/robot.cpp:3: /usr/include/c++/11/type_traits: **In instantiation of ‘struct std::is_move_constructible >’:** /usr/include/c++/11/type_traits:152:12: required from ‘struct std::__and_ >, std::is_move_assignable > >’ /usr/include/c++/11/type_traits:157:12: required from ‘struct std::__and_ > >, std::is_move_constructible >, std::is_move_assignable > >’ /usr/include/c++/11/type_traits:2209:11: required by substitution of ‘template using _Require = std::__enable_if_t >::value> [with _Cond = {std::__not_ > >, std::is_move_constructible >, std::is_move_assignable >}]’ /usr/include/c++/11/bits/move.h:196:5: required by substitution of ‘template std::_Require >, std::is_move_constructible<_Tp>, std::is_move_assignable<_Tp> > std::swap(_Tp&, _Tp&) [with _Tp = vpImage]’ - /home/sfelton/software/visp-sfelton/modules/core/include/visp3/core/vpImage.h:341:15: required from ‘class vpImage’ - /home/sfelton/software/visp-sfelton/modules/core/include/visp3/core/vpImage.h:369:17: required from here + /home/user/visp_ws/visp/modules/core/include/visp3/core/vpImage.h:341:15: required from ‘class vpImage’ + /home/user/visp_ws/visp/modules/core/include/visp3/core/vpImage.h:369:17: required from here /usr/include/c++/11/type_traits:1010:52: error: static assertion failed: template argument must be a complete class or an unbounded array 1010 | **static_assert(std::__is_complete_or_unbounded(__type_identity<_Tp>{}),** \endverbatim You should delete the files in `modules/python/` of the **build** directory. + +If you have an error that looks like this: + +\verbatim +/home/user/miniconda3/envs/visp-python11/include/pybind11/cast.h: In instantiation of ‘pybind11::arg_v::arg_v(pybind11::arg&&, T&&, const char*) [with T = vpColVector (&)(const vpColVector&, const vpColVector&)]’: +/home/user/miniconda3/envs/visp-python11/include/pybind11/cast.h:1452:53: required from ‘pybind11::arg_v::arg_v(const char*, T&&, const char*) [with T = vpColVector (&)(const vpColVector&, const vpColVector&)]’ +/home/user/visp_ws/visp-build/modules/python/bindings/src/core.cpp:20491:347: required from here +/home/user/miniconda3/envs/visp-python11/include/pybind11/cast.h:1432:82: error: call of overloaded ‘cast(vpColVector (&)(const vpColVector&, const vpColVector&), pybind11::return_value_policy, )’ is ambiguous + 1432 | : arg(base), value(reinterpret_steal(detail::make_caster::cast( + | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~^ + 1433 | std::forward(x), return_value_policy::automatic, {}))), + | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +In file included from /home/user/miniconda3/envs/visp-python11/include/pybind11/cast.h:15, + from /home/user/miniconda3/envs/visp-python11/include/pybind11/attr.h:14, + from /home/user/miniconda3/envs/visp-python11/include/pybind11/detail/class.h:12, + from /home/user/miniconda3/envs/visp-python11/include/pybind11/pybind11.h:13, + from /home/user/visp_ws/visp_ukf/modules/python/bindings/src/core.cpp:3: +\endverbatim + +This is due to an argument that cannot have a default value. In the example above, this argument is **vpColVector (&)(const vpColVector&, const vpColVector&)** (converted from an std::function). +To solve this, go to `modules/python/generator/visp_python_bindgen/generator_config.py`. Then, add a regular expression in the **FORBIDDEN_DEFAULT_ARGUMENT_TYPES_REGEXS** list to match with this type. +Here, we would add '^std::function' to exclude the std::function from being constructed with a default value. + + \subsubsection py_bindings_known_errors_import_dll ImportError: DLL load failed while importing _visp The following error may occur on Windows diff --git a/doc/tutorial/tutorial-users.dox b/doc/tutorial/tutorial-users.dox index 8fd25a3367..fb1db6ceb5 100644 --- a/doc/tutorial/tutorial-users.dox +++ b/doc/tutorial/tutorial-users.dox @@ -186,6 +186,7 @@ This page introduces the user to other tools that may be useful. - \subpage tutorial-json
This tutorial explains how to read and save data in the portable JSON format. It focuses on saving the data generated by a visual servoing experiment and exporting it to Python in order to generate plots. - \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-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/CMakeLists.txt b/example/CMakeLists.txt index 884f6bae92..80b6f569d9 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -70,6 +70,7 @@ visp_add_subdirectory(device/light REQUIRED_DEPS visp_core visp_robo visp_add_subdirectory(direct-visual-servoing REQUIRED_DEPS visp_core visp_robot visp_visual_features visp_io visp_gui) visp_add_subdirectory(homography REQUIRED_DEPS visp_core visp_vision visp_io) visp_add_subdirectory(image REQUIRED_DEPS visp_core visp_io) +visp_add_subdirectory(kalman REQUIRED_DEPS visp_core visp_gui) visp_add_subdirectory(manual REQUIRED_DEPS visp_core visp_sensor visp_vs visp_robot visp_ar visp_vision visp_io visp_gui) visp_add_subdirectory(math REQUIRED_DEPS visp_core visp_io) visp_add_subdirectory(moments/image REQUIRED_DEPS visp_core visp_vs visp_robot visp_gui) diff --git a/example/kalman/CMakeLists.txt b/example/kalman/CMakeLists.txt new file mode 100644 index 0000000000..ccfb268df9 --- /dev/null +++ b/example/kalman/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.5) + +project(example-kalman) + +find_package(VISP REQUIRED visp_core visp_gui) + +set(example_cpp) + +list(APPEND example_cpp ukf-linear-example.cpp) +list(APPEND example_cpp ukf-nonlinear-example.cpp) +list(APPEND example_cpp ukf-nonlinear-complex-example.cpp) + +foreach(cpp ${example_cpp}) + visp_add_target(${cpp}) + if(COMMAND visp_add_dependency) + visp_add_dependency(${cpp} "examples") + endif() +endforeach() diff --git a/example/kalman/ukf-linear-example.cpp b/example/kalman/ukf-linear-example.cpp new file mode 100644 index 0000000000..7ff71e57b9 --- /dev/null +++ b/example/kalman/ukf-linear-example.cpp @@ -0,0 +1,229 @@ +/**************************************************************************** + * + * 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 ukf-linear-example.cpp + * Example of a simple linear use-case of the Unscented Kalman Filter (UKF). Using a UKF + * in this case is not necessary, it is done for learning purpous only. + * + * The system we are interested in is a system moving on a 2D-plane. + * + * The state vector of the UKF is: + * \f{eqnarray*}{ + \textbf{x}[0] &=& x \\ + \textbf{x}[1] &=& \dot{x} \\ + \textbf{x}[1] &=& y \\ + \textbf{x}[2] &=& \dot{y} + \f} + + * The measurement \f$ \textbf{z} \f$ corresponds to the position along the x-axis + * and y-axis. The measurement vector can be written as: + * \f{eqnarray*}{ + \textbf{z}[0] &=& x \\ + \textbf{z}[1] &=& y + \f} + + * Some noise is added to the measurement vector to simulate a sensor which is + * not perfect. +*/ + +// UKF includes +#include +#include + +// ViSP includes +#include +#include +#ifdef VISP_HAVE_DISPLAY +#include +#endif + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +/** + * \brief The process function, that updates the prior. + * + * \param[in] chi A sigma point. + * \param[in] dt The period. + * \return vpColVector The sigma point projected in the future. + */ +vpColVector fx(const vpColVector &chi, const double &dt) +{ + vpColVector point(4); + point[0] = chi[1] * dt + chi[0]; + point[1] = chi[1]; + point[2] = chi[3] * dt + chi[2]; + point[3] = chi[3]; + return point; +} + +/** + * \brief The measurement function, that project the prior in the measurement space. + * + * \param[in] chi The prior. + * \return vpColVector The prior projected in the measurement space. + */ +vpColVector hx(const vpColVector &chi) +{ + vpColVector point(2); + point[0] = chi[0]; + point[1] = chi[2]; + return point; +} + +int main(/*const int argc, const char *argv[]*/) +{ + const double dt = 0.01; // Period of 1s + const double gt_dx = 0.01; // Ground truth displacement along x axis between two measurements + const double gt_dy = 0.005; // Ground truth displacement along x axis between two measurements + vpColVector gt_dX(2); // Ground truth displacement between two measurements + gt_dX[0] = gt_dx; + gt_dX[1] = gt_dy; + const double gt_vx = gt_dx / dt; // Ground truth velocity along x axis + const double gt_vy = gt_dy / dt; // Ground truth velocity along y axis + const double processVariance = 0.000004; + const double sigmaXmeas = 0.05; // Standard deviation of the measure along the x-axis + const double sigmaYmeas = 0.05; // Standard deviation of the measure along the y-axis + + // Initialize the attributes of the UKF + std::shared_ptr drawer = std::make_shared(4, 0.3, 2., -1.); + vpMatrix P0(4, 4); // The initial guess of the process covariance + P0.eye(4, 4); + P0 = P0 * 1.; + vpMatrix R(2, 2); // The covariance of the noise introduced by the measurement + R.eye(2, 2); + R = R * 0.01; + vpMatrix Q(4, 4, 0.); // The covariance of the process + vpMatrix Q1d(2, 2); // The covariance of a process whose states are {x, dx/dt} and for which the variance is 1 + Q1d[0][0] = std::pow(dt, 3) / 3.; + Q1d[0][1] = std::pow(dt, 2)/2.; + Q1d[1][0] = std::pow(dt, 2)/2.; + Q1d[1][1] = dt; + Q.insert(Q1d, 0, 0); + Q.insert(Q1d, 2, 2); + Q = Q * processVariance; + vpUnscentedKalman::vpProcessFunction f = fx; + vpUnscentedKalman::vpMeasurementFunction h = hx; + + // Initialize the UKF + vpUnscentedKalman ukf(Q, R, drawer, f, h); + ukf.init(vpColVector({ 0., 0.75 * gt_vx, 0., 0.75 * gt_vy }), P0); + +#ifdef VISP_HAVE_DISPLAY + // Initialize the plot + vpPlot plot(4); + plot.initGraph(0, 3); + 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, "Measure"); + plot.setLegend(0, 2, "Filtered"); + + 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, "Measure"); + plot.setLegend(1, 2, "Filtered"); + + 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, "Measure"); + plot.setLegend(2, 2, "Filtered"); + + 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, "Measure"); + plot.setLegend(3, 2, "Filtered"); +#endif + + // Initialize measurement noise + vpGaussRand rngX(sigmaXmeas, 0., 4224); + vpGaussRand rngY(sigmaYmeas, 0., 2112); + + // Main loop + vpColVector gt_X(2, 0.); + vpColVector z_prec(2, 0.); + for (int i = 0; i < 100; ++i) { + // Perform the measurement + double x_meas = gt_X[0] + rngX(); + double y_meas = gt_X[1] + rngY(); + vpColVector z(2); + z[0] = x_meas; + z[1] = y_meas; + + // Use the UKF to filter the measurement + ukf.filter(z, dt); + vpColVector Xest = ukf.getXest(); + +#ifdef VISP_HAVE_DISPLAY + // Plot the ground truth, measurement and filtered state + plot.plot(0, 0, i, gt_X[0]); + plot.plot(0, 1, i, x_meas); + plot.plot(0, 2, i, Xest[0]); + + double vX_meas = (x_meas - z_prec[0]) / dt; + plot.plot(1, 0, i, gt_vx); + plot.plot(1, 1, i, vX_meas); + plot.plot(1, 2, i, Xest[1]); + + plot.plot(2, 0, i, gt_X[1]); + plot.plot(2, 1, i, y_meas); + plot.plot(2, 2, i, Xest[2]); + + double vY_meas = (y_meas - z_prec[1]) / dt; + plot.plot(3, 0, i, gt_vy); + plot.plot(3, 1, i, vY_meas); + plot.plot(3, 2, i, Xest[3]); +#endif + + // Update + gt_X += gt_dX; + z_prec = z; + } + std::cout << "Press Enter to quit..." << std::endl; + std::cin.get(); + return 0; +} +#else +int main() +{ + std::cout << "vpUnscentedKalman is only available if you compile ViSP in C++11 standard or higher." << std::endl; + return 0; +} +#endif diff --git a/example/kalman/ukf-nonlinear-complex-example.cpp b/example/kalman/ukf-nonlinear-complex-example.cpp new file mode 100644 index 0000000000..5fa5550bf1 --- /dev/null +++ b/example/kalman/ukf-nonlinear-complex-example.cpp @@ -0,0 +1,633 @@ +/**************************************************************************** + * + * 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 ukf-nonlinear-complex-example.cpp + * Example of a complex non-linear use-case of the Unscented Kalman Filter (UKF). + * 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 UKF is: + * \f{eqnarray*}{ + \textbf{x}[0] &=& x \\ + \textbf{x}[1] &=& y \\ + \textbf{x}[2] &=& \theta + \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{eqnarray*}{ + \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 + \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 Unscented Transform. 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 Unscented Transform 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. + + Additionnally, the addition and substraction 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$ . +*/ + +// UKF includes +#include +#include + +// ViSP includes +#include +#include +#ifdef VISP_HAVE_DISPLAY +#include +#endif + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +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 weighted mean of measurement vectors. + * + * \param[in] measurements The measurement vectors. + * \param[in] wm The associated weights. + * \return vpColVector + */ +vpColVector measurementMean(const std::vector &measurements, const std::vector &wm) +{ + const unsigned int nbPoints = measurements.size(); + const unsigned int sizeMeasurement = measurements[0].size(); + const unsigned int nbLandmarks = sizeMeasurement / 2; + vpColVector mean(sizeMeasurement, 0.); + std::vector sumCos(nbLandmarks, 0.); + std::vector sumSin(nbLandmarks, 0.); + for (unsigned int i = 0; i < nbPoints; ++i) { + for (unsigned int j = 0; j < nbLandmarks; ++j) { + mean[2*j] += wm[i] * measurements[i][2*j]; + sumCos[j] += wm[i] * std::cos(measurements[i][(2*j)+1]); + sumSin[j] += wm[i] * std::sin(measurements[i][(2*j)+1]); + } + } + for (unsigned int j = 0; j < nbLandmarks; ++j) { + mean[(2*j)+1] = std::atan2(sumSin[j], sumCos[j]); + } + return mean; +} + +/** + * \brief Compute the substraction between two vectors expressed in the measurement space, + * such as v[0] = dist_0 ; v[1] = bearing_0; v[2] = dist_1 ; v[3] = bearing_1 ... + * + * \param[in] meas Measurement to which we must substract something. + * \param[in] toSubstract The something we must substract. + * \return vpColVector \b meas - \b toSubstract . + */ +vpColVector measurementResidual(const vpColVector &meas, const vpColVector &toSubstract) +{ + vpColVector res = meas - toSubstract; + unsigned int nbMeasures = res.size(); + for (unsigned int i = 1; i < nbMeasures; i += 2) { + res[i] = normalizeAngle(res[i]); + } + return res; +} + +/** + * \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) +{ + vpColVector mean(3, 0.); + unsigned int nbPoints = 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 substraction 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 substract something. + * \param[in] toSubstract The something we must substract. + * \return vpColVector \b state - \b toSubstract . + */ +vpColVector stateResidual(const vpColVector &state, const vpColVector &toSubstract) +{ + vpColVector res = state - toSubstract; + res[2] = normalizeAngle(res[2]); + return res; +} + +/** + * \brief As the state model {x, y, \f$ \theta \f$} does not contain any velocity + * information, it does not evolve without commands. + * + * \param[in] x The state vector + * \return vpColVector The state vector unchanged. + */ +vpColVector fx(const vpColVector &x, const double & /*dt*/) +{ + return x; +} + +/** + * \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 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) + { + 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 / m_w) * std::tan(steeringAngle); + double radius = m_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 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 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., vpTime::measureTimeMicros()) + , m_rngRelativeAngle(rel_angle_std, 0., vpTime::measureTimeMicros() + 4221) + { } + + /** + * \brief Convert the prior of the UKF into the measurement space. + * + * \param[in] chi The prior. + * \return vpColVector The prior expressed in the measurement space. + */ + vpColVector state_to_measurement(const vpColVector &chi) + { + vpColVector meas(2); + double dx = m_x - chi[0]; + double dy = m_y - chi[1]; + meas[0] = std::sqrt(dx * dx + dy * dy); + meas[1] = normalizeAngle(std::atan2(dy, dx) - chi[2]); + 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) - pos[2]); + 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; + } + +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 landmarks The list of landmarks forming the grid. + */ + vpLandmarksGrid(const std::vector &landmarks) + : m_landmarks(landmarks) + { } + + /** + * \brief Convert the prior of the UKF into the measurement space. + * + * \param[in] chi The prior. + * \return vpColVector The prior expressed in the measurement space. + */ + vpColVector state_to_measurement(const vpColVector &chi) + { + unsigned int nbLandmarks = m_landmarks.size(); + vpColVector measurements(2*nbLandmarks); + for (unsigned int i = 0; i < nbLandmarks; ++i) { + vpColVector landmarkMeas = m_landmarks[i].state_to_measurement(chi); + 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) + { + unsigned int nbLandmarks = m_landmarks.size(); + vpColVector measurements(2*nbLandmarks); + for (unsigned int i = 0; i < 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) + { + unsigned int nbLandmarks = m_landmarks.size(); + vpColVector measurements(2*nbLandmarks); + for (unsigned int i = 0; i < nbLandmarks; ++i) { + vpColVector landmarkMeas = m_landmarks[i].measureWithNoise(pos); + measurements[2*i] = landmarkMeas[0]; + measurements[(2*i) + 1] = landmarkMeas[1]; + } + return measurements; + } + +private: + std::vector m_landmarks; /*!< The list of landmarks forming the grid.*/ +}; + +int main(/*const int argc, const char *argv[]*/) +{ + const double dt = 0.1; // Period of 0.1s + const double step = 1.; // Number of update of the robot position between two UKF 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 constituing the grid + const unsigned int nbLandmarks = landmarks.size(); // Number of landmarks constituing the grid + std::vector cmds = generateCommands(); + const unsigned int nbCmds = cmds.size(); + + // Initialize the attributes of the UKF + std::shared_ptr drawer = std::make_shared(3, 0.00001, 2., 0, stateResidual, stateAdd); + + vpMatrix R1landmark(2, 2, 0.); // The covariance of the noise introduced by the measurement with 1 landmark + R1landmark[0][0] = sigmaRange*sigmaRange; + R1landmark[1][1] = sigmaBearing*sigmaBearing; + vpMatrix R(2*nbLandmarks, 2 * nbLandmarks); + for (unsigned int i = 0; i < nbLandmarks; ++i) { + R.insert(R1landmark, 2*i, 2*i); + } + + const double processVariance = 0.0001; + vpMatrix Q; // The covariance of the process + Q.eye(3); + Q = Q * processVariance; + + vpMatrix P0(3, 3); // The initial guess of the process covariance + P0.eye(3); + P0[0][0] = 0.1; + P0[1][1] = 0.1; + P0[2][2] = 0.05; + + vpColVector X0(3); + X0[0] = 2.; // x = 2m + X0[1] = 6.; // y = 6m + X0[2] = 0.3; // robot orientation = 0.3 rad + + vpUnscentedKalman::vpProcessFunction f = fx; + vpLandmarksGrid grid(landmarks); + vpBicycleModel robot(wheelbase); + using std::placeholders::_1; + using std::placeholders::_2; + using std::placeholders::_3; + vpUnscentedKalman::vpMeasurementFunction h = std::bind(&vpLandmarksGrid::state_to_measurement, &grid, _1); + vpUnscentedKalman::vpCommandStateFunction bx = std::bind(&vpBicycleModel::computeMotion, &robot, _1, _2, _3); + + // Initialize the UKF + vpUnscentedKalman ukf(Q, R, drawer, f, h); + ukf.init(X0, P0); + ukf.setCommandStateFunction(bx); + ukf.setMeasurementMeanFunction(measurementMean); + ukf.setMeasurementResidualFunction(measurementResidual); + ukf.setStateAddFunction(stateAdd); + ukf.setStateMeanFunction(stateMean); + ukf.setStateResidualFunction(stateResidual); + +#ifdef VISP_HAVE_DISPLAY + // Initialize the plot + vpPlot plot(1); + plot.initGraph(0, 2); + 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"); +#endif + + // Initialize the simulation + vpColVector robot_pos = X0; + + 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); + + // Use the UKF to filter the measurement + ukf.filter(z, dt, cmds[i]); + +#ifdef VISP_HAVE_DISPLAY + // Plot the filtered state + vpColVector Xest = ukf.getXest(); + plot.plot(0, 1, Xest[0], Xest[1]); +#endif + } + +#ifdef VISP_HAVE_DISPLAY + // Plot the ground truth + plot.plot(0, 0, robot_pos[0], robot_pos[1]); +#endif + } + std::cout << "Press Enter to quit..." << std::endl; + std::cin.get(); + return 0; +} +#else +int main() +{ + std::cout << "vpUnscentedKalman is only available if you compile ViSP in C++11 standard or higher." << std::endl; + return 0; +} +#endif diff --git a/example/kalman/ukf-nonlinear-example.cpp b/example/kalman/ukf-nonlinear-example.cpp new file mode 100644 index 0000000000..eb4f88e5e1 --- /dev/null +++ b/example/kalman/ukf-nonlinear-example.cpp @@ -0,0 +1,427 @@ +/**************************************************************************** + * + * 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 ukf-nonlinear-example.cpp + * Example of a simple non-linear use-case of the Unscented Kalman Filter (UKF). + * + * 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 UKF corresponds to a constant velocity model and can be written as: + * \f{eqnarray*}{ + \textbf{x}[0] &=& x \\ + \textbf{x}[1] &=& \dot{x} \\ + \textbf{x}[1] &=& y \\ + \textbf{x}[2] &=& \dot{y} + \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{eqnarray*}{ + \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}} + \f} + + * Some noise is added to the measurement vector to simulate a sensor which is + * not perfect. + * + * The mean of several angles must be computed in the Unscented Transform. 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 Unscented Transform 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. + + Additionnally, the addition and substraction 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$ . +*/ + +// UKF includes +#include +#include + +// ViSP includes +#include +#include +#ifdef VISP_HAVE_DISPLAY +#include +#endif + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +namespace +{ +/** + * \brief The process function, that updates the prior. + * + * \param[in] chi A sigma point. + * \param[in] dt The period. + * \return vpColVector The sigma points projected in the future. + */ +vpColVector fx(const vpColVector &chi, const double &dt) +{ + vpColVector point(4); + point[0] = chi[1] * dt + chi[0]; + point[1] = chi[1]; + point[2] = chi[3] * dt + chi[2]; + point[3] = chi[3]; + return point; +} + +/** + * \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 weighted mean of measurement vectors. + * + * \param[in] measurements The measurement vectors, such as measurements[i][0] = range and + * measurements[i][1] = elevation_angle. + * \param[in] wm The associated weights. + * \return vpColVector + */ +vpColVector measurementMean(const std::vector &measurements, const std::vector &wm) +{ + const unsigned int nbPoints = measurements.size(); + const unsigned int sizeMeasurement = measurements[0].size(); + vpColVector mean(sizeMeasurement, 0.); + double sumCos(0.); + double sumSin(0.); + for (unsigned int i = 0; i < nbPoints; ++i) { + mean[0] += wm[i] * measurements[i][0]; + sumCos += wm[i] * std::cos(measurements[i][1]); + sumSin += wm[i] * std::sin(measurements[i][1]); + } + + mean[1] = std::atan2(sumSin, sumCos); + + return mean; +} + +/** + * \brief Compute the substraction between two vectors expressed in the measurement space, + * such as v[0] = range ; v[1] = elevation_angle + * + * \param[in] meas Measurement to which we must substract something. + * \param[in] toSubstract The something we must substract. + * \return vpColVector \b meas - \b toSubstract . + */ +vpColVector measurementResidual(const vpColVector &meas, const vpColVector &toSubstract) +{ + vpColVector res = meas - toSubstract; + res[1] = normalizeAngle(res[1]); + return res; +} +} + +/** + * \brief Class that permits to convert the position of the aircraft into + * range and elevation angle measurements. + */ +class vpRadarStation +{ +public: + /** + * \brief Construct a new vpRadarStation object. + * + * \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. + */ + vpRadarStation(const double &x, const double &y, const double &range_std, const double &elev_angle_std) + : m_x(x) + , m_y(y) + , m_rngRange(range_std, 0., 4224) + , m_rngElevAngle(elev_angle_std, 0., 2112) + { } + + /** + * \brief Convert the prior of the UKF into the measurement space. + * + * \param chi The prior. + * \return vpColVector The prior expressed in the measurement space. + */ + vpColVector state_to_measurement(const vpColVector &chi) + { + vpColVector meas(2); + double dx = chi[0] - m_x; + double dy = chi[2] - m_y; + meas[0] = std::sqrt(dx * dx + dy * dy); + meas[1] = std::atan2(dy, dx); + return meas; + } + + /** + * \brief Perfect measurement of the range and elevation angle that + * correspond to pos. + * + * \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 &pos) + { + 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; + } + + /** + * \brief Noisy measurement of the range and elevation angle that + * correspond to pos. + * + * \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 measureWithNoise(const vpColVector &pos) + { + vpColVector measurementsGT = measureGT(pos); + vpColVector measurementsNoisy = measurementsGT; + measurementsNoisy[0] += m_rngRange(); + measurementsNoisy[1] += m_rngElevAngle(); + return measurementsNoisy; + } + +private: + 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 +}; + +/** + * \brief Class to simulate a flying aircraft. + */ +class vpACSimulator +{ +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.) + { + + } + + /** + * \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 +}; + +int main(/*const int argc, const char *argv[]*/) +{ + const double dt = 3.; // Period of 3s + const double sigmaRange = 5; // Standard deviation of the range measurement: 5m + const double sigmaElevAngle = vpMath::rad(0.5); // Standard deviation of the elevation angle measurent: 0.5deg + const double stdevAircraftVelocity = 0.2; // Standard deviation of the velocity of the simulated aircraft, to make it deviate a bit from the constant velocity model + const double gt_X_init = -500.; // Ground truth initial position along the X-axis, in meters + const double gt_Y_init = 1000.; // Ground truth initial position along the Y-axis, in meters + const double gt_vX_init = 100.; // Ground truth initial velocity along the X-axis, in meters + const double gt_vY_init = 5.; // Ground truth initial velocity along the Y-axis, in meters + + // Initialize the attributes of the UKF + std::shared_ptr drawer = std::make_shared(4, 0.1, 2., -1.); + + vpMatrix R(2, 2, 0.); // The covariance of the noise introduced by the measurement + R[0][0] = sigmaRange*sigmaRange; + R[1][1] = sigmaElevAngle*sigmaElevAngle; + + const double processVariance = 0.1; + vpMatrix Q(4, 4, 0.); // The covariance of the process + vpMatrix Q1d(2, 2); // The covariance of a process whose states are {x, dx/dt} and for which the variance is 1 + Q1d[0][0] = std::pow(dt, 3) / 3.; + Q1d[0][1] = std::pow(dt, 2)/2.; + Q1d[1][0] = std::pow(dt, 2)/2.; + Q1d[1][1] = dt; + Q.insert(Q1d, 0, 0); + Q.insert(Q1d, 2, 2); + Q = Q * processVariance; + + vpMatrix P0(4, 4); // The initial guess of the process covariance + P0.eye(4, 4); + P0[0][0] = std::pow(300, 2); + P0[1][1] = std::pow(30, 2); + P0[2][2] = std::pow(150, 2); + P0[3][3] = std::pow(30, 2); + + vpColVector X0(4); + X0[0] = 0.9 * gt_X_init; // x, i.e. 10% of error with regard to ground truth + X0[1] = 0.9 * gt_vX_init; // dx/dt, i.e. 10% of error with regard to ground truth + X0[2] = 0.9 * gt_Y_init; // y, i.e. 10% of error with regard to ground truth + X0[3] = 0.9 * gt_vY_init; // dy/dt, i.e. 10% of error with regard to ground truth + + vpUnscentedKalman::vpProcessFunction f = fx; + vpRadarStation radar(0., 0., sigmaRange, sigmaElevAngle); + using std::placeholders::_1; + vpUnscentedKalman::vpMeasurementFunction h = std::bind(&vpRadarStation::state_to_measurement, &radar, _1); + + // Initialize the UKF + vpUnscentedKalman ukf(Q, R, drawer, f, h); + ukf.init(X0, P0); + ukf.setMeasurementMeanFunction(measurementMean); + ukf.setMeasurementResidualFunction(measurementResidual); + +#ifdef VISP_HAVE_DISPLAY + // Initialize the plot + vpPlot plot(4); + plot.initGraph(0, 2); + 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.initGraph(1, 2); + 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.initGraph(2, 2); + 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.initGraph(3, 2); + 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"); +#endif + + // Initialize the simulation + vpColVector ac_pos(2); + ac_pos[0] = gt_X_init; + ac_pos[1] = gt_Y_init; + vpColVector ac_vel(2); + ac_vel[0] = gt_vX_init; + ac_vel[1] = gt_vY_init; + vpACSimulator ac(ac_pos, ac_vel, stdevAircraftVelocity); + vpColVector gt_Xprec = ac_pos; + for (int i = 0; i < 500; ++i) { + // Perform the measurement + vpColVector gt_X = ac.update(dt); + vpColVector gt_V = (gt_X - gt_Xprec) / dt; + vpColVector z = radar.measureWithNoise(gt_X); + + // Use the UKF to filter the measurement + ukf.filter(z, dt); + vpColVector Xest = ukf.getXest(); + +#ifdef VISP_HAVE_DISPLAY + // 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(1, 0, i, gt_V[0]); + plot.plot(1, 1, i, Xest[1]); + + plot.plot(2, 0, i, gt_X[1]); + plot.plot(2, 1, i, Xest[2]); + + plot.plot(3, 0, i, gt_V[1]); + plot.plot(3, 1, i, Xest[3]); +#endif + + gt_Xprec = gt_X; + } + std::cout << "Press Enter to quit..." << std::endl; + std::cin.get(); + return 0; +} +#else +int main() +{ + std::cout << "vpUnscentedKalman is only available if you compile ViSP in C++11 standard or higher." << std::endl; + return 0; +} +#endif diff --git a/modules/core/include/visp3/core/vpMatrix.h b/modules/core/include/visp3/core/vpMatrix.h index a2151f7498..d871de4a6c 100644 --- a/modules/core/include/visp3/core/vpMatrix.h +++ b/modules/core/include/visp3/core/vpMatrix.h @@ -347,6 +347,16 @@ class VISP_EXPORT vpMatrix : public vpArray2D #if defined(VISP_HAVE_OPENCV) double detByLUOpenCV() const; #endif + vpMatrix cholesky() const; +#if defined(VISP_HAVE_EIGEN3) + vpMatrix choleskyByEigen3() const; +#endif +#if defined(VISP_HAVE_LAPACK) + vpMatrix choleskyByLapack() const; +#endif +#if defined(VISP_HAVE_OPENCV) + vpMatrix choleskyByOpenCV() const; +#endif // Compute the exponential matrix of a square matrix vpMatrix expm() const; diff --git a/modules/core/include/visp3/core/vpUKSigmaDrawerAbstract.h b/modules/core/include/visp3/core/vpUKSigmaDrawerAbstract.h new file mode 100644 index 0000000000..24e51c8259 --- /dev/null +++ b/modules/core/include/visp3/core/vpUKSigmaDrawerAbstract.h @@ -0,0 +1,83 @@ +/* + * 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: + * Display a point cloud using PCL library. + */ + +#ifndef _vpUKSigmaDrawerAbstract_h_ +#define _vpUKSigmaDrawerAbstract_h_ + +#include + +#include + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +#include + +/*! + \class vpUKSigmaDrawerAbstract + \ingroup group_core_kalman + This abstract class defines the interface to draw the sigma points for the Unscented Kalman filter. +*/ +class VISP_EXPORT vpUKSigmaDrawerAbstract +{ +public: + /** + * \brief The weights corresponding to the sigma points drawing. + */ + typedef struct vpSigmaPointsWeights + { + std::vector m_wm; /*!< The weights for the computation of the mean.*/ + std::vector m_wc; /*!< The weights for the computation of the covariance.*/ + }vpSigmaPointsWeights; + + inline vpUKSigmaDrawerAbstract(const unsigned int &n) : m_n(n) { } + + /** + * \brief Draw the sigma points according to the current mean and covariance of the state + * of the Unscented Kalman filter. + * + * \param[in] mean The current mean of the state of the UKF. + * \param[in] covariance The current process covariance of the UKF. + * @return std::vector The sigma points. + */ + virtual std::vector drawSigmaPoints(const vpColVector &mean, const vpMatrix &covariance) = 0; + + /** + * \brief Computed the weigths that correspond to the sigma poitns that have been drawn. + * + * \return vpSigmaPointsWeights The weights that correspond to the sigma points. + */ + virtual vpSigmaPointsWeights computeWeights() = 0; +protected: + unsigned int m_n; /*!< The size of the state of the UKF.*/ +}; +#endif +#endif diff --git a/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h b/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h new file mode 100644 index 0000000000..cd0039da9d --- /dev/null +++ b/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h @@ -0,0 +1,150 @@ +/* + * 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: + * Display a point cloud using PCL library. + */ + +#ifndef _vpUKSigmaDrawerMerwe_h_ +#define _vpUKSigmaDrawerMerwe_h_ + +#include + +#include + +#include +#include + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +/*! + \class vpUKSigmaDrawerMerwe + \ingroup group_core_kalman + This class defines a class to draw sigma points following the E. A. Wan and R. van der Merwe's method proposed in + \cite Merwe00. + + The method has four parameters: \f$ n \f$, which is the dimension of the input, and \f$ \alpha \f$, \f$ \beta \f$ and + \f$ \kappa \f$, which are three reals. For notational convenience, we define \f$ \lambda = \alpha^2 (n - \kappa) - n \f$. + + Be \f$ \boldsymbol{\mu} \in {R}^n \f$ the mean and \f$ \boldsymbol{\Sigma} \in {R}^{n x n} \f$ the covariance matrix of the + input of the algorithm. The algorithm will draw \f$ 2n + 1 \f$ sigma points \f$ \chi_i \in {R}^n \f$ such as: + + \f{eqnarray*}{ + \chi_0 &=& \boldsymbol{\mu} \\ + \chi_i &=& \begin{cases} \boldsymbol{\mu} + \left[ \sqrt{(n + \lambda) \boldsymbol{\Sigma}} \right]_i^T & i = 1 .. n \\ + \boldsymbol{\mu} - \left[ \sqrt{(n + \lambda) \boldsymbol{\Sigma}} \right]_{i - n}^T & i = n + 1 .. 2n + \end{cases} + \f} + + where the subscript \f$ i \f$ denotes that we keep the \f$ i^{th} \f$ of the matrix. + + Several definitions of the square root of a matrix exists. We decided to use the following definition: \f$ \textbf{L} \f$ + is the square root of the matrix \f$ \boldsymbol{\Sigma} \f$ if \f$ \boldsymbol{\Sigma} \f$ can be written as: + + \f$ \boldsymbol{\Sigma} = \textbf{L} \textbf{L}^T \f$ + + This definition is favored because it can be computed using the Cholesky's decomposition. + + The computation of the weights that go along the sigma points is the following. The weight used for the + computation of the mean of \f$ \chi_0 \f$ is computed such as: + + \f$ w_0^m = \frac{\lambda}{n + \lambda} \f$ + + The weight used for the computation of the mean of \f$ \chi_0 \f$ is computed such as: + + \f$ w_0^c = \frac{\lambda}{n + \lambda} + 1 - \alpha^2 + \beta \f$ + + The weights for the other sigma points \f$ \chi_1 ... \chi_{2n} \f$ are the same for the mean and covariance + and are computed as follow: + + \f$ w_i^m = w_i^c = \frac{1}{2(n + \lambda)} i = 1..2n \f$ + + \b Note: the weights do not sum to one. Negative values can even be expected. + + \b Additionnal \b note: the original author recommended to set \f$ \beta = 2 \f$ for Gaussian problems, \f$ \kappa = 3 - n \f$ + and \f$ 0 \leq \alpha \leq 1 \f$, where a larger value for \f$ \alpha \f$ spreads the sigma points further from the mean, + which can be a problem for highly non-linear problems. + +*/ +class VISP_EXPORT vpUKSigmaDrawerMerwe : public vpUKSigmaDrawerAbstract +{ +public: + typedef vpUnscentedKalman::vpAddSubFunction vpAddSubFunction; + + /** + * \brief Construct a new vpUKSigmaDrawerMerwe object. + * + * \param[in] n The size of the state vector. + * \param[in] alpha A factor, which should be a real in the interval [0; 1]. The larger alpha is, + * the further the sigma points are spread from the mean. + * \param[in] beta Another factor, which should be set to 2 if the problem is Gaussian. + * \param[in] kappa A third factor, whose value should be set to 3 - n for most problems. + * \param[in] resFunc Residual function expressed in the state space. + * \param[in] addFunc Addition function expressed in the state space. + */ + vpUKSigmaDrawerMerwe(const int &n, const double &alpha, const double &beta, const double &kappa, + const vpAddSubFunction &resFunc = vpUnscentedKalman::simpleResidual, + const vpAddSubFunction &addFunc = vpUnscentedKalman::simpleAdd); + + /** + * Destructor that does nothing. + */ + virtual ~vpUKSigmaDrawerMerwe() { } + + /** + * \brief Draw the sigma points according to the current mean and covariance of the state + * of the Unscented Kalman filter. + * + * \param[in] mean The current mean of the state of the UKF. + * \param[in] covariance The current process covariance of the UKF. + * @return std::vector The sigma points. + */ + virtual std::vector drawSigmaPoints(const vpColVector &mean, const vpMatrix &covariance) override; + + /** + * \brief Computed the weigths that correspond to the sigma poitns that have been drawn. + * + * \return vpSigmaPointsWeights The weights that correspond to the sigma points. + */ + virtual vpSigmaPointsWeights computeWeights() override; +protected: + inline void computeLambda() + { + m_lambda = m_alpha * m_alpha * (m_n + m_kappa) - m_n; + } + + double m_alpha; /*!< A factor, which should be a real in the interval [0; 1]. The larger alpha is, + the further the sigma points are spread from the mean.*/ + double m_beta; /*!< Another factor, which should be set to 2 if the problem is Gaussian.*/ + double m_kappa; /*!< A third factor, whose value should be set to 3 - n for most problems.*/ + double m_lambda; /*!< \f$ \alpha^2 (n + \kappa) - n \f$, where \f$ n \f$ is the size of the state vector.*/ + vpAddSubFunction m_resFunc; /*!< Residual function expressed in the state space.*/ + vpAddSubFunction m_addFunc; /*!< Addition function expressed in the state space.*/ +}; +#endif +#endif diff --git a/modules/core/include/visp3/core/vpUnscentedKalman.h b/modules/core/include/visp3/core/vpUnscentedKalman.h new file mode 100644 index 0000000000..60c3eba162 --- /dev/null +++ b/modules/core/include/visp3/core/vpUnscentedKalman.h @@ -0,0 +1,431 @@ +/* + * 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: + * Display a point cloud using PCL library. + */ + +#ifndef _vpUnscentedKalman_h_ +#define _vpUnscentedKalman_h_ + +#include + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +#include +#include +#include + +#include // std::function +#include // std::shared_ptr + +/*! + \class vpUnscentedKalman + \ingroup group_core_kalman + This class permits to use Unscented Kalman Filter (UKF) to tackle non-linear problems. Non-linearity + can arise in the process function \f$ f: {R}^n \rightarrow {R}^n \f$, which makes evolve the internal + state \f$ \textbf{x} \in {R}^n \f$ of the UKF over time, or in the measurement function \f$ h: {R}^n \rightarrow {R}^m \f$, + which expresses the internal state of the UKF in the measurement space of dimension \f$ m \f$. + + We will briefly explain the principles of the UKF and the maths behind the wheel. We refer the interested + readers to the [web-book](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python) by R. Labbe, chapter 10, + for more details. + + The UKF is performed in two steps. First, the prediction step, during which we draw the sigma points \f$ \chi \f$ and compute + their corresponding weights \f$ \textbf{w}^m \in {R}^{2n + 1} \f$ and \f$ \textbf{w}^c \in {R}^{2n + 1} \f$. + Be \f$ \textbf{x} \in {R}^n \f$ the internal state of the UKF and \f$ \textbf{P} \in {R}^{n\text{ x }n} \f$ the process covariance matrix. + We have: + + \f{eqnarray*}{ + \chi &=& sigma-function(\textbf{x}, \textbf{P}) \\ + \textbf{w}^m, \textbf{w}^c &=& weight-function(n, parameters) + \f} + + There are different ways of drawing the sigma points and associated weights in the litterature, such as the one + proposed by Julier or the one proposed by E. A. Wan and R. van der Merwe. + + Be \f$ \textbf{u} \f$ the vector containing the known commands sent to the system, if any. Then, we pass each sigma + point through the process function \f$ f(\chi, \Delta t) \f$, the command function + \f$ b( \textbf{u}, \Delta t ) \f$ and the command function depending on the state \f$ bx( \textbf{u}, \chi, \Delta t ) \f$ + to project them forward in time, forming the new prior: + + \f$ {Y} = f( \chi , \Delta t ) + b( \textbf{u}, \Delta t ) + bx( \textbf{u}, \chi, \Delta t ) \f$ + + Then, we apply the Unscented Transform to compute the mean \f$ \boldsymbol{\mu} \f$ + and covariance \f$ \overline{\textbf{P}} \f$ of the prior: + + \f{eqnarray*}{ + \boldsymbol{\mu}, \overline{\textbf{P}} &=& UT({Y}, \textbf{w}^m, \textbf{w}^c, \textbf{Q}) \\ + \boldsymbol{\mu} &=& \sum_{i=0}^{2n} w_i^m {Y}_i \\ + \overline{\textbf{P}} &=& \sum_{i=0}^{2n} ( w_i^c ({Y}_i - \boldsymbol{\mu}) ({Y}_i - \boldsymbol{\mu})^T ) + \textbf{Q} + \f} + + where \f$ \textbf{Q} \f$ is the covariance of the error introduced by the process function. + + The second step is the update step. It is performed in the measurement space, so we must convert the sigma points of + the prior into measurements using the measurement function \f$ h: {R}^n \rightarrow {R}^m \f$: + + \f$ {Z} = h({Y}) \f$ + + Then, we use once again the Unscented Transform to compute the mean \f$ \boldsymbol{\mu}_z \in {R}^m \f$ and the + covariance \f$ \textbf{P}_z \in {R}^{m\text{ x }m} \f$ of these points: + + \f{eqnarray*}{ + \boldsymbol{\mu}_z, \textbf{P}_z &=& UT({Z}, \textbf{w}^m, \textbf{w}^c, \textbf{R}) \\ + \boldsymbol{\mu}_z &=& \sum_{i=0}^{2n} w_i^m {Z}_i \\ + \textbf{P}_z &=& \sum_{i=0}^{2n} ( w_i^c ({Z}_i - \boldsymbol{\mu}_z) ({Z}_i - \boldsymbol{\mu}_z)^T ) + \textbf{R} + \f} + + where \f$ \textbf{R} \f$ is the measurement covariance matrix. + + Then, we compute the residual \f$ \textbf{y} \f$ of the measurement \f$ \textbf{z} \f$: + + \f$ \textbf{y} = \textbf{z} - \boldsymbol{\mu}_z \f$ + + To compute the Kalman's gain, we first need to compute the cross covariance of the state and the measurements: + + \f$ \textbf{P}_{xy} = \sum_{i=0}^{2n} w_i^c ({Y}_i - \boldsymbol{\mu})({Z}_i - \boldsymbol{\mu}_z)^T \f$ + + The Kalman's gain is then defined as: + + \f$ \textbf{K} = \textbf{P}_{xz} \textbf{P}_z^{-1} \f$ + + Finally, we can compute the new state estimate \f$ \textbf{x} \f$ and the new covariance \f$ \textbf{P} \f$: + + \f{eqnarray*}{ + \textbf{x} &=& \boldsymbol{\mu} + \textbf{K} \textbf{y} \\ + \textbf{P} &=& \overline{\textbf{P}} - \textbf{K} \textbf{P}_z \textbf{K}^T + \f} + +*/ +class VISP_EXPORT vpUnscentedKalman +{ +public: + /** + * \brief Command model function, which projects effect of the command + * on the state. + * The first argument is the command(s), the second is the period and the return is the + * effect of the command on the state after period seconds. + */ + typedef std::function vpCommandOnlyFunction; + + /** + * \brief Command model function, which projects effect of the command + * on the state, when the effect of the command depends on the current state. + * The first argument is the command(s), the second is the state and the third is the period. + * The return is the effect of the command on the state after period seconds. + */ + typedef std::function vpCommandStateFunction; + + /** + * \brief Mean function, which computes the weighted mean of either the prior or the + * prior expressed in the measurement space. + * The first argument is either the prior or the prior expressed in the measurement space + * and the second argument is the associated vector of weights. The return is the + * corresponding mean. + */ + typedef std::function &, const std::vector &)> vpMeanFunction; + + /** + * \brief Measurement function, which converts the prior points in the measurement space. + * The argument is a point of a prior point and the return is its projection in the measurement + * space. + */ + typedef std::function vpMeasurementFunction; + + /** + * \brief Process model function, which projects the sigma points forward in time. + * The first argument is a sigma point, the second is the period and the return is the + * sigma point projected in the future (i.e. a point of the prior). + */ + typedef std::function vpProcessFunction; + + /** + * \brief Function that computes either the equivalent of an addition or the equivalent + * of a substraction in the state space or in the measurement space. + * The first argument is the vector to which we must add/substract something + * and the second argument is the thing to be added/substracted. The return is the + * result of this operation. + */ + typedef std::function vpAddSubFunction; + + /** + * \brief Residual function, which computes either the equivalent of the substraction in the + * state space or the equivalent of the substraction in the measurement space. + * The first argument is the vector to which we must substract something + * and the second argument is the thing to be substracted. The return is the + * result of this "substraction". + */ + typedef std::function vpAddFunction; + + /** + * \brief Construct a new vpUnscentedKalman object. + * + * \param[in] Q The covariance introduced by performing the prediction step. + * \param[in] R The covariance introduced by performing the update step. + * \param[in] drawer Object that permits to draw the sigma points. + * \param[in] f Process model function, which projects the sigma points forward in time. + * The first argument is a sigma point, the second is the period and the return is the + * sigma point projected in the future (i.e. a point of the prior). + * \param[in] h Measurement function, which converts the prior points in the measurement space. + * The argument is a point of a prior point and the return is its projection in the measurement + * space. + */ + vpUnscentedKalman(const vpMatrix &Q, const vpMatrix &R, std::shared_ptr &drawer, const vpProcessFunction &f, const vpMeasurementFunction &h); + + /** + * \brief Set the guess of the initial state and covariance. + * + * \param[in] mu0 Guess of the initial state. + * \param[in] P0 Guess of the initial covariance. + */ + void init(const vpColVector &mu0, const vpMatrix &P0); + + /** + * \brief Set the command function to use when computing the prior. + * + * \param b The command function to use. + */ + inline void setCommandOnlyFunction(const vpCommandOnlyFunction &b) + { + m_b = b; + } + + /** + * \brief Set the command function to use when computing the prior. + * + * \param bx The command function to use. + */ + inline void setCommandStateFunction(const vpCommandStateFunction &bx) + { + m_bx = bx; + } + + /** + * \brief Set the measurement mean function to use when computing a mean + * in the measurement space. + * + * \param meanFunc The mean function to use. + */ + inline void setMeasurementMeanFunction(const vpMeanFunction &meanFunc) + { + m_measMeanFunc = meanFunc; + } + + /** + * \brief Set the measurement residual function to use when computing a substraction + * in the measurement space. + * + * \param measResFunc The residual function to use. + */ + inline void setMeasurementResidualFunction(const vpAddSubFunction &measResFunc) + { + m_measResFunc = measResFunc; + } + + /** + * \brief Set the state addition function to use when computing a addition + * in the state space. + * + * \param stateAddFunc The addition function to use. + */ + inline void setStateAddFunction(const vpAddSubFunction &stateAddFunc) + { + m_stateAddFunction = stateAddFunc; + } + + /** + * \brief Set the state mean function to use when computing a mean + * in the state space. + * + * \param meanFunc The mean function to use. + */ + inline void setStateMeanFunction(const vpMeanFunction &meanFunc) + { + m_stateMeanFunc = meanFunc; + } + + /** + * \brief Set the state residual function to use when computing a substraction + * in the state space. + * + * \param stateResFunc The residual function to use. + */ + inline void setStateResidualFunction(const vpAddSubFunction &stateResFunc) + { + m_stateResFunc = stateResFunc; + } + + /** + * \brief Perform first the prediction step and then the filtering step. + * + * \param[in] z The new measurement. + * \param[in] dt The time in the future we must predict. + * \param[in] u The command(s) given to the system, if the impact of the system is known. + * + * \warning To use the commands, the method vpUnscentedKalman::setCommandOnlyFunction or + * vpUnscentedKalman::setCommandStateFunction must be called beforehand. + */ + void filter(const vpColVector &z, const double &dt, const vpColVector &u = vpColVector()); + + /** + * \brief Predict the new state based on the last state and how far in time we want to predict. + * + * \param[in] dt The time in the future we must predict. + * \param[in] u The command(s) given to the system, if the impact of the system is known. + * + * \warning To use the commands, the method vpUnscentedKalman::setCommandOnlyFunction or + * vpUnscentedKalman::setCommandStateFunction must be called beforehand. + */ + void predict(const double &dt, const vpColVector &u = vpColVector()); + + /** + * \brief Update the estimate of the state based on a new measurement. + * + * \param[in] z The measurements at the current timestep. + */ + void update(const vpColVector &z); + + /** + * \brief Get the estimated (i.e. filtered) state. + * + * \return vpColVector The estimated state. + */ + inline vpColVector getXest() const + { + return m_Xest; + } + + /** + * \brief Get the predicted state (i.e. the prior). + * + * \return vpColVector The predicted state. + */ + inline vpColVector getXpred() const + { + return m_mu; + } + + /** + * \brief Simple function to compute an addition, which just does \f$ \textbf{res} = \textbf{a} + \textbf{toAdd} \f$ + * + * \param[in] a Vector to which we must add something. + * \param[in] toAdd The something we must add to \b a . + * \return vpColVector \f$ \textbf{res} = \textbf{a} + \textbf{toAdd} \f$ + */ + inline static vpColVector simpleAdd(const vpColVector &a, const vpColVector &toAdd) + { + vpColVector res = a + toAdd; + return res; + } + + /** + * \brief Simple function to compute a residual, which just does \f$ \textbf{res} = \textbf{a} - \textbf{toSubstract} \f$ + * + * \param[in] a Vector to which we must substract something. + * \param[in] toSubstract The something we must substract to \b a . + * \return vpColVector \f$ \textbf{res} = \textbf{a} - \textbf{toSubstract} \f$ + */ + inline static vpColVector simpleResidual(const vpColVector &a, const vpColVector &toSubstract) + { + vpColVector res = a - toSubstract; + return res; + } + + /** + * \brief Simple function to compute a mean, which just does \f$ \boldsymbol{\mu} = \sum_{i} wm_i \textbf{vals}_i \f$ + * + * \param[in] vals Vector containing all the vectors we must compute the mean. + * \param[in] wm The correspond list of weights. + * \return vpColVector \f$ \boldsymbol{\mu} = \sum_{i} wm_i \textbf{vals}_i \f$ + */ + inline static vpColVector simpleMean(const std::vector &vals, const std::vector &wm) + { + unsigned int nbPoints = vals.size(); + if (nbPoints == 0) { + throw(vpException(vpException::dimensionError, "No points to add when computing the mean")); + } + vpColVector mean = vals[0] * wm[0]; + for (unsigned int i = 1; i < nbPoints; ++i) { + mean += vals[i] * wm[i]; + } + return mean; + } +private: + vpColVector m_Xest; /*!< The estimated (i.e. filtered) state variables.*/ + vpMatrix m_Pest; /*!< The estimated (i.e. filtered) covariance matrix.*/ + vpMatrix m_Q; /*!< The covariance introduced by performing the prediction step.*/ + std::vector m_chi; /*!< The sigma points.*/ + std::vector m_wm; /*!< The weights for the mean computation.*/ + std::vector m_wc; /*!< The weights for the covariance computation.*/ + std::vector m_Y; /*!< The projection forward in time of the sigma points according to the process model, called the prior.*/ + vpColVector m_mu; /*!< The mean of the prior.*/ + vpMatrix m_P; /*!< The covariance matrix of the prior.*/ + vpMatrix m_R; /*!< The covariance introduced by performing the update step.*/ + std::vector m_Z; /*!< The sigma points of the prior expressed in the measurement space, called the measurement sigma points.*/ + vpColVector m_muz; /*!< The mean of the measurement sigma points.*/ + vpMatrix m_Pz; /*!< The covariance matrix of the measurement sigma points.*/ + vpMatrix m_Pxz; /*!< The cross variance of the state and the measurements.*/ + vpColVector m_y; /*!< The residual.*/ + vpMatrix m_K; /*!< The Kalman gain.*/ + vpProcessFunction m_f; /*!< Process model function, which projects the sigma points forward in time.*/ + vpMeasurementFunction m_h; /*!< Measurement function, which converts the sigma points in the measurement space.*/ + std::shared_ptr m_sigmaDrawer; /*!< Object that permits to draw the sigma points.*/ + vpCommandOnlyFunction m_b; /*!< Function that permits to compute the effect of the commands on the prior, without knowledge of the state.*/ + vpCommandStateFunction m_bx; /*!< Function that permits to compute the effect of the commands on the prior, with knowledge of the state.*/ + vpMeanFunction m_measMeanFunc; /*!< Function to compute a weighted mean in the measurement space.*/ + vpAddSubFunction m_measResFunc; /*!< Function to compute a substraction in the measurement space.*/ + vpAddSubFunction m_stateAddFunction; /*!< Function to compute an addition in the state space.*/ + vpMeanFunction m_stateMeanFunc; /*!< Function to compute a weighted mean in the state space.*/ + vpAddSubFunction m_stateResFunc; /*!< Function to compute a substraction in the state space.*/ + + /** + * \brief Structure that stores the results of the unscented transform. + * + */ + typedef struct vpUnscentedTransformResult + { + vpColVector m_mu; /*!< The mean.*/ + vpMatrix m_P; /*!< The covariance matrix.*/ + } vpUnscentedTransformResult; + + /** + * \brief Compute the unscented transform of the sigma points. + * + * \param[in] sigmaPoints The sigma points we consider. + * \param[in] wm The weights to apply for the mean computation. + * \param[in] wc The weights to apply for the covariance computation. + * \param[in] cov The constant covariance matrix to add to the computed covariance matrix. + * \return vpUnscentedTransformResult The mean and covariance of the sigma points. + */ + static vpUnscentedTransformResult unscentedTransform(const std::vector &sigmaPoints, const std::vector &wm, + const std::vector &wc, const vpMatrix &cov, const vpAddSubFunction &resFunc, const vpMeanFunction &meanFunc); +}; + +#endif +#endif diff --git a/modules/core/src/math/kalman/vpUKSigmaDrawerMerwe.cpp b/modules/core/src/math/kalman/vpUKSigmaDrawerMerwe.cpp new file mode 100644 index 0000000000..56d64fb2dd --- /dev/null +++ b/modules/core/src/math/kalman/vpUKSigmaDrawerMerwe.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2023 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: + * Kalman filtering. + * +*****************************************************************************/ + +/*! + \file vpUKSigmaDrawerMerwe.cpp + \brief Sigma points drawer following the E. A. Wan and R. van der Merwe's method. +*/ + +#include + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +vpUKSigmaDrawerMerwe::vpUKSigmaDrawerMerwe(const int &n, const double &alpha, const double &beta, const double &kappa, + const vpAddSubFunction &resFunc, const vpAddSubFunction &addFunc) + : vpUKSigmaDrawerAbstract(n) + , m_alpha(alpha) + , m_beta(beta) + , m_kappa(kappa) + , m_resFunc(resFunc) + , m_addFunc(addFunc) +{ + computeLambda(); +} + +std::vector vpUKSigmaDrawerMerwe::drawSigmaPoints(const vpColVector &mean, const vpMatrix &covariance) +{ + const unsigned int nbSigmaPoints = 2 * m_n + 1; + std::vector sigmaPoints(nbSigmaPoints); + sigmaPoints[0] = mean; + vpMatrix scaledCov = (static_cast(m_n) + m_lambda) * covariance; + vpMatrix cholesky = scaledCov.cholesky(); + for (unsigned int i = 0; i < m_n; ++i) { + sigmaPoints[i + 1] = m_addFunc(mean, cholesky.getRow(i).transpose()); + sigmaPoints[i + m_n + 1] = m_resFunc(mean, cholesky.getRow(i).transpose()); + } + return sigmaPoints; +} + +vpUKSigmaDrawerMerwe::vpSigmaPointsWeights vpUKSigmaDrawerMerwe::computeWeights() +{ + const unsigned int nbSigmaPoints = 2 * m_n + 1; + vpSigmaPointsWeights weights; + weights.m_wm.resize(nbSigmaPoints); + weights.m_wc.resize(nbSigmaPoints); + + weights.m_wm[0] = m_lambda / (static_cast(m_n) + m_lambda); + weights.m_wc[0] = (m_lambda / (static_cast(m_n) + m_lambda)) + 1.0 - m_alpha * m_alpha + m_beta; + + double cstWeight = 1. / (2. * (static_cast(m_n) + m_lambda)); + for (unsigned int i = 1; i < nbSigmaPoints; ++i) { + weights.m_wm[i] = cstWeight; + weights.m_wc[i] = cstWeight; + } + return weights; +} +#else +void vpUKSigmaDrawerMerwe_dummy() +{ + +} +#endif diff --git a/modules/core/src/math/kalman/vpUnscentedKalman.cpp b/modules/core/src/math/kalman/vpUnscentedKalman.cpp new file mode 100644 index 0000000000..c873e3b2b7 --- /dev/null +++ b/modules/core/src/math/kalman/vpUnscentedKalman.cpp @@ -0,0 +1,153 @@ +/* + * 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: + * Display a point cloud using PCL library. + */ + +/*! + \file vpUnscentedKalman.cpp + \brief Unscented Kalman filtering implementation. +*/ + +#include + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +vpUnscentedKalman::vpUnscentedKalman(const vpMatrix &Q, const vpMatrix &R, std::shared_ptr &drawer, const vpProcessFunction &f, const vpMeasurementFunction &h) + : m_Q(Q) + , m_R(R) + , m_f(f) + , m_h(h) + , m_sigmaDrawer(drawer) + , m_b(nullptr) + , m_bx(nullptr) + , m_measMeanFunc(vpUnscentedKalman::simpleMean) + , m_measResFunc(vpUnscentedKalman::simpleResidual) + , m_stateAddFunction(vpUnscentedKalman::simpleAdd) + , m_stateMeanFunc(vpUnscentedKalman::simpleMean) + , m_stateResFunc(vpUnscentedKalman::simpleResidual) +{ } + +void vpUnscentedKalman::init(const vpColVector &mu0, const vpMatrix &P0) +{ + m_Xest = mu0; + m_Pest = P0; +} + +void vpUnscentedKalman::filter(const vpColVector &z, const double &dt, const vpColVector &u) +{ + predict(dt, u); + update(z); +} + +void vpUnscentedKalman::predict(const double &dt, const vpColVector &u) +{ + // Drawing the sigma points + m_chi = m_sigmaDrawer->drawSigmaPoints(m_Xest, m_Pest); + + // Computation of the attached weights + vpUKSigmaDrawerAbstract::vpSigmaPointsWeights weights = m_sigmaDrawer->computeWeights(); + m_wm = weights.m_wm; + m_wc = weights.m_wc; + + // Computation of the prior based on the sigma points + unsigned int nbPoints = m_chi.size(); + if (m_Y.size() != nbPoints) { + m_Y.resize(nbPoints); + } + for (unsigned int i = 0; i < nbPoints; ++i) { + vpColVector prior = m_f(m_chi[i], dt); + if (m_b) { + prior = m_stateAddFunction(prior, m_b(u, dt)); + } + else if (m_bx) { + prior = m_stateAddFunction(prior, m_bx(u, m_chi[i], dt)); + } + m_Y[i] = prior; + } + + // Computation of the mean and covariance of the prior + vpUnscentedTransformResult transformResults = unscentedTransform(m_Y, m_wm, m_wc, m_Q, m_stateResFunc, m_stateMeanFunc); + m_mu = transformResults.m_mu; + m_P = transformResults.m_P; +} + +void vpUnscentedKalman::update(const vpColVector &z) +{ + unsigned int nbPoints = m_chi.size(); + if (m_Z.size() != nbPoints) { + m_Z.resize(nbPoints); + } + for (unsigned int i = 0; i < nbPoints; ++i) { + m_Z[i] = (m_h(m_Y[i])); + } + + // Computation of the mean and covariance of the prior expressed in the measurement space + vpUnscentedTransformResult transformResults = unscentedTransform(m_Z, m_wm, m_wc, m_R, m_measResFunc, m_measMeanFunc); + m_muz = transformResults.m_mu; + m_Pz = transformResults.m_P; + + // Computation of the Kalman gain + vpMatrix Pxz = m_wc[0] * m_stateResFunc(m_Y[0], m_mu) * m_measResFunc(m_Z[0], m_muz).transpose(); + unsigned int nbPts = m_wc.size(); + for (unsigned int i = 1; i < nbPts; ++i) { + Pxz += m_wc[i] * m_stateResFunc(m_Y[i], m_mu) * m_measResFunc(m_Z[i], m_muz).transpose(); + } + m_K = Pxz * m_Pz.inverseByCholesky(); + + // Updating the estimate + m_Xest = m_stateAddFunction(m_mu, m_K * m_measResFunc(z, m_muz)); + m_Pest = m_P - m_K * m_Pz * m_K.transpose(); +} + +vpUnscentedKalman::vpUnscentedTransformResult vpUnscentedKalman::unscentedTransform(const std::vector &sigmaPoints, + const std::vector &wm, const std::vector &wc, const vpMatrix &cov, + const vpAddSubFunction &resFunc, const vpMeanFunction &meanFunc +) +{ + vpUnscentedKalman::vpUnscentedTransformResult result; + + // Computation of the mean + result.m_mu = meanFunc(sigmaPoints, wm); + + // Computation of the covariance + result.m_P = cov; + unsigned int nbSigmaPoints = sigmaPoints.size(); + for (unsigned int i = 0; i < nbSigmaPoints; ++i) { + vpColVector e = resFunc(sigmaPoints[i], result.m_mu); + result.m_P += wc[i] * e*e.transpose(); + } + return result; +} +#else +void vpUnscentedKalman_dummy() +{ + +} +#endif diff --git a/modules/core/src/math/matrix/vpMatrix.cpp b/modules/core/src/math/matrix/vpMatrix.cpp index db8fc13027..18fec0aecd 100644 --- a/modules/core/src/math/matrix/vpMatrix.cpp +++ b/modules/core/src/math/matrix/vpMatrix.cpp @@ -6283,6 +6283,20 @@ double vpMatrix::det(vpDetMethod method) const return det; } +vpMatrix vpMatrix::cholesky() const +{ +#if defined(VISP_HAVE_EIGEN3) + return choleskyByEigen3(); +#elif defined(VISP_HAVE_LAPACK) + return choleskyByLapack(); +#elif defined(VISP_HAVE_OPENCV) + return choleskyByOpenCV(); +#else + throw(vpException(vpException::fatalError, "Cannot compute matrix Chloesky decomposition. " + "Install Lapack, Eigen3 or OpenCV 3rd party")); +#endif +} + /*! Compute the exponential matrix of a square matrix. diff --git a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp index 9822aa5fe4..30ae5de122 100644 --- a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp +++ b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp @@ -116,9 +116,7 @@ vpMatrix vpMatrix::inverseByCholesky() const #elif defined(VISP_HAVE_OPENCV) return inverseByCholeskyOpenCV(); #else - throw(vpException(vpException::fatalError, "Cannot inverse matrix by " - "Cholesky. Install Lapack or " - "OpenCV 3rd party")); + throw(vpException(vpException::fatalError, "Cannot inverse matrix by Cholesky. Install Lapack or OpenCV 3rd party")); #endif } @@ -213,6 +211,62 @@ vpMatrix vpMatrix::inverseByCholeskyLapack() const } #endif } + +/*! + * \brief Compute the Cholesky decomposition of a Hermitian positive-definite matrix + * using Lapack library. + * + * \return vpMatrix The lower triangular matrix resulting from the Cholesky decomposition + */ +vpMatrix vpMatrix::choleskyByLapack()const +{ + vpMatrix L = *this; +#if defined(VISP_HAVE_GSL) + gsl_matrix cholesky; + cholesky.size1 = rowNum; + cholesky.size2 = colNum; + cholesky.tda = cholesky.size2; + cholesky.data = L.data; + cholesky.owner = 0; + cholesky.block = 0; + +#if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 3) + gsl_linalg_cholesky_decomp1(&cholesky); +#else + gsl_linalg_cholesky_decomp(&cholesky); +#endif +#else + if (rowNum != colNum) { + throw(vpMatrixException(vpMatrixException::matrixError, "Cannot inverse a non-square matrix (%ux%u) by Cholesky", + rowNum, colNum)); + } + + integer rowNum_ = (integer)this->getRows(); + integer lda = (integer)rowNum_; // lda is the number of rows because we don't use a submatrix + integer info; + dpotrf_((char *)"L", &rowNum_, L.data, &lda, &info); + L = L.transpose(); // For an unknown reason, dpotrf seems to return the transpose of L + if (info < 0) { + std::stringstream errMsg; + errMsg << "The " << -info << "th argument has an illegal value" << std::endl; + throw(vpMatrixException(vpMatrixException::forbiddenOperatorError, errMsg.str())); + } + else if (info > 0) { + std::stringstream errMsg; + errMsg << "The leading minor of order" << info << "is not positive definite." << std::endl; + throw(vpMatrixException(vpMatrixException::forbiddenOperatorError, errMsg.str())); + } +#endif + // Make sure that the upper part of L is null + unsigned int nbRows = this->getRows(); + unsigned int nbCols = this->getCols(); + for (unsigned int r = 0; r < nbRows; ++r) { + for (unsigned int c = r + 1; c < nbCols; ++c) { + L[r][c] = 0.; + } + } + return L; +} #endif #if defined(VISP_HAVE_OPENCV) @@ -267,4 +321,61 @@ vpMatrix vpMatrix::inverseByCholeskyOpenCV() const return A; } + +/*! + * \brief Compute the Cholesky decomposition of a Hermitian positive-definite matrix + * using OpenCV library. + * + * \return vpMatrix The lower triangular matrix resulting from the Cholesky decomposition + */ +vpMatrix vpMatrix::choleskyByOpenCV() const +{ + cv::Mat M(rowNum, colNum, CV_64F); + memcpy(M.data, this->data, (size_t)(8 * M.rows * M.cols)); + std::size_t bytes_per_row = sizeof(double)*rowNum; + bool result = cv::Cholesky(M.ptr(), bytes_per_row, rowNum, nullptr, bytes_per_row, rowNum); + if (!result) { + throw(vpMatrixException(vpMatrixException::fatalError, "Could not compute the Cholesky's decomposition of the input matrix.")); + } + vpMatrix L(rowNum, colNum); + memcpy(L.data, M.data, (size_t)(8 * M.rows * M.cols)); + // Make sure that the upper part of L is null + unsigned int nbRows = this->getRows(); + unsigned int nbCols = this->getCols(); + for (unsigned int r = 0; r < nbRows; ++r) { + for (unsigned int c = r + 1; c < nbCols; ++c) { + L[r][c] = 0.; + } + } + return L; +} +#endif + +#if defined(VISP_HAVE_EIGEN3) +#include +/*! + * \brief Compute the Cholesky decomposition of a Hermitian positive-definite matrix + * using Eigen3 library. + * + * \return vpMatrix The lower triangular matrix resulting from the Cholesky decomposition + */ +vpMatrix vpMatrix::choleskyByEigen3() const +{ + unsigned int nbRows = this->getRows(); + unsigned int nbCols = this->getCols(); + Eigen::MatrixXd A(nbRows, nbCols); + for (unsigned int r = 0; r < nbRows; ++r) { + for (unsigned int c = 0; c < nbCols; ++c) { + A(r, c) = (*this)[r][c]; + } + } + Eigen::MatrixXd L = A.llt().matrixL(); + vpMatrix Lvisp(L.rows(), L.cols(), 0.); + for (unsigned int r = 0; r < nbRows; ++r) { + for (unsigned int c = 0; c <= r; ++c) { + Lvisp[r][c] = L(r, c); + } + } + return Lvisp; +} #endif diff --git a/modules/core/test/math/testMatrixCholesky.cpp b/modules/core/test/math/testMatrixCholesky.cpp new file mode 100644 index 0000000000..8f4572d0fa --- /dev/null +++ b/modules/core/test/math/testMatrixCholesky.cpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2023 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 matrix condition number. + * +*****************************************************************************/ + +/*! + \example testMatrixCholesky.cpp + \brief Test matrix Cholesky's decomposition. +*/ + +#include + +#ifdef VISP_HAVE_CATCH2 +#include +#include +#include + +#define CATCH_CONFIG_RUNNER +#include + + +bool equal(const vpMatrix &A, const vpMatrix &B) +{ + unsigned int rowsA = A.getRows(), rowsB = B.getRows(); + unsigned int colsA = A.getCols(), colsB = B.getCols(); + bool areEqual = (rowsA == rowsB) && (colsA == colsB); + + if (!areEqual) { + return false; + } + + for (unsigned int r = 0; r < rowsA; ++r) { + for (unsigned int c = 0; c < colsA; ++c) { + areEqual = areEqual && vpMath::equal(A[r][c], B[r][c]); + if (!areEqual) { + return false; + } + } + } + return areEqual; +} + +bool testCholeskyDecomposition(const vpMatrix &M, const vpMatrix >Result, const vpMatrix &result, + const std::string &title, const bool &verbose) +{ + if (verbose) { + std::cout << "-------------------------" << std::endl; + std::cout << "Test: " << title << std::endl; + std::cout << std::endl; + } + unsigned int gtRows = gtResult.getRows(), resRows = result.getRows(); + unsigned int gtCols = gtResult.getCols(), resCols = result.getCols(); + bool areEqual = (gtRows == resRows) && (gtCols == resCols); + if (!areEqual) { + if (verbose) { + std::cout << "Failed: dimensions mismatch (" << gtRows << " x " << gtCols << ")"; + std::cout << " vs (" << resRows << " x " << resCols << ")" << std::endl; + } + return false; + } + + areEqual = equal(gtResult, result); + + if ((!areEqual) && verbose) { + std::cout << "Failed: L matrices differ." << std::endl; + std::cout << "Result =\n" << result << std::endl; + std::cout << "GT =\n" << gtResult << std::endl; + return false; + } + + vpMatrix LLt = result * result.transpose(); + areEqual = equal(M, LLt); + + if ((!areEqual) && verbose) { + std::cout << "Failed: LL^T differ from M." << std::endl; + std::cout << "LL^T =\n" << LLt << std::endl; + std::cout << "GT M =\n" << M << std::endl; + } + if (areEqual && verbose) { + std::cout << "Test " << title << " succeeded" << std::endl; + } + + return areEqual; +} + + +TEST_CASE("3 x 3 input", "[cholesky]") +{ + vpMatrix M(3, 3); + M[0][0] = 4; M[0][1] = 12; M[0][2] = -16; + M[1][0] = 12; M[1][1] = 37; M[1][2] = -43; + M[2][0] = -16; M[2][1] = -43; M[2][2] = 98; + + vpMatrix gtL(3, 3, 0.); + gtL[0][0] = 2; // M[0][1] = 0; M[0][2] = 0; + gtL[1][0] = 6; gtL[1][1] = 1; // M[1][2] = 0; + gtL[2][0] = -8; gtL[2][1] = 5; gtL[2][2] = 3; + +#if defined(VISP_HAVE_LAPACK) + SECTION("LAPACK") + { + vpMatrix L = M.choleskyByLapack(); + CHECK(testCholeskyDecomposition(M, gtL, L, "Test 1 Cholesky's decomposition using Lapack", true)); + } +#endif + +#if defined(VISP_HAVE_EIGEN3) + SECTION("EIGEN3") + { + vpMatrix L = M.choleskyByEigen3(); + CHECK(testCholeskyDecomposition(M, gtL, L, "Test Cholesky's decomposition using Eigen3", true)); + } +#endif + +#if defined(VISP_HAVE_OPENCV) + SECTION("OPENCV") + { + vpMatrix L = M.choleskyByOpenCV(); + CHECK(testCholeskyDecomposition(M, gtL, L, "Test Cholesky's decomposition using OpenCV", true)); + } +#endif +} + +TEST_CASE("4 x 4 input", "[cholesky]") +{ + vpMatrix M(4, 4); + M[0][0] = 4.16; M[0][1] = -3.12; M[0][2] = 0.56; M[0][3] = -0.10; + M[1][0] = -3.12; M[1][1] = 5.03; M[1][2] = -0.83; M[1][3] = 1.18; + M[2][0] = 0.56; M[2][1] = -0.83; M[2][2] = 0.76; M[2][3] = 0.34; + M[3][0] = -0.10; M[3][1] = 1.18; M[3][2] = 0.34; M[3][3] = 1.18; + + vpMatrix gtL(4, 4, 0.); + gtL[0][0] = 2.0396; + gtL[1][0] = -1.5297; gtL[1][1] = 1.6401; + gtL[2][0] = 0.2746; gtL[2][1] = -0.2500; gtL[2][2] = 0.7887; + gtL[3][0] = -0.0490; gtL[3][1] = 0.6737; gtL[3][2] = 0.6617; gtL[3][3] = 0.5347; + +#if defined(VISP_HAVE_LAPACK) + SECTION("LAPACK") + { + vpMatrix L = M.choleskyByLapack(); + CHECK(testCholeskyDecomposition(M, gtL, L, "Test 1 Cholesky's decomposition using Lapack", true)); + } +#endif + +#if defined(VISP_HAVE_EIGEN3) + SECTION("EIGEN3") + { + vpMatrix L = M.choleskyByEigen3(); + CHECK(testCholeskyDecomposition(M, gtL, L, "Test Cholesky's decomposition using Eigen3", true)); + } +#endif + +#if defined(VISP_HAVE_OPENCV) + SECTION("OPENCV") + { + vpMatrix L = M.choleskyByOpenCV(); + CHECK(testCholeskyDecomposition(M, gtL, L, "Test Cholesky's decomposition using OpenCV", true)); + } +#endif +} + +int main(int argc, char *argv[]) +{ + Catch::Session session; // There must be exactly one instance + + // 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() +{ + std::cout << "Test ignored: Catch2 is not available" << std::endl; + return EXIT_SUCCESS; +} +#endif diff --git a/modules/python/bindings/include/blob.hpp b/modules/python/bindings/include/blob.hpp index e64420faf4..07cf661513 100644 --- a/modules/python/bindings/include/blob.hpp +++ b/modules/python/bindings/include/blob.hpp @@ -43,7 +43,7 @@ namespace py = pybind11; -void bindings_vpDot2(py::class_ &pyDot2) +void bindings_vpDot2(py::class_, vpTracker> &pyDot2) { pyDot2.def_static("defineDots", [](std::vector &dots, const std::string &dotFile, diff --git a/modules/python/bindings/include/core/arrays.hpp b/modules/python/bindings/include/core/arrays.hpp index 6e365e9da4..484f75affe 100644 --- a/modules/python/bindings/include/core/arrays.hpp +++ b/modules/python/bindings/include/core/arrays.hpp @@ -218,7 +218,7 @@ const char *numpy_fn_doc_nonwritable = R"doc( )doc"; template -void bindings_vpArray2D(py::class_> &pyArray2D) +void bindings_vpArray2D(py::class_, std::shared_ptr>> &pyArray2D) { pyArray2D.def_buffer(&get_buffer_info); @@ -239,10 +239,10 @@ Construct a 2D ViSP array by **copying** a 2D numpy array. )doc", py::arg("np_array")); - define_get_item_2d_array>, vpArray2D, T>(pyArray2D); + define_get_item_2d_array, std::shared_ptr>>, vpArray2D, T>(pyArray2D); } -void bindings_vpMatrix(py::class_> &pyMatrix) +void bindings_vpMatrix(py::class_, vpArray2D> &pyMatrix) { pyMatrix.def_buffer(&get_buffer_info); @@ -268,11 +268,11 @@ Construct a matrix by **copying** a 2D numpy array. add_print_helper(pyMatrix, &vpMatrix::matlabPrint, "strMatlab", matlab_str_help); add_cpp_print_helper(pyMatrix, &vpMatrix::cppPrint); - define_get_item_2d_array>, vpMatrix, double>(pyMatrix); + define_get_item_2d_array, vpArray2D>, vpMatrix, double>(pyMatrix); } -void bindings_vpRotationMatrix(py::class_> &pyRotationMatrix) +void bindings_vpRotationMatrix(py::class_, vpArray2D> &pyRotationMatrix) { pyRotationMatrix.def_buffer(&get_buffer_info); @@ -296,10 +296,10 @@ If it is not a rotation matrix, an exception will be raised. :param np_array: The numpy 1D array to copy. )doc", py::arg("np_array")); - define_get_item_2d_array>, vpRotationMatrix, double>(pyRotationMatrix); + define_get_item_2d_array, vpArray2D>, vpRotationMatrix, double>(pyRotationMatrix); } -void bindings_vpHomogeneousMatrix(py::class_> &pyHomogeneousMatrix) +void bindings_vpHomogeneousMatrix(py::class_, vpArray2D> &pyHomogeneousMatrix) { pyHomogeneousMatrix.def_buffer(get_buffer_info); pyHomogeneousMatrix.def("numpy", [](vpHomogeneousMatrix &self) -> np_array_cf { @@ -323,12 +323,12 @@ If it is not a homogeneous matrix, an exception will be raised. :param np_array: The numpy 1D array to copy. )doc", py::arg("np_array")); - define_get_item_2d_array>, vpHomogeneousMatrix, double>(pyHomogeneousMatrix); + define_get_item_2d_array, vpArray2D>, vpHomogeneousMatrix, double>(pyHomogeneousMatrix); } -void bindings_vpTranslationVector(py::class_> &pyTranslationVector) +void bindings_vpTranslationVector(py::class_, vpArray2D> &pyTranslationVector) { pyTranslationVector.def_buffer(&get_buffer_info); @@ -349,11 +349,11 @@ Construct a Translation vector by **copying** a 1D numpy array of size 3. :param np_array: The numpy 1D array to copy. )doc", py::arg("np_array")); - define_get_item_1d_array>, vpTranslationVector, double>(pyTranslationVector); + define_get_item_1d_array, vpArray2D>, vpTranslationVector, double>(pyTranslationVector); } -void bindings_vpColVector(py::class_> &pyColVector) +void bindings_vpColVector(py::class_, vpArray2D> &pyColVector) { pyColVector.def_buffer(&get_buffer_info); @@ -373,7 +373,7 @@ Construct a column vector by **copying** a 1D numpy array. :param np_array: The numpy 1D array to copy. )doc", py::arg("np_array")); - define_get_item_1d_array>, vpColVector, double>(pyColVector); + define_get_item_1d_array, vpArray2D>, vpColVector, double>(pyColVector); add_print_helper(pyColVector, &vpColVector::csvPrint, "strCsv", csv_str_help); add_print_helper(pyColVector, &vpColVector::maplePrint, "strMaple", maple_str_help); @@ -382,7 +382,7 @@ Construct a column vector by **copying** a 1D numpy array. } -void bindings_vpRowVector(py::class_> &pyRowVector) +void bindings_vpRowVector(py::class_, vpArray2D> &pyRowVector) { pyRowVector.def_buffer(&get_buffer_info); pyRowVector.def("numpy", [](vpRowVector &self) -> np_array_cf { @@ -400,7 +400,7 @@ Construct a row vector by **copying** a 1D numpy array. :param np_array: The numpy 1D array to copy. )doc", py::arg("np_array")); - define_get_item_1d_array>, vpRowVector, double>(pyRowVector); + define_get_item_1d_array, vpArray2D>, vpRowVector, double>(pyRowVector); add_print_helper(pyRowVector, &vpRowVector::csvPrint, "strCsv", csv_str_help); add_print_helper(pyRowVector, &vpRowVector::maplePrint, "strMaple", maple_str_help); add_print_helper(pyRowVector, &vpRowVector::matlabPrint, "strMatlab", matlab_str_help); diff --git a/modules/python/bindings/include/core/image_conversions.hpp b/modules/python/bindings/include/core/image_conversions.hpp index 91d8045157..b0d5c04810 100644 --- a/modules/python/bindings/include/core/image_conversions.hpp +++ b/modules/python/bindings/include/core/image_conversions.hpp @@ -75,7 +75,7 @@ struct SimpleConversionStruct unsigned int srcBytesPerPixel; unsigned int destBytesPerPixel; - void add_conversion_binding(py::class_ &pyImageConvert) + void add_conversion_binding(py::class_> &pyImageConvert) { pyImageConvert.def_static(name.c_str(), [this](py::array_t &src, py::array_t &dest) { @@ -123,7 +123,7 @@ struct SimpleConversionStruct unsigned int srcBytesPerPixel; unsigned int destBytesPerPixel; - void add_conversion_binding(py::class_ &pyImageConvert) + void add_conversion_binding(py::class_> &pyImageConvert) { pyImageConvert.def_static(name.c_str(), [this](py::array_t &src, py::array_t &dest, bool flip) { @@ -172,7 +172,7 @@ struct ConversionFromYUVLike unsigned int destBytesPerPixel; - void add_conversion_binding(py::class_ &pyImageConvert) + void add_conversion_binding(py::class_> &pyImageConvert) { pyImageConvert.def_static(name.c_str(), [this](py::array_t &src, py::array_t &dest) { @@ -228,26 +228,26 @@ unsigned size411(unsigned h, unsigned w) void rgb_or_rgba_to_hsv_verification(const py::buffer_info &bufsrc, const py::buffer_info &bufdest, const unsigned destBytes, const unsigned height, const unsigned width) { - if (bufsrc.ndim != 3 || bufdest.ndim != 3) { - throw std::runtime_error("Expected to have src and dest arrays with at least two dimensions."); - } - if (bufdest.shape[0] != 3) { - throw std::runtime_error("Source array should be a 3D array of shape (3, H, W) "); - } - if (bufsrc.shape[2] != destBytes) { - std::stringstream ss; - ss << "Target array should be a 3D array of shape (H, W, " << destBytes << ")"; - throw std::runtime_error(ss.str()); - } - if (bufsrc.shape[0] != height || bufsrc.shape[1] != width) { - std::stringstream ss; - ss << "src and dest must have the same number of pixels, but got HSV planes with dimensions (" << height << ", " << width << ")"; - ss << "and RGB array with dimensions (" << bufdest.shape[0] << ", " << bufdest.shape[1] << ")"; - throw std::runtime_error(ss.str()); - } + if (bufsrc.ndim != 3 || bufdest.ndim != 3) { + throw std::runtime_error("Expected to have src and dest arrays with at least two dimensions."); + } + if (bufdest.shape[0] != 3) { + throw std::runtime_error("Source array should be a 3D array of shape (3, H, W) "); + } + if (bufsrc.shape[2] != destBytes) { + std::stringstream ss; + ss << "Target array should be a 3D array of shape (H, W, " << destBytes << ")"; + throw std::runtime_error(ss.str()); + } + if (bufsrc.shape[0] != height || bufsrc.shape[1] != width) { + std::stringstream ss; + ss << "src and dest must have the same number of pixels, but got HSV planes with dimensions (" << height << ", " << width << ")"; + ss << "and RGB array with dimensions (" << bufdest.shape[0] << ", " << bufdest.shape[1] << ")"; + throw std::runtime_error(ss.str()); + } } -void add_hsv_double_to_rgb_or_rgba_binding(py::class_ &pyImageConvert, +void add_hsv_double_to_rgb_or_rgba_binding(py::class_> &pyImageConvert, void (*fn)(const double *, const double *, const double *, unsigned char *, unsigned int), const char *name, const unsigned destBytes) { pyImageConvert.def_static(name, [fn, destBytes](py::array_t &src, @@ -266,7 +266,7 @@ void add_hsv_double_to_rgb_or_rgba_binding(py::class_ &pyImageCo }, "Convert from HSV Planes (as a 3 x H x W array) to a an RGB/RGBA array (as an H x W x 3 or H x W x 4 array)", py::arg("hsv"), py::arg("rgb")); } -void add_hsv_uchar_to_rgb_or_rgba_binding(py::class_ &pyImageConvert, +void add_hsv_uchar_to_rgb_or_rgba_binding(py::class_> &pyImageConvert, void (*fn)(const unsigned char *, const unsigned char *, const unsigned char *, unsigned char *, unsigned int, bool), const char *name, const unsigned destBytes) { pyImageConvert.def_static(name, [fn, destBytes](py::array_t &src, @@ -282,10 +282,10 @@ void add_hsv_uchar_to_rgb_or_rgba_binding(py::class_ &pyImageCon unsigned char *dest_ptr = static_cast(bufdest.ptr); fn(h, s, v, dest_ptr, height * width, h_full); - }, "Convert from HSV Planes (as a 3 x H x W array) to a an RGB/RGBA array (as an H x W x 3 or H x W x 4 array)", py::arg("hsv"), py::arg("rgb"), py::arg("h_full")=true); + }, "Convert from HSV Planes (as a 3 x H x W array) to a an RGB/RGBA array (as an H x W x 3 or H x W x 4 array)", py::arg("hsv"), py::arg("rgb"), py::arg("h_full") = true); } -void add_rgb_or_rgba_uchar_to_hsv_binding(py::class_ &pyImageConvert, +void add_rgb_or_rgba_uchar_to_hsv_binding(py::class_> &pyImageConvert, void (*fn)(const unsigned char *, unsigned char *, unsigned char *, unsigned char *, unsigned int, bool), const char *name, const unsigned destBytes) { pyImageConvert.def_static(name, [fn, destBytes](py::array_t &src, @@ -302,10 +302,10 @@ void add_rgb_or_rgba_uchar_to_hsv_binding(py::class_ &pyImageCon const unsigned char *rgb = static_cast(bufsrc.ptr); fn(rgb, h, s, v, height * width, h_full); - }, "Convert from HSV Planes (as a 3 x H x W array) to a an RGB/RGBA array (as an H x W x 3 or H x W x 4 array)", py::arg("rgb"), py::arg("hsv"), py::arg("h_full")=true); + }, "Convert from HSV Planes (as a 3 x H x W array) to a an RGB/RGBA array (as an H x W x 3 or H x W x 4 array)", py::arg("rgb"), py::arg("hsv"), py::arg("h_full") = true); } -void add_rgb_or_rgba_double_to_hsv_binding(py::class_ &pyImageConvert, +void add_rgb_or_rgba_double_to_hsv_binding(py::class_> &pyImageConvert, void (*fn)(const unsigned char *, double *, double *, double *, unsigned int), const char *name, const unsigned destBytes) { pyImageConvert.def_static(name, [fn, destBytes](py::array_t &src, @@ -326,7 +326,7 @@ void add_rgb_or_rgba_double_to_hsv_binding(py::class_ &pyImageCo /* Demosaicing implem */ template -void add_demosaic_to_rgba_fn(py::class_ &pyImageConvert, void (*fn)(const DataType *, DataType *, unsigned int, unsigned int, unsigned int), const char *name) +void add_demosaic_to_rgba_fn(py::class_> &pyImageConvert, void (*fn)(const DataType *, DataType *, unsigned int, unsigned int, unsigned int), const char *name) { pyImageConvert.def_static(name, [fn](py::array_t &src, py::array_t &dest, @@ -360,7 +360,7 @@ void add_demosaic_to_rgba_fn(py::class_ &pyImageConvert, void (* } -void bindings_vpImageConvert(py::class_ &pyImageConvert) +void bindings_vpImageConvert(py::class_> &pyImageConvert) { // Simple conversions where the size input is a single argument { diff --git a/modules/python/bindings/include/core/images.hpp b/modules/python/bindings/include/core/images.hpp index 3303b341b0..2d7bd3630d 100644 --- a/modules/python/bindings/include/core/images.hpp +++ b/modules/python/bindings/include/core/images.hpp @@ -53,7 +53,7 @@ const char *numpy_fn_doc_image = R"doc( * Image 2D indexing */ template -void define_get_item_2d_image(py::class_> &pyClass) +void define_get_item_2d_image(py::class_, std::shared_ptr>> &pyClass) { pyClass.def("__getitem__", [](const vpImage &self, std::pair pair) -> T { int i = pair.first, j = pair.second; @@ -98,7 +98,7 @@ void define_get_item_2d_image(py::class_> &pyClass) */ template typename std::enable_if::value, void>::type -bindings_vpImage(py::class_> &pyImage) +bindings_vpImage(py::class_, std::shared_ptr>> &pyImage) { pyImage.def_buffer([](vpImage &image) -> py::buffer_info { return make_array_buffer(image.bitmap, { image.getHeight(), image.getWidth() }, false); @@ -138,7 +138,7 @@ Construct an image by **copying** a 2D numpy array. template typename std::enable_if::value, void>::type -bindings_vpImage(py::class_> &pyImage) +bindings_vpImage(py::class_, std::shared_ptr>> &pyImage) { using NpRep = unsigned char; static_assert(sizeof(T) == 4 * sizeof(NpRep)); @@ -182,7 +182,7 @@ where the 4 denotes the red, green, blue and alpha components of the image. } template typename std::enable_if::value, void>::type -bindings_vpImage(py::class_> &pyImage) +bindings_vpImage(py::class_, std::shared_ptr>> &pyImage) { using NpRep = float; static_assert(sizeof(T) == 3 * sizeof(NpRep)); diff --git a/modules/python/bindings/include/core/pixel_meter.hpp b/modules/python/bindings/include/core/pixel_meter.hpp index 3e4a08f6b1..627b5ef3d5 100644 --- a/modules/python/bindings/include/core/pixel_meter.hpp +++ b/modules/python/bindings/include/core/pixel_meter.hpp @@ -44,7 +44,7 @@ #include "core/utils.hpp" -void bindings_vpPixelMeterConversion(py::class_ &pyPM) +void bindings_vpPixelMeterConversion(py::class_> &pyPM) { pyPM.def_static("convertPoints", [](const vpCameraParameters &cam, const py::array_t &us, const py::array_t &vs) { py::buffer_info bufu = us.request(), bufv = vs.request(); @@ -107,7 +107,7 @@ Example usage: )doc", py::arg("cam"), py::arg("us"), py::arg("vs")); } -void bindings_vpMeterPixelConversion(py::class_ &pyMP) +void bindings_vpMeterPixelConversion(py::class_> &pyMP) { pyMP.def_static("convertPoints", [](const vpCameraParameters &cam, const py::array_t &xs, const py::array_t &ys) { py::buffer_info bufx = xs.request(), bufy = ys.request(); diff --git a/modules/python/bindings/include/mbt.hpp b/modules/python/bindings/include/mbt.hpp index 4ef94b65d4..8298b90e9b 100644 --- a/modules/python/bindings/include/mbt.hpp +++ b/modules/python/bindings/include/mbt.hpp @@ -42,7 +42,7 @@ namespace py = pybind11; -void bindings_vpMbGenericTracker(py::class_ &pyMbGenericTracker) +void bindings_vpMbGenericTracker(py::class_, vpMbTracker> &pyMbGenericTracker) { pyMbGenericTracker.def("track", [](vpMbGenericTracker &self, std::map *> &mapOfImages, std::map> &mapOfPointClouds) { diff --git a/modules/python/config/core.json b/modules/python/config/core.json index 49797242f6..8189dafc0a 100644 --- a/modules/python/config/core.json +++ b/modules/python/config/core.json @@ -5,6 +5,9 @@ "vpDisplayException", "vpMatrixException"], "user_defined_headers": ["core.hpp"], "config_includes": ["core_image.json", "core_math.json"], + "header_additional_dependencies": { + "vpUKSigmaDrawerMerwe.h": ["vpUnscentedKalman.h"] + }, "enums": { "vpMunkres::STEP_T": { "ignore": true diff --git a/modules/python/doc/rst/dev/config.rst b/modules/python/doc/rst/dev/config.rst index 50f9f58736..6cbb6f3370 100644 --- a/modules/python/doc/rst/dev/config.rst +++ b/modules/python/doc/rst/dev/config.rst @@ -45,6 +45,9 @@ Module-level options "ignored_headers": ["vpGEMM.h", "vpDebug.h"], "ignored_classes": ["vpException", "vpImageException"], "user_defined_headers": ["core.hpp"], + "header_additional_dependencies": { + "vpUKSigmaDrawerMerwe.h": ["vpUnscentedKalman.h"] + }, "enums": {}, "classes": {}, "functions": {} @@ -74,6 +77,11 @@ Module-level options - Paths to user-defined bindings that will be called when generating the overall bindings (see class options and :ref:`Custom binding`). These paths are relative to the **modules/python/bindings/include** folder. If a file does not exist, an error will be raised at compile time. + * - :code:`header_additional_dependencies` + - Dictionary of header name to list of header names + - Forcefully specify that a header (defined as a key of the dictionary) depends on all the headers listed as the values. + This is helpful if your header depends on typedefs defined in other non-related (i.e., not linked by inheritance) files. + In the example above, vpUKSigmaDrawerMerwe uses a typedef from vpUnscentedKalman, but the link is not seen by the generator. * - :code:`enums` - Dictionary - Mapping from C++ enum name to an :ref:`enum configuration `. diff --git a/modules/python/examples/ukf-linear-example.py b/modules/python/examples/ukf-linear-example.py new file mode 100644 index 0000000000..1e27fde959 --- /dev/null +++ b/modules/python/examples/ukf-linear-example.py @@ -0,0 +1,230 @@ +############################################################################# +# +# ViSP, open source Visual Servoing Platform software. +# Copyright (C) 2005 - 2023 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 Python example to simulate an image-based visual servo on four 2D points +# +############################################################################# + +""" @example ukf-linear-example.py + Example of a simple linear use-case of the Unscented Kalman Filter (UKF). Using a UKF + in this case is not necessary, it is done for learning purpous only. + + The system we are interested in is a system moving on a 2D-plane. + + The state vector of the UKF is: + \f{eqnarray*}{ + \textbf{x}[0] &=& x \\ + \textbf{x}[1] &=& \dot{x} \\ + \textbf{x}[1] &=& y \\ + \textbf{x}[2] &=& \dot{y} + \f} + + The measurement \f$ \textbf{z} \f$ corresponds to the position along the x-axis + and y-axis. The measurement vector can be written as: + \f{eqnarray*}{ + \textbf{z}[0] &=& x \\ + \textbf{z}[1] &=& y + \f} + + Some noise is added to the measurement vector to simulate a sensor which is + not perfect. +""" + +from visp.core import ColVector, Matrix, UnscentedKalman, UKSigmaDrawerMerwe, GaussRand + +# For the Graphical User Interface +try: + from visp.gui import Plot + has_gui = True +except: + has_gui = False +import numpy as np + +def fx(x: ColVector, dt: float) -> ColVector: + """ + @brief Process function that projects in time the internal state of the UKF. + + @param x The internal state of the UKF. + @param dt The sampling time: how far in the future are we projecting x. + + @return ColVector The updated internal state, projected in time, also known as the prior. + """ + return ColVector([ + x[0] + dt * x[1], + x[1], + x[2] + dt * x[3], + x[3] + ]) + + +def hx(x: ColVector) -> ColVector: + """ + @brief Measurement function that expresses the internal state of the UKF in the measurement space. + + @param x The internal state of the UKF. + + @return ColVector The internal state, expressed in the measurement space. + """ + return ColVector([ + x[0], + x[2] + ]) + +def add_state_vectors(a, b) -> ColVector: + """ + @brief Method that permits to add two state vectors. + + @param a The first state vector to which another state vector must be added. + @param b The other state vector that must be added to a. + + @return ColVector The sum a + b. + """ + return a + b + +def residual_state_vectors(a, b) -> ColVector: + """ + @brief Method that permits to substract a state vector to another. + + @param a The first state vector to which another state vector must be substracted. + @param b The other state vector that must be substracted to a. + + @return ColVector The substraction a - b. + """ + return a - b + +def generate_Q_matrix(dt: float) -> Matrix: + """ + @brief Method that generates the process covariance matrix for a process for which the + state vector can be written as (x, dx/dt)^T + + @param dt The sampling period. + + @return Matrix The corresponding process covariance matrix. + """ + return Matrix( + [[dt**3/3, dt**2/2, 0, 0], + [dt**2/2, dt, 0, 0], + [0, 0, dt**3/3, dt**2/2], + [0, 0, dt**2/2, dt]]) + +if __name__ == '__main__': + dt = 0.01 # The sampling period + gt_dx = 0.01 # The displacement along the x-axis between two timesteps + gt_dy = 0.005 # The displacement along the y-axis between two timesteps + gt_dX = ColVector([gt_dx, gt_dy]) # The displacement vector between two timesteps + gt_vx = gt_dx / dt # The velocity along the x-axis + gt_vy = gt_dy / dt # The velocity along the y-axis + procVar = 0.000004 # The variance of the process function + sigmaXmeas = 0.05 # The standard deviation of the measurement noise for the x-axis measurement + sigmaYmeas = 0.05 # The standard deviation of the measurement noise for the y-axis measurement + + # The object that draws the sigma points used by the UKF + drawer = UKSigmaDrawerMerwe(n=4, alpha=0.3, beta=2, kappa=-1, resFunc=residual_state_vectors, addFunc=add_state_vectors) + + P0 = Matrix(np.eye(4) * 1.) # The initial estimate of the state covariance matrix + R = Matrix(np.eye(2) * 0.01) # The measurement covariance matrix + Q = generate_Q_matrix(dt) * procVar # The process covariance matrix + ukf = UnscentedKalman(Q, R, drawer, fx, hx) # The Unscented Kalman Filter instance + + # Initializing the state vector and state covariance matrix estimates + ukf.init(ColVector([0., 0.75 * gt_vx, 0., 0.75 * gt_vy]), P0) + + # Initializing the Graphical User Interface if the needed libraries are available + if has_gui: + num_plots = 4 + plot = Plot(num_plots) + plot_titles = [ + 'Position along X-axis', 'Velocity along X-axis', + 'Position along Y-axis', 'Velocity along Y-axis' + ] + plot_y_units = [ + 'Position (m)', 'Velocity (m/s)', + 'Position (m)', 'Velocity (m/s)' + ] + plot_legends = ['GT', 'Measure', 'Filtered'] + + # Initializing the subplots + for plot_index in range(num_plots): + plot.initGraph(plot_index, len(plot_legends)) + plot.setTitle(plot_index, plot_titles[plot_index]) + plot.setUnitY(plot_index, plot_y_units[plot_index]) + plot.setUnitX(plot_index, 'Time (s)') + for legend_index in range(len(plot_legends)): + plot.setLegend(plot_index, legend_index, plot_legends[legend_index]) + + gt_X = ColVector(2, 0.) # Ground truth position + z_prec = ColVector(2, 0.) # Previous measurement vector + np.random.seed(4224) + + rngX = GaussRand(sigmaXmeas, 0., 4224) # Gaussian noise random generator for x-axis measurement + rngY = GaussRand(sigmaYmeas, 0., 2112) # Gaussian noise random generator for y-axis measurement + for i in range(100): + # Creating noisy measurements + x_meas = gt_X[0] + np.random.normal(0.0, sigmaXmeas) + y_meas = gt_X[1] + np.random.normal(0.0, sigmaYmeas) + z = ColVector([x_meas, y_meas]) + + # Filtering using the UKF + ukf.filter(z, dt) + + # Getting the filtered state vector + Xest = ukf.getXest() + + # Computing the noisy velocity from the measueremnts + vX_meas = (x_meas - z_prec[0]) / dt + vY_meas = (y_meas - z_prec[1]) / dt + + # Update the GUI if available + if has_gui: + plot.plot(0, 0, i, gt_X[0]) + plot.plot(0, 1, i, x_meas) + plot.plot(0, 2, i, Xest[0]) + + plot.plot(1, 0, i, gt_vx) + plot.plot(1, 1, i, vX_meas) + plot.plot(1, 2, i, Xest[1]) + + plot.plot(2, 0, i, gt_X[1]) + plot.plot(2, 1, i, y_meas) + plot.plot(2, 2, i, Xest[2]) + + plot.plot(3, 0, i, gt_vy) + plot.plot(3, 1, i, vY_meas) + plot.plot(3, 2, i, Xest[3]) + + # Updating ground-truth position + gt_X += gt_dX + + # Updating last measurement for future computation of the noisy velocity + z_prec = z + + print('Finished') + input('Press enter to quit') diff --git a/modules/python/examples/ukf-nonlinear-example.py b/modules/python/examples/ukf-nonlinear-example.py new file mode 100644 index 0000000000..5f10197dee --- /dev/null +++ b/modules/python/examples/ukf-nonlinear-example.py @@ -0,0 +1,367 @@ +############################################################################# +# +# ViSP, open source Visual Servoing Platform software. +# Copyright (C) 2005 - 2023 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 Python example to simulate an image-based visual servo on four 2D points +# +############################################################################# + +""" @example ukf-nonlinear-example.py + Example of a simple non-linear use-case of the Unscented Kalman Filter (UKF). + + The system we are interested in is an aircraft flying in the sky and + observed by a radar station. + + 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 UKF is: + \f{eqnarray*}{ + \textbf{x}[0] &=& x \\ + \textbf{x}[1] &=& \dot{x} \\ + \textbf{x}[1] &=& y \\ + \textbf{x}[2] &=& \dot{y} + \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{eqnarray*}{ + \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}} + \f} + + Some noise is added to the measurement vector to simulate a sensor which is + not perfect. +""" + +from visp.core import ColVector, Matrix, UnscentedKalman, UKSigmaDrawerMerwe, GaussRand, Math +import numpy as np +from typing import List +from math import cos, sin + +# For the Graphical User Interface +try: + from visp.gui import Plot + has_gui = True +except: + has_gui = False +import numpy as np + +def normalizeAngle(angle: float) -> float: + angleIn0to2pi = Math.modulo(angle, 2. * np.pi) + angleInMinPiPi = angleIn0to2pi + if angleInMinPiPi > np.pi: + # Substract 2 PI to be in interval [-Pi; Pi] + angleInMinPiPi = angleInMinPiPi - 2. * np.pi + return angleInMinPiPi + +def measurementMean(measurements: List[ColVector], wm: List[float]) -> ColVector: + """ + @brief Compute the weighted mean of measurement vectors. + + @param[in] measurements The measurement vectors, such as measurements[i][0] = range and + measurements[i][1] = elevation_angle. + @param[in] wm The associated weights. + + @return vpColVector + """ + nbPoints = len(measurements) + sumCos = 0. + sumSin = 0. + meanRange = 0. + for i in range(nbPoints): + meanRange += wm[i] * measurements[i][0] + sumCos += wm[i] * cos(measurements[i][1]) + sumSin += wm[i] * sin(measurements[i][1]) + + meanAngle = np.arctan2(sumSin, sumCos) + + return ColVector([meanRange, meanAngle]) + +def measurementResidual(meas: ColVector, toSubstract: ColVector) -> ColVector: + """ + @brief Compute the substraction between two vectors expressed in the measurement space, + such as v[0] = range ; v[1] = elevation_angle + + @param[in] meas Measurement to which we must substract something. + @param[in] toSubstract The something we must substract. + + @return vpColVector \b meas - \b toSubstract . + """ + resTemp = meas - toSubstract + return ColVector([resTemp[0], normalizeAngle(resTemp[1])]) + +def add_state_vectors(a, b) -> ColVector: + """ + @brief Method that permits to add two state vectors. + + @param a The first state vector to which another state vector must be added. + @param b The other state vector that must be added to a. + + @return ColVector The sum a + b. + """ + return a + b + +def residual_state_vectors(a, b) -> ColVector: + """ + @brief Method that permits to substract a state vector to another. + + @param a The first state vector to which another state vector must be substracted. + @param b The other state vector that must be substracted to a. + + @return ColVector The substraction a - b. + """ + return a - b + +def fx(x: ColVector, dt: float) -> ColVector: + """ + @brief Process function that projects in time the internal state of the UKF. + + @param x The internal state of the UKF. + @param dt The sampling time: how far in the future are we projecting x. + + @return ColVector The updated internal state, projected in time, also known as the prior. + """ + return ColVector([ + x[0] + dt * x[1], + x[1], + x[2] + dt * x[3], + x[3] + ]) + +class vpRadarStation: + """ + @brief Class that permits to convert the position of the aircraft into + range and elevation angle measurements. + """ + def __init__(self, x, y, range_std, elev_angle_std): + """ + @brief Construct a new vpRadarStation + + @param x The position on the ground of the radar. + @param y The altitude of the radar. + @param range_std The standard deviation of the range measurements. + @param elev_angle_std The standard deviation of the elevation angle measurements. + """ + self.m_x = x + self.m_y = y + self.m_stdevRange = range_std + self.m_stdevElevAngle = elev_angle_std + + def state_to_measurement(self, x) -> ColVector: + """ + @brief Measurement function that expresses the internal state of the UKF in the measurement space. + + @param x The internal state of the UKF. + + @return ColVector The internal state, expressed in the measurement space. + """ + dx = x[0] - self.m_x + dy = x[2] - self.m_y + range = np.sqrt(dx * dx + dy * dy) + elev_angle = np.arctan2(dy, dx) + return ColVector([range, elev_angle]) + + def measureGT(self, pos) -> ColVector: + """ + @brief Perfect measurement of the range and elevation angle that + correspond to pos. + + @param pos The actual position of the aircraft (pos[0]: projection of the position + on the ground, pos[1]: altitude). + + @return ColVector [0] the range [1] the elevation angle. + """ + dx = pos[0] - self.m_x + dy = pos[1] - self.m_y + range = np.sqrt(dx * dx + dy * dy) + elev_angle = np.arctan2(dy, dx) + return ColVector([range, elev_angle]) + + def measureWithNoise(self, pos: ColVector) -> ColVector: + """ + @brief Noisy measurement of the range and elevation angle that + correspond to pos. + + @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. + """ + measurementsGT = self.measureGT(pos) + measurementsNoisy = ColVector([measurementsGT[0] + np.random.normal(0., self.m_stdevRange), measurementsGT[1] + np.random.normal(0., self.m_stdevElevAngle)]) + return measurementsNoisy + +class vpACSimulator: + """ + @brief Class to simulate a flying aircraft. + """ + + def __init__(self, X0: ColVector, vel: ColVector, vel_std: float): + """ + @brief Construct a new vpACSimulator object. + + @param X0 Initial position of the aircraft. + @param vel Velocity of the aircraft. + @param vel_std Standard deviation of the variation of the velocity. + """ + self.m_pos = X0 # Position of the simulated aircraft + self.m_vel = vel # Velocity of the simulated aircraft + np.random.seed(4224) + self.m_stdevVel = vel_std # Standard deviation of the random generator for slight variations of the velocity of the aircraft + + + def update(self, dt: float) -> ColVector: + """ + \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. + """ + dx_temp = self.m_vel * dt + dx = ColVector([dx_temp[0] + np.random.normal(0., self.m_stdevVel) * dt, dx_temp[1] + np.random.normal(0., self.m_stdevVel) * dt]) + self.m_pos += dx + return self.m_pos + +def generate_Q_matrix(dt: float) -> Matrix: + """ + @brief Method that generates the process covariance matrix for a process for which the + state vector can be written as (x, dx/dt)^T + + @param dt The sampling period. + + @return Matrix The corresponding process covariance matrix. + """ + return Matrix( + [[dt**3/3, dt**2/2, 0, 0], + [dt**2/2, dt, 0, 0], + [0, 0, dt**3/3, dt**2/2], + [0, 0, dt**2/2, dt]]) + +def generate_P0_matrix() -> Matrix: + """ + @brief Method that generates the intial guess of the state covariance matrix. + + @return Matrix The corresponding state covariance matrix. + """ + return Matrix( + [[300*300, 0, 0, 0], + [0, 30*30, 0, 0], + [0, 0, 150*150, 0], + [0, 0, 0, 30*30]]) + +if __name__ == '__main__': + dt = 3. # The sampling period + gt_X_init = -500. # Ground truth initial position along the X-axis, in meters + gt_Y_init = 1000. # Ground truth initial position along the Y-axis, in meters + gt_vX_init = 100. # The velocity along the x-axis + gt_vY_init = 5. # The velocity along the y-axis + procVar = 0.1 # The variance of the process function + sigmaRange = 5 # Standard deviation of the range measurement: 5m + sigmaElevAngle = Math.rad(0.5) # Standard deviation of the elevation angle measurent: 0.5deg + stdevAircraftVelocity = 0.2; # Standard deviation of the velocity of the simulated aircraft, + # to make it deviate a bit from the constant velocity model + + # The object that draws the sigma points used by the UKF + drawer = UKSigmaDrawerMerwe(n=4, alpha=0.3, beta=2, kappa=-1, resFunc=residual_state_vectors, addFunc=add_state_vectors) + + # The object that performs radar measurements + radar = vpRadarStation(0., 0., sigmaRange, sigmaElevAngle) + + P0 = generate_P0_matrix() # The initial estimate of the state covariance matrix + R = Matrix([[sigmaRange * sigmaRange, 0], [0, sigmaElevAngle * sigmaElevAngle]]) # The measurement covariance matrix + Q = generate_Q_matrix(dt) * procVar # The process covariance matrix + ukf = UnscentedKalman(Q, R, drawer, fx, radar.state_to_measurement) # The Unscented Kalman Filter instance + + # Initializing the state vector and state covariance matrix estimates + ukf.init(ColVector([0.9 * gt_X_init, 0.9 * gt_vX_init, 0.9 * gt_Y_init, 0.9 * gt_vY_init]), P0) + ukf.setMeasurementMeanFunction(measurementMean) + ukf.setMeasurementResidualFunction(measurementResidual) + + # Initializing the Graphical User Interface if the needed libraries are available + if has_gui: + num_plots = 4 + plot = Plot(num_plots) + plot_titles = [ + 'Position along X-axis', 'Velocity along X-axis', + 'Position along Y-axis', 'Velocity along Y-axis' + ] + plot_y_units = [ + 'Position (m)', 'Velocity (m/s)', + 'Position (m)', 'Velocity (m/s)' + ] + plot_legends = ['GT', 'Filtered'] + + # Initializing the subplots + for plot_index in range(num_plots): + plot.initGraph(plot_index, len(plot_legends)) + plot.setTitle(plot_index, plot_titles[plot_index]) + plot.setUnitY(plot_index, plot_y_units[plot_index]) + plot.setUnitX(plot_index, 'Time (s)') + for legend_index in range(len(plot_legends)): + plot.setLegend(plot_index, legend_index, plot_legends[legend_index]) + + ac_pos = ColVector([gt_X_init, gt_Y_init]) # Ground truth position + ac_vel = ColVector([gt_vX_init, gt_vY_init]) # Ground truth velocity + ac = vpACSimulator(ac_pos, ac_vel, stdevAircraftVelocity) + gt_Xprec = ColVector([ac_pos[0], ac_pos[1]]) + for i in range(500): + # Creating noisy measurements + gt_X = ac.update(dt) + gt_V = (gt_X - gt_Xprec) / dt + z = radar.measureWithNoise(gt_X) + + # Filtering using the UKF + ukf.filter(z, dt) + + # Getting the filtered state vector + Xest = ukf.getXest() + + # Update the GUI if available + if has_gui: + plot.plot(0, 0, i, gt_X[0]) + plot.plot(0, 1, i, Xest[0]) + + plot.plot(1, 0, i, gt_V[0]) + plot.plot(1, 1, i, Xest[1]) + + plot.plot(2, 0, i, gt_X[1]) + plot.plot(2, 1, i, Xest[2]) + + plot.plot(3, 0, i, gt_V[1]) + plot.plot(3, 1, i, Xest[3]) + + # Updating last measurement for future computation of the noisy velocity + gt_Xprec = ColVector([gt_X[0], gt_X[1]]) + + print('Finished') + input('Press enter to quit') diff --git a/modules/python/generator/visp_python_bindgen/enum_binding.py b/modules/python/generator/visp_python_bindgen/enum_binding.py index 23f417ee1e..884c41a799 100644 --- a/modules/python/generator/visp_python_bindgen/enum_binding.py +++ b/modules/python/generator/visp_python_bindgen/enum_binding.py @@ -103,7 +103,7 @@ def get_cpp_identifier_scope(fully_qualified_name: str, root_scope: Union[Namesp return get_cpp_identifier_scope('::'.join(remainder), root_scope.namespaces[ns]) return root_scope -def resolve_enums_and_typedefs(root_scope: NamespaceScope, mapping: Dict) -> Tuple[List[EnumRepr], List[EnumRepr]]: +def resolve_enums_and_typedefs_to_enums(root_scope: NamespaceScope, mapping: Dict) -> Tuple[List[EnumRepr], List[EnumRepr]]: final_data: List[EnumRepr] = [] temp_data: List[EnumRepr] = [] # Data that is incomplete for preprocessing match_id = lambda repr, enum_id: repr.id is not None and enum_id is not None and repr.id == enum_id @@ -175,7 +175,7 @@ def accumulate_data(scope: Union[NamespaceScope, ClassScope]): def get_enum_bindings(root_scope: NamespaceScope, mapping: Dict, submodule: Submodule, header: 'HeaderFile') -> List[SingleObjectBindings]: - final_data, filtered_reprs = resolve_enums_and_typedefs(root_scope, mapping) + final_data, filtered_reprs = resolve_enums_and_typedefs_to_enums(root_scope, mapping) for repr in filtered_reprs: logging.info(f'Enum {repr} was ignored, because it is incomplete (missing values or name)') diff --git a/modules/python/generator/visp_python_bindgen/generator_config.py b/modules/python/generator/visp_python_bindgen/generator_config.py index 4735813a71..b82551b65e 100644 --- a/modules/python/generator/visp_python_bindgen/generator_config.py +++ b/modules/python/generator/visp_python_bindgen/generator_config.py @@ -97,6 +97,7 @@ def to_pcpp_args_list(self) -> List[str]: Specific argument regexs for which having default arguments is specifically forbidden ''' FORBIDDEN_DEFAULT_ARGUMENT_TYPES_REGEXS = [ + '^std::function', '^std::ostream', '^std::initializer_list', '^rs2::', diff --git a/modules/python/generator/visp_python_bindgen/header.py b/modules/python/generator/visp_python_bindgen/header.py index c32bc6db6a..b278c1035f 100644 --- a/modules/python/generator/visp_python_bindgen/header.py +++ b/modules/python/generator/visp_python_bindgen/header.py @@ -77,15 +77,24 @@ def __setstate__(self, d): def get_header_dependencies(self, headers: List['HeaderFile']) -> List['HeaderFile']: if len(self.depends) == 0: return [] + + user_required_headers = [] + all_user_required_header_names = self.submodule.config.get('header_additional_dependencies') + if all_user_required_header_names is not None: + user_required_headers = all_user_required_header_names.get(self.path.name, []) + header_deps = [] for header in headers: if header == self: continue is_dependency = False - for d in self.depends: - if d in header.contains: - is_dependency = True - break + if header.path.name in user_required_headers: + is_dependency = True + else: + for d in self.depends: + if d in header.contains: + is_dependency = True + break if is_dependency: header_deps.append(header) upper_dependencies = header.get_header_dependencies(headers) @@ -301,12 +310,14 @@ def add_method_doc_to_pyargs(method: types.Method, py_arg_strs: List[str]) -> Li # Reference public base classes when creating pybind class binding base_class_strs = list(map(lambda base_class: get_typename(base_class.typename, owner_specs, header_env.mapping), filter(lambda b: b.access == 'public', cls.class_decl.bases))) - class_template_str = ', '.join([name_cpp] + base_class_strs) + # py::class template contains the class, its holder type, and its base clases. + # The default holder type is std::unique_ptr. when the cpp function argument is a shared_ptr, Pybind will raise an error when calling the method. + py_class_template_str = ', '.join([name_cpp, f'std::shared_ptr<{name_cpp}>'] + base_class_strs) doc_param = [] if class_doc is None else [class_doc.documentation] buffer_protocol_arg = ['py::buffer_protocol()'] if cls_config['use_buffer_protocol'] else [] cls_argument_strs = ['submodule', f'"{name_python}"'] + doc_param + buffer_protocol_arg - class_decl = f'\tpy::class_ {python_ident} = py::class_<{class_template_str}>({", ".join(cls_argument_strs)});' + class_decl = f'\tpy::class_ {python_ident} = py::class_<{py_class_template_str}>({", ".join(cls_argument_strs)});' # Definitions # Skip constructors for classes that have pure virtual methods since they cannot be instantiated diff --git a/modules/python/generator/visp_python_bindgen/header_utils.py b/modules/python/generator/visp_python_bindgen/header_utils.py index d81c7d5051..d31750556a 100644 --- a/modules/python/generator/visp_python_bindgen/header_utils.py +++ b/modules/python/generator/visp_python_bindgen/header_utils.py @@ -93,8 +93,8 @@ class HeaderEnvironment(): def __init__(self, data: ParsedData): self.mapping: Dict[str, str] = self.build_naive_mapping(data.namespace, {}) # Step 2: resolve enumeration names that are possibly hidden behind typedefs - from visp_python_bindgen.enum_binding import resolve_enums_and_typedefs - enum_reprs, _ = resolve_enums_and_typedefs(data.namespace, self.mapping) + from visp_python_bindgen.enum_binding import resolve_enums_and_typedefs_to_enums + enum_reprs, _ = resolve_enums_and_typedefs_to_enums(data.namespace, self.mapping) for enum_repr in enum_reprs: for value in enum_repr.values: self.mapping[value.name] = enum_repr.name + '::' + value.name @@ -110,8 +110,10 @@ def build_naive_mapping(self, data: Union[NamespaceScope, ClassScope], mapping, current_mapping[alias.alias] = get_type(alias.type, {}, current_mapping) for typedef in data.typedefs: - current_mapping[typedef.name] = scope + typedef.name - + if not name_is_anonymous(typedef.type.typename): + current_mapping[typedef.name] = get_type(typedef.type, {}, current_mapping) + else: + current_mapping[typedef.name] = scope + typedef.name for enum in data.enums: if not name_is_anonymous(enum.typename): enum_name = '::'.join([seg.name for seg in enum.typename.segments]) diff --git a/modules/python/generator/visp_python_bindgen/submodule.py b/modules/python/generator/visp_python_bindgen/submodule.py index 7b9cefc0ce..ee7b61bf8d 100644 --- a/modules/python/generator/visp_python_bindgen/submodule.py +++ b/modules/python/generator/visp_python_bindgen/submodule.py @@ -158,6 +158,7 @@ def generate(self) -> None: //#define PYBIND11_DETAILED_ERROR_MESSAGES #include #include +#include #include #include diff --git a/tutorial/CMakeLists.txt b/tutorial/CMakeLists.txt index 6fba397487..468f5ffd7a 100644 --- a/tutorial/CMakeLists.txt +++ b/tutorial/CMakeLists.txt @@ -44,6 +44,7 @@ visp_add_subdirectory(imgproc/contrast-sharpening REQUIRED_DEPS visp_co visp_add_subdirectory(imgproc/count-coins REQUIRED_DEPS visp_core visp_io visp_gui visp_imgproc) visp_add_subdirectory(imgproc/flood-fill REQUIRED_DEPS visp_core visp_io visp_gui visp_imgproc) visp_add_subdirectory(imgproc/hough-transform REQUIRED_DEPS visp_core visp_gui visp_imgproc) +visp_add_subdirectory(kalman REQUIRED_DEPS visp_core visp_gui) 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) diff --git a/tutorial/kalman/CMakeLists.txt b/tutorial/kalman/CMakeLists.txt new file mode 100644 index 0000000000..67158faec3 --- /dev/null +++ b/tutorial/kalman/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-kalman) + +find_package(VISP REQUIRED visp_core visp_gui) + +set(tutorial_cpp) + +list(APPEND tutorial_cpp tutorial-ukf.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/kalman/tutorial-ukf.cpp b/tutorial/kalman/tutorial-ukf.cpp new file mode 100644 index 0000000000..9ff0282e30 --- /dev/null +++ b/tutorial/kalman/tutorial-ukf.cpp @@ -0,0 +1,518 @@ +/**************************************************************************** + * + * 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-ukf.cpp + * Tutorial on how to use the Unscented Kalman Filter (UKF) 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 UKF 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. +*/ + +// ViSP includes +#include +#include +#include +#include +#include +#include +//! [Display_includes] +#ifdef VISP_HAVE_DISPLAY +#include +#include +#include +#include +#include +#include +#endif +//! [Display_includes] +#include + +//! [UKF_includes] +#include +#include +//! [UKF_includes] + +#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] + +//! [Object_simulator] +/** + * \brief Class that simulates the moving 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. + */ + 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) + { } + + /** + * \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; + } + +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] + +//! [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 + */ + vpMarkersMeasurements(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMw, const vpRotationMatrix &wRo, + const std::vector &markers, 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) + { } + + //! [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 = m_markers.size(); + vpColVector meas(2*nbMarkers); + vpHomogeneousMatrix wMo; + vpTranslationVector wTo(x[0], x[1], x[2]); + wMo.buildFrom(wTo, m_wRo); + for (unsigned int i = 0; i < nbMarkers; ++i) { + vpColVector cX = m_cMw * wMo * m_markers[i]; + double u = 0., v = 0.; + vpMeterPixelConversion::convertPoint(m_cam, cX[0] / cX[2], cX[1] / cX[2], u, v); + meas[2*i] = u; + meas[2*i + 1] = v; + } + return meas; + } + //! [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 = m_markers.size(); + vpColVector meas(2*nbMarkers); + vpHomogeneousMatrix wMo; + vpTranslationVector wTo(wX[0], wX[1], wX[2]); + wMo.buildFrom(wTo, m_wRo); + for (unsigned int i = 0; i < nbMarkers; ++i) { + vpColVector cX = m_cMw * wMo * m_markers[i]; + double u = 0., v = 0.; + vpMeterPixelConversion::convertPoint(m_cam, cX[0] / cX[2], cX[1] / cX[2], u, v); + meas[2*i] = u; + meas[2*i + 1] = v; + } + return meas; + } + //! [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] + +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. + vpGaussRand m_rng; // Noise simulator for the measurements +}; +//! [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. + */ +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] + +int main(/*const int argc, const char *argv[]*/) +{ + //! [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 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 + const unsigned int nbMarkers = 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] + + // Initialize the attributes of the UKF + //! [Sigma_points_drawer] + std::shared_ptr drawer = std::make_shared(4, 0.001, 2., -1); + //! [Sigma_points_drawer] + + //! [Covariance_measurements] + vpMatrix R1landmark(2, 2, 0.); // The covariance of the noise introduced by the measurement with 1 landmark + R1landmark[0][0] = sigmaMeasurements*sigmaMeasurements; + R1landmark[1][1] = sigmaMeasurements*sigmaMeasurements; + vpMatrix R(2*nbMarkers, 2 * nbMarkers); + for (unsigned int i = 0; i < nbMarkers; ++i) { + R.insert(R1landmark, 2*i, 2*i); + } + //! [Covariance_measurements] + + //! [Covariance_process] + const double processVariance = 0.000025; // Variance of the process of (0.005cm)^2 + vpMatrix Q; // The covariance of the process + Q.eye(4); + Q = Q * processVariance; + //! [Covariance_process] + + //! [Initial_estimates] + vpMatrix P0(4, 4); // The initial guess of the process covariance + P0.eye(4); + P0[0][0] = 1.; + P0[1][1] = 1.; + P0[2][2] = 1.; + P0[2][2] = 5.; + + vpColVector X0(4); // The initial guess for the state + X0[0] = 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.75 * w * dt; // Wrong estimation of the pulsation: error of 25% + //! [Initial_estimates] + + //! [Init_functions] + vpUnscentedKalman::vpProcessFunction f = fx; + vpMarkersMeasurements markerMeas(cam, cMw, wRo, markers, sigmaMeasurements, seed); + using std::placeholders::_1; + vpUnscentedKalman::vpMeasurementFunction h = std::bind(&vpMarkersMeasurements::state_to_measurement, &markerMeas, _1); + //! [Init_functions] + + //! [Init_UKF] + // 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, 3); + 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, "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 + 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"); + } +#endif + //! [Init_renderer] + + //! [Init_simu] + // Initialize the simulation + vpObjectSimulator object(radius, w, phi, wZ); + vpColVector object_pos(4, 0.); + object_pos[3] = 1.; + //! [Init_simu] + + //! [Simu_loop] + for (unsigned int i = 0; i < 200; ++i) { + //! [Update obj pose] + // Update object pose + object_pos = object.move(dt * static_cast(i)); + //! [Update obj pose] + + //! [Update_measurement] + // Perform the measurement + vpColVector z = markerMeas.measureWithNoise(object_pos); + //! [Update_measurement] + + //! [Perform_filtering] + // Use the UKF to filter the measurement + ukf.filter(z, dt); + //! [Perform_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 filtered state + vpColVector Xest = ukf.getXest(); + plot.plot(0, 1, Xest[0], Xest[1]); + + // Plot the noisy pose + 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] +#endif + //! [Update_displays] + } + //! [Simu_loop] + 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 +int main() +{ + std::cout << "vpUnscentedKalman is only available if you compile ViSP in C++11 standard or higher." << std::endl; + return 0; +} +#endif diff --git a/tutorial/mean-drift/CMakeLists.txt b/tutorial/mean-drift/CMakeLists.txt index fed2f435fd..61aaff4046 100644 --- a/tutorial/mean-drift/CMakeLists.txt +++ b/tutorial/mean-drift/CMakeLists.txt @@ -1,3 +1,38 @@ +############################################################################# +# +# 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-meandrift)