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..0abf16cedf --- /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 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() diff --git a/tutorial/kalman/tutorial-ukf-linear-example.cpp b/example/kalman/example-ukf-linear-example.cpp similarity index 97% rename from tutorial/kalman/tutorial-ukf-linear-example.cpp rename to example/kalman/example-ukf-linear-example.cpp index 240d5f06f8..fdece611fd 100644 --- a/tutorial/kalman/tutorial-ukf-linear-example.cpp +++ b/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 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 * 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 diff --git a/tutorial/kalman/tutorial-ukf-nonlinear-complex-example.cpp b/example/kalman/example-ukf-nonlinear-complex-example.cpp similarity index 99% rename from tutorial/kalman/tutorial-ukf-nonlinear-complex-example.cpp rename to example/kalman/example-ukf-nonlinear-complex-example.cpp index afebbdf18b..76c18eecd8 100644 --- a/tutorial/kalman/tutorial-ukf-nonlinear-complex-example.cpp +++ b/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 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; @@ -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); diff --git a/tutorial/kalman/tutorial-ukf-nonlinear-example.cpp b/example/kalman/example-ukf-nonlinear-example.cpp similarity index 98% rename from tutorial/kalman/tutorial-ukf-nonlinear-example.cpp rename to example/kalman/example-ukf-nonlinear-example.cpp index f884b4dcaf..aa28f0fdd0 100644 --- a/tutorial/kalman/tutorial-ukf-nonlinear-example.cpp +++ b/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 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; @@ -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 diff --git a/modules/core/include/visp3/core/vpUnscentedKalman.h b/modules/core/include/visp3/core/vpUnscentedKalman.h index 755b57fa33..99b937117b 100644 --- a/modules/core/include/visp3/core/vpUnscentedKalman.h +++ b/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 &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 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.*/ diff --git a/modules/core/src/math/kalman/vpUnscentedKalman.cpp b/modules/core/src/math/kalman/vpUnscentedKalman.cpp index 0f836f54d9..c873e3b2b7 100644 --- a/modules/core/src/math/kalman/vpUnscentedKalman.cpp +++ b/modules/core/src/math/kalman/vpUnscentedKalman.cpp @@ -39,7 +39,7 @@ #include #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 &drawer, const vpProcessFunction &f, const vpMeasurementFunction &h) : m_Q(Q) , m_R(R) , m_f(f) diff --git a/tutorial/kalman/CMakeLists.txt b/tutorial/kalman/CMakeLists.txt index aec59df9ac..c01707ae6f 100644 --- a/tutorial/kalman/CMakeLists.txt +++ b/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}) diff --git a/tutorial/kalman/tutorial-ukf.cpp b/tutorial/kalman/tutorial-ukf.cpp new file mode 100644 index 0000000000..f586a8943a --- /dev/null +++ b/tutorial/kalman/tutorial-ukf.cpp @@ -0,0 +1,445 @@ +/**************************************************************************** + * + * 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. + * +*****************************************************************************/ + +/** \example tutorial-ukf.cpp + * Tutorial on how to use the Unscented Kalman Filter (UKF) on a complex non-linear use-case. + * The system is a plate, whose coordinate frame origin is the point O, on which are sticked four markers. + * The plate revolves 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. + * + * 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. +*/ + +// UKF includes +#include +#include + +// ViSP includes +#include +#include +#include +#include +#include +#include +#ifdef VISP_HAVE_DISPLAY +#include +#include +#include +#include +#include +#include +#endif +#include + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +/** + * \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[2] * std::sin(x[3]); // wX + x_kPlus1[1] = x[1]; // wY + x_kPlus1[2] = x[0] * std::sin(x[3]) + x[2] * std::cos(x[3]); // wZ + x_kPlus1[3] = x[3]; // omega * dt + return x_kPlus1; +} + +/** + * \brief Class that simulates the moving plate. + */ +class vpPlateSimulator +{ +public: + /** + * \brief Construct a new vpPlateSimulator 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] wY The y-coordinate of the object in the world frame. + */ + vpPlateSimulator(const double &R, const double &w, const double &phi, const double &wY) + : m_R(R) + , m_w(w) + , m_phi(phi) + , m_wY(wY) + { } + + /** + * \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_wY; + wX[2] = m_R * std::sin(m_w * t + m_phi); + 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_wY; // The y-coordinate of the object in the world frame. +}; + +/** + * \brief Class that permits to convert the 3D position of the plate 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) + { } + + /** + * \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; + } + + /** + * \brief Perfect measurement of the projection of the markers in the image when the plate + * 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; + } + + /** + * \brief Noisy measurement of the projection of the markers in the image when the plate + * 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; + } + +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 +}; + +/** + * \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; +} + +int main(/*const int argc, const char *argv[]*/) +{ + const double dt = 0.001; // Period of 0.1s + const double sigmaMeasurements = 2.; // Standard deviation of the measurements: 2 pixels + const double radius = 0.5; // Radius of revolution of 0.5m + 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., -0.05, 1.}) + , vpColVector({0.05, 0., -0.05, 1.}) + , vpColVector({0.05, 0., 0.05, 1.}) + , vpColVector({-0.05, 0., 0.05, 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] = 0.; cMw[1][2] = -1.; cMw[1][3] = 0.3; + cMw[2][0] = 0.; cMw[2][1] = 1.; cMw[2][2] = 0.; 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 wY = wMo[1][3]; + + // 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); + + // Initialize the attributes of the UKF + std::shared_ptr drawer = std::make_shared(4, 0.001, 2., -1); + + 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); + } + + 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; + + 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.95 * wY; // Wrong estimation of the position along the y-axis: error of 5% + X0[2] = 0; // wZ = 0m + X0[3] = 0.75 * w * dt; // Wrong estimation of the pulsation: error of 25% + + vpUnscentedKalman::vpProcessFunction f = fx; + vpMarkersMeasurements markerMeas(cam, cMw, wRo, markers, sigmaMeasurements, seed); + vpPlateSimulator plate(radius, w, phi, wY); + using std::placeholders::_1; + vpUnscentedKalman::vpMeasurementFunction h = std::bind(&vpMarkersMeasurements::state_to_measurement, &markerMeas, _1); + + // Initialize the UKF + vpUnscentedKalman ukf(Q, R, drawer, f, h); + ukf.init(X0, P0); + +#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 z (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 + + // Initialize the display + // Depending on the detected third party libraries, we instantiate here the + // first video device which is available + 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"); + } + + // Initialize the simulation + vpColVector plate_pos = X0; + + for (unsigned int i = 0; i < 200; ++i) { + plate_pos = plate.move(dt * static_cast(i)); + + // Perform the measurement + vpColVector z = markerMeas.measureWithNoise(plate_pos); + + // Use the UKF to filter the measurement + ukf.filter(z, dt); + +#ifdef VISP_HAVE_DISPLAY + // Plot the filtered state + vpColVector Xest = ukf.getXest(); + plot.plot(0, 1, Xest[0], Xest[2]); + + // Plot the ground truth + plot.plot(0, 0, plate_pos[0], plate_pos[2]); + + // Display the projection of the markers + vpDisplay::display(Idisp); + vpColVector zGT = markerMeas.measureGT(plate_pos); + vpColVector zFilt = markerMeas.state_to_measurement(Xest); + std::vector ip; + 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); + 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 wZnoisy = wMo_noisy[2][3]; + plot.plot(0, 2, wXnoisy, wZnoisy); + + 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); +#endif + } + std::cout << "Press Enter to quit..." << std::endl; + std::cin.get(); + + if (d != nullptr) { + delete d; + } + 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