Improve doc

doc/tutorial/misc/tutorial-ukf.dox

@@ -18,7 +18,7 @@ The equations that describe the motion of the object in the world frame are the
   {}^W \textbf{X}_x &=& R cos(\omega t + \phi) \\
   {}^W \textbf{X}_y &=& R sin(\omega t + \phi) \\
   {}^W \textbf{X}_z &=& constant
-\end{array}
+  \end{array}
 \f]
 
 where \f$ \omega \f$ and \f$ \phi \f$ are respectively the pulsation and the phase of the motion, while \f$ R \f$ is the
@@ -44,7 +44,7 @@ The first step of the UKF is the prediction step. During this step, some particu
   \begin{array}{lcl}
   \chi &=& sigma-function(\textbf{x}, \textbf{P}) \\
   \textbf{w}^m, \textbf{w}^c &=& weight-function(n, parameters)
-\end{array}
+  \end{array}
 \f]
 
 There are different ways of drawing the sigma points and associated weights in the litterature, such as the one
@@ -64,7 +64,7 @@ and covariance \f$ \overline{\textbf{P}} \f$ of the prior:
     \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}
- \end{array}
+  \end{array}
 \f]
 
 where \f$ \textbf{Q} \f$ is the covariance of the error introduced by the process function.
@@ -82,7 +82,7 @@ covariance \f$ \textbf{P}_z \in {R}^{m\text{ x }m} \f$ of these points:
     \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}
-\end{array}
+  \end{array}
 \f]
 
 where \f$ \textbf{R} \f$ is the measurement covariance matrix.
@@ -103,9 +103,9 @@ Finally, we can compute the new state estimate \f$ \textbf{x} \f$ and the new co
 
 \f[
   \begin{array}{lcl}
- \textbf{x} &=& \boldsymbol{\mu} + \textbf{K} \textbf{y} \\
- \textbf{P} &=& \overline{\textbf{P}} - \textbf{K} \textbf{P}_z \textbf{K}^T
-\end{array}
+  \textbf{x} &=& \boldsymbol{\mu} + \textbf{K} \textbf{y} \\
+  \textbf{P} &=& \overline{\textbf{P}} - \textbf{K} \textbf{P}_z \textbf{K}^T
+  \end{array}
 \f]
 
 \section tuto-ukf-tutorial Explanations about the tutorial
@@ -138,7 +138,7 @@ the 3D position of the object expressed in the world frame, along with the pulsa
   \textbf{x}[1] &=& {}^WX_y \\
   \textbf{x}[2] &=& {}^WX_z \\
   \textbf{x}[3] &=& \omega \Delta t
-\end{array}
+  \end{array}
 \f]
 
 The measurement \f$ \textbf{z} \f$ corresponds to the perspective projection of the different markers in the image.
@@ -149,7 +149,7 @@ The measurement vector can be written as:
   \begin{array}{lcl}
   \textbf{z}[2i] &=& u_i \\
   \textbf{z}[2i+1] &=& v_i
-\end{array}
+  \end{array}
 \f]
 
 Be \f$ \textbf{K}_{intr} \f$ the camera instrinsic parameters matrix defined by:
@@ -213,7 +213,7 @@ As stated in the \ref tuto-ukf-intro, the equations of motion of the object are
   {}^W X_x(t) &=& R cos(\omega t + \phi) \\
   {}^W X_y(t) &=& R sin(\omega t + \phi) \\
   {}^W X_z(t) &=& constant
-\end{array}
+  \end{array}
 \f]
 
 Thus, we have:
@@ -223,7 +223,7 @@ Thus, we have:
   {}^WX_x( t + \Delta t) &=& R cos(\omega (t + \Delta t) + \phi) &=& R cos((\omega t + \phi) + \omega \Delta t )\\
   {}^WX_y( t + \Delta t) &=& R sin(\omega (t + \Delta t) + \phi) &=& R sin((\omega t + \phi) + \omega \Delta t )\\
   {}^WX_z( t + \Delta t) &=& constant
-\end{array}
+  \end{array}
 \f]
 
 Which can be rewritten:
@@ -232,7 +232,7 @@ Which can be rewritten:
   {}^WX_x( t + \Delta t) &=& R cos((\omega t + \phi) + \omega \Delta t ) &=& R cos(\omega t + \phi) cos (\omega \Delta t ) - R sin(\omega t + \phi) sin(\omega \Delta t) \\
   {}^WX_y( t + \Delta t) &=& R sin((\omega t + \phi) + \omega \Delta t ) &=& R cos(\omega t + \phi) sin (\omega \Delta t ) + R sin(\omega t + \phi) cos(\omega \Delta t)\\
   {}^WX_z( t + \Delta t) &=& constant
-\end{array}
+  \end{array}
 \f]
 
 And can be finally written as:
@@ -241,7 +241,7 @@ And can be finally written as:
   {}^WX_x( t + \Delta t) &=& R cos(\omega t + \phi) cos (\omega \Delta t ) - R sin(\omega t + \phi) sin(\omega \Delta t) &=& {}^W X_x( t) cos(\omega \Delta t) - {}^W X_y(t) sin(\omega \Delta t) \\
   {}^WX_y( t + \Delta t) &=& R cos(\omega t + \phi) sin (\omega \Delta t ) + R sin(\omega t + \phi) cos(\omega \Delta t) &=& {}^W X_x( t) sin(\omega \Delta t) + {}^W X_y(t) cos(\omega \Delta t) \\
   {}^WX_z( t + \Delta t) &=& constant
-\end{array}
+  \end{array}
 \f]
 
 This motivates us to choose the following non-linear process function:
@@ -252,7 +252,7 @@ This motivates us to choose the following non-linear process function:
   \textbf{x}[1]_{t + \Delta t} &=& {}^WX_y (t + \Delta t) &=& \textbf{x}[0]_{t} sin(\textbf{x}[3]_{t}) + \textbf{x}[1]_{t} cos(\textbf{x}[3]_{t}) \\
   \textbf{x}[2]_{t + \Delta t} &=& {}^WX_z (t + \Delta t) &=& \textbf{x}[2]_{t} \\
   \textbf{x}[3]_{t + \Delta t} &=& \omega \Delta t &=& \textbf{x}[3]_{t}
-\end{array}
+  \end{array}
 \f]
 
 As the process function is pretty simple, a simple function called here `fx()` is enough:

example/kalman/ukf-linear-example.cpp

@@ -1,5 +1,4 @@
-/****************************************************************************
- *
+/*
  * ViSP, open source Visual Servoing Platform software.
  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
  *
@@ -27,34 +26,32 @@
  *
  * 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.
+ * in this case is not necessary, it is done for learning purpose only.
  *
  * The system we are interested in is a system moving on a 2D-plane.
  *
  * The state vector of the UKF is:
- *  \f[
-      \begin{array}{lcl}
-        \textbf{x}[0] &=& x \\
-        \textbf{x}[1] &=& \dot{x} \\
-        \textbf{x}[1] &=& y \\
-        \textbf{x}[2] &=& \dot{y}
-     \end{array}
-    \f]
-
+ * \f[
+ * \begin{array}{lcl}
+ *   \textbf{x}[0] &=& x \\
+ *   \textbf{x}[1] &=& \dot{x} \\
+ *   \textbf{x}[1] &=& y \\
+ *   \textbf{x}[2] &=& \dot{y}
+ * \end{array}
+ * \f]
+ *
  * The measurement \f$ \textbf{z} \f$ corresponds to the position along the x-axis
  * and y-axis. The measurement vector can be written as:
  * \f[
-      \begin{array}{lcl}
-        \textbf{z}[0] &=& x \\
-        \textbf{z}[1] &=& y
-      \end{array}
-    \f]
-
+ * \begin{array}{lcl}
+ *   \textbf{z}[0] &=& x \\
+ *   \textbf{z}[1] &=& y
+ * \end{array}
+ * \f]
  * Some noise is added to the measurement vector to simulate a sensor which is
  * not perfect.
 */

example/kalman/ukf-nonlinear-complex-example.cpp

@@ -1,5 +1,4 @@
-/****************************************************************************
- *
+/*
  * ViSP, open source Visual Servoing Platform software.
  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
  *
@@ -27,52 +26,51 @@
  *
  * 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[
-      \begin{array}{lcl}
-        \textbf{x}[0] &=& x \\
-        \textbf{x}[1] &=& y \\
-        \textbf{x}[2] &=& \theta
-     \end{array}
-    \f]
-   where \f$ \theta \f$ is the heading of the robot.
-
-   The measurement \f$ \textbf{z} \f$ corresponds to the distance and relative orientation of the
-   robot with different landmarks. Be \f$ p_x^i \f$ and \f$ p_y^i \f$ the position of the \f$ i^{th} \f$ landmark
-   along the x and y axis, the measurement vector can be written as:
-   \f[
-     \begin{array}{lcl}
-        \textbf{z}[2i] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\
-        \textbf{z}[2i+1] &=& \tan^{-1}{\frac{p_y^i - y}{p_x^i - x}} - \theta
-     \end{array}
-   \f]
-
+ * \f[
+ * \begin{array}{lcl}
+ *   \textbf{x}[0] &=& x \\
+ *   \textbf{x}[1] &=& y \\
+ *   \textbf{x}[2] &=& \theta
+ * \end{array}
+ * \f]
+ * where \f$ \theta \f$ is the heading of the robot.
+ *
+ * The measurement \f$ \textbf{z} \f$ corresponds to the distance and relative orientation of the
+ * robot with different landmarks. Be \f$ p_x^i \f$ and \f$ p_y^i \f$ the position of the \f$ i^{th} \f$ landmark
+ * along the x and y axis, the measurement vector can be written as:
+ * \f[
+ *   \begin{array}{lcl}
+ *      \textbf{z}[2i] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\
+ *      \textbf{z}[2i+1] &=& \tan^{-1}{\frac{p_y^i - y}{p_x^i - x}} - \theta
+ *    \end{array}
+ * \f]
+ *
  * Some noise is added to the measurement vector to simulate measurements which are
  * not perfect.
-
-   The mean of several angles must be computed in the 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$ .
+ *
+ * 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.
+ *
+ * Additionally, the addition and subtraction of angles must be carefully done, as the result
+ * must stay in the interval \f$[- \pi ; \pi ]\f$ or \f$[0 ; 2 \pi ]\f$ . We decided to use
+ * the interval \f$[- \pi ; \pi ]\f$ .
 */
 
 // UKF includes

example/kalman/ukf-nonlinear-example.cpp

@@ -1,5 +1,4 @@
-/****************************************************************************
- *
+/*
  * ViSP, open source Visual Servoing Platform software.
  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
  *
@@ -27,8 +26,7 @@
  *
  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
- *
-*****************************************************************************/
+ */
 
 /** \example ukf-nonlinear-example.cpp
  * Example of a simple non-linear use-case of the Unscented Kalman Filter (UKF).

modules/core/include/visp3/core/vpUKSigmaDrawerAbstract.h

@@ -31,8 +31,8 @@
  * Display a point cloud using PCL library.
  */
 
-#ifndef _vpUKSigmaDrawerAbstract_h_
-#define _vpUKSigmaDrawerAbstract_h_
+#ifndef VP_UK_SIGMA_DRAWER_ABSTRACT_H
+#define VP_UK_SIGMA_DRAWER_ABSTRACT_H
 
 #include <vector>
 

modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h

@@ -31,8 +31,8 @@
  * Display a point cloud using PCL library.
  */
 
-#ifndef _vpUKSigmaDrawerMerwe_h_
-#define _vpUKSigmaDrawerMerwe_h_
+#ifndef VP_UK_SIGMA_DRAWER_MERWE_H
+#define VP_UK_SIGMA_DRAWER_MERWE_H
 
 #include <vector>
 

modules/core/include/visp3/core/vpUnscentedKalman.h

@@ -31,8 +31,8 @@
  * Display a point cloud using PCL library.
  */
 
-#ifndef _vpUnscentedKalman_h_
-#define _vpUnscentedKalman_h_
+#ifndef VP_UNSCENTED_KALMAN_H
+#define VP_UNSCENTED_KALMAN_H
 
 #include <visp3/core/vpConfig.h>
 

tutorial/kalman/tutorial-ukf.cpp

@@ -1,35 +1,3 @@
-/****************************************************************************
- *
- * 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 [email protected]
- *
- * 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.
@@ -38,25 +6,25 @@
  * fixed to the ceiling.
  *
  * The state vector of the UKF is:
- *  \f[
-      \begin{array}{lcl}
-        \textbf{x}[0] &=& {}^WX_x \\
-        \textbf{x}[1] &=& {}^WX_y \\
-        \textbf{x}[2] &=& {}^WX_z \\
-        \textbf{x}[3] &=& \omega \Delta t
-   \end{array}
-  \f]
-
-   The measurement \f$ \textbf{z} \f$ corresponds to the coordinates in pixels of the different markers.
-   Be \f$ u_i \f$ and \f$ v_i \f$ the horizontal and vertical pixel coordinates of the \f$ i^{th} \f$ marker.
-   The measurement vector can be written as:
-   \f[
-     \begin{array}{lcl}
-        \textbf{z}[2i] &=& u_i \\
-        \textbf{z}[2i+1] &=& v_i
-     \end{array}
-   \f]
-
+ * \f[
+ * \begin{array}{lcl}
+ *   \textbf{x}[0] &=& {}^WX_x \\
+ *   \textbf{x}[1] &=& {}^WX_y \\
+ *   \textbf{x}[2] &=& {}^WX_z \\
+ *   \textbf{x}[3] &=& \omega \Delta t
+ * \end{array}
+ * \f]
+ *
+ * The measurement \f$ \textbf{z} \f$ corresponds to the coordinates in pixels of the different markers.
+ * Be \f$ u_i \f$ and \f$ v_i \f$ the horizontal and vertical pixel coordinates of the \f$ i^{th} \f$ marker.
+ * The measurement vector can be written as:
+ * \f[
+ *   \begin{array}{lcl}
+ *       \textbf{z}[2i] &=& u_i \\
+ *       \textbf{z}[2i+1] &=& v_i
+ *   \end{array}
+ * \f]
+ *
  * Some noise is added to the measurement vector to simulate measurements which are
  * not perfect.
 */