[EXAMPLE] Move the content of tutorial/kalman into example/kalman [TU…

…TO] Created a new tutorial for the UKF

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)

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 example-ukf-linear-example.cpp)
+list(APPEND example_cpp example-ukf-nonlinear-example.cpp)
+list(APPEND example_cpp example-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()

tutorial/kalman/tutorial-ukf-linear-example.cpp → example/kalman/example-ukf-linear-example.cpp

@@ -113,7 +113,7 @@ int main(/*const int argc, const char *argv[]*/)
   const double sigmaYmeas = 0.3; // Standard deviation of the measure along the y-axis
 
   // Initialize the attributes of the UKF
-  vpUKSigmaDrawerMerwe drawer(4, 0.3, 2., -1.);
+  std::shared_ptr<vpUKSigmaDrawerAbstract> drawer = std::make_shared<vpUKSigmaDrawerMerwe>(4, 0.3, 2., -1.);
   vpMatrix P0(4, 4); //  The initial guess of the process covariance
   P0.eye(4, 4);
   P0 = P0 * 10.;
@@ -133,7 +133,7 @@ int main(/*const int argc, const char *argv[]*/)
   vpUnscentedKalman::vpMeasurementFunction h = hx;
 
   // Initialize the UKF
-  vpUnscentedKalman ukf(Q, R, &drawer, f, h);
+  vpUnscentedKalman ukf(Q, R, drawer, f, h);
   ukf.init(vpColVector(4, 0.), P0);
 
 #ifdef VISP_HAVE_DISPLAY

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

@@ -540,7 +540,7 @@ int main(/*const int argc, const char *argv[]*/)
   const unsigned int nbCmds = cmds.size();
 
   // Initialize the attributes of the UKF
-  vpUKSigmaDrawerMerwe drawer(3, 0.00001, 2., 0, stateResidual, stateAdd);
+  std::shared_ptr<vpUKSigmaDrawerAbstract> drawer = std::make_shared<vpUKSigmaDrawerMerwe>(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;
@@ -576,7 +576,7 @@ int main(/*const int argc, const char *argv[]*/)
   vpUnscentedKalman::vpCommandStateFunction bx = std::bind(&vpBicycleModel::computeMotion, &robot, _1, _2, _3);
 
   // Initialize the UKF
-  vpUnscentedKalman ukf(Q, R, &drawer, f, h);
+  vpUnscentedKalman ukf(Q, R, drawer, f, h);
   ukf.init(X0, P0);
   ukf.setCommandStateFunction(bx);
   ukf.setMeasurementMeanFunction(measurementMean);

tutorial/kalman/tutorial-ukf-nonlinear-example.cpp → example/kalman/example-ukf-nonlinear-example.cpp

@@ -219,7 +219,7 @@ int main(/*const int argc, const char *argv[]*/)
   const double sigmaElevAngle = vpMath::rad(0.5); // Standard deviation of the elevation angle measurent: 0.5deg
 
   // Initialize the attributes of the UKF
-  vpUKSigmaDrawerMerwe drawer(4, 0.1, 2., -1.);
+  std::shared_ptr<vpUKSigmaDrawerAbstract> drawer = std::make_shared<vpUKSigmaDrawerMerwe>(4, 0.1, 2., -1.);
 
   vpMatrix R(2, 2, 0.); // The covariance of the noise introduced by the measurement
   R[0][0] = sigmaRange*sigmaRange;
@@ -255,7 +255,7 @@ int main(/*const int argc, const char *argv[]*/)
   vpUnscentedKalman::vpMeasurementFunction h = std::bind(&vpRadarStation::state_to_measurement, &radar, _1);
 
   // Initialize the UKF
-  vpUnscentedKalman ukf(Q, R, &drawer, f, h);
+  vpUnscentedKalman ukf(Q, R, drawer, f, h);
   ukf.init(X0, P0);
 
 #ifdef VISP_HAVE_DISPLAY

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

@@ -193,7 +193,7 @@ class VISP_EXPORT vpUnscentedKalman
    * 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, vpUKSigmaDrawerAbstract *drawer, const vpProcessFunction &f, const vpMeasurementFunction &h);
+  vpUnscentedKalman(const vpMatrix &Q, const vpMatrix &R, std::shared_ptr<vpUKSigmaDrawerAbstract> &drawer, const vpProcessFunction &f, const vpMeasurementFunction &h);
 
   /**
    * \brief Set the guess of the initial state and covariance.
@@ -392,7 +392,7 @@ class VISP_EXPORT vpUnscentedKalman
   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.*/
-  vpUKSigmaDrawerAbstract *m_sigmaDrawer; /*!< Object that permits to draw the sigma points.*/
+  std::shared_ptr<vpUKSigmaDrawerAbstract> 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.*/

modules/core/src/math/kalman/vpUnscentedKalman.cpp

@@ -39,7 +39,7 @@
 #include <visp3/core/vpUnscentedKalman.h>
 
 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
-vpUnscentedKalman::vpUnscentedKalman(const vpMatrix &Q, const vpMatrix &R, vpUKSigmaDrawerAbstract *drawer, const vpProcessFunction &f, const vpMeasurementFunction &h)
+vpUnscentedKalman::vpUnscentedKalman(const vpMatrix &Q, const vpMatrix &R, std::shared_ptr<vpUKSigmaDrawerAbstract> &drawer, const vpProcessFunction &f, const vpMeasurementFunction &h)
   : m_Q(Q)
   , m_R(R)
   , m_f(f)

tutorial/kalman/CMakeLists.txt

@@ -6,9 +6,7 @@ find_package(VISP REQUIRED visp_core visp_gui)
 
 set(tutorial_cpp)
 
-list(APPEND tutorial_cpp tutorial-ukf-linear-example.cpp)
-list(APPEND tutorial_cpp tutorial-ukf-nonlinear-example.cpp)
-list(APPEND tutorial_cpp tutorial-ukf-nonlinear-complex-example.cpp)
+list(APPEND tutorial_cpp tutorial-ukf.cpp)
 
 foreach(cpp ${tutorial_cpp})
   visp_add_target(${cpp})