Unscented Kalman filter (UKF)

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/software/visp_build/modules/python/bindings/src/robot.cpp:3:
   /usr/include/c++/11/type_traits: **In instantiation of ‘struct std::is_move_constructible<vpImage<double> >’:**
   /usr/include/c++/11/type_traits:152:12:   required from ‘struct std::__and_<std::is_move_constructible<vpImage<double> >, std::is_move_assignable<vpImage<double> > >’
   /usr/include/c++/11/type_traits:157:12:   required from ‘struct std::__and_<std::__not_<std::__is_tuple_like<vpImage<double> > >, std::is_move_constructible<vpImage<double> >, std::is_move_assignable<vpImage<double> > >’
   /usr/include/c++/11/type_traits:2209:11:   required by substitution of ‘template<class ... _Cond> using _Require = std::__enable_if_t<std::__and_< <template-parameter-1-1> >::value> [with _Cond = {std::__not_<std::__is_tuple_like<vpImage<double> > >, std::is_move_constructible<vpImage<double> >, std::is_move_assignable<vpImage<double> >}]’
   /usr/include/c++/11/bits/move.h:196:5:   required by substitution of ‘template<class _Tp> std::_Require<std::__not_<std::__is_tuple_like<_Tp> >, std::is_move_constructible<_Tp>, std::is_move_assignable<_Tp> > std::swap(_Tp&, _Tp&) [with _Tp = vpImage<double>]’
-  /home/sfelton/software/visp-sfelton/modules/core/include/visp3/core/vpImage.h:341:15:   required from ‘class vpImage<double>’
-  /home/sfelton/software/visp-sfelton/modules/core/include/visp3/core/vpImage.h:369:17:   required from here
+  /home/user/software/visp-user/modules/core/include/visp3/core/vpImage.h:341:15:   required from ‘class vpImage<double>’
+  /home/user/software/visp-user/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_ukf/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, <brace-enclosed initializer list>)’ is ambiguous
+ 1432 |         : arg(base), value(reinterpret_steal<object>(detail::make_caster<T>::cast(
+      |                                                      ~~~~~~~~~~~~~~~~~~~~~~~~~~~~^
+ 1433 |                          std::forward<T>(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

modules/core/include/visp3/core/vpMatrix.h

@@ -347,8 +347,15 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
 #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
 
-  // Compute the exponential matrix of a square matrix
+    // Compute the exponential matrix of a square matrix
   vpMatrix expm() const;
 
   // operation A = A + 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 <vector>
+
+#include <visp3/core/vpConfig.h>
+
+#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
+#include <visp3/core/vpColVector.h>
+
+/*!
+  \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<double> m_wm; /*!< The weights for the computation of the mean.*/
+    std::vector<double> 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<vpColVector> The sigma points.
+   */
+  virtual std::vector<vpColVector> 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

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

@@ -0,0 +1,141 @@
+/*
+ * 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 <vector>
+
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/core/vpUKSigmaDrawerAbstract.h>
+#include <visp3/core/vpUnscentedKalman.h>
+
+#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.
+
+  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 \mathcal{R}^n \f$ the mean and \f$ \boldsymbol{\Sigma} \in \mathcal{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 \mathcal{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.
+   */
+  vpUKSigmaDrawerMerwe(const int &n, const double &alpha, const double &beta, const double &kappa,
+                       const vpAddSubFunction &resFunc = vpUnscentedKalman::simpleResidual,
+                       const vpAddSubFunction &addFunc = vpUnscentedKalman::simpleAdd);
+
+  /**
+   * \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<vpColVector> The sigma points.
+   */
+  virtual std::vector<vpColVector> 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;
+  double m_beta;
+  double m_kappa;
+  double m_lambda;
+  vpAddSubFunction m_resFunc;
+  vpAddSubFunction m_addFunc;
+};
+#endif
+#endif