diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index 6477806fe5..5e53d521a9 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -78,6 +78,7 @@ visp_add_subdirectory(moments/points REQUIRED_DEPS visp_core visp_vs v visp_add_subdirectory(moments/polygon REQUIRED_DEPS visp_core visp_vs visp_robot visp_gui) visp_add_subdirectory(ogre-simulator REQUIRED_DEPS visp_core visp_vision visp_ar visp_blob visp_io visp_gui) visp_add_subdirectory(parse-argv REQUIRED_DEPS visp_core visp_io) +visp_add_subdirectory(particle-filter REQUIRED_DEPS visp_core visp_gui) visp_add_subdirectory(pose-estimation REQUIRED_DEPS visp_core visp_blob visp_vision visp_io visp_gui) visp_add_subdirectory(reflex-takktile REQUIRED_DEPS visp_core visp_robot) visp_add_subdirectory(robot-simulator/afma6 REQUIRED_DEPS visp_core visp_vs visp_robot visp_io visp_gui) diff --git a/example/kalman/ukf-linear-example.cpp b/example/kalman/ukf-linear-example.cpp index 030b0f2b95..003e043552 100644 --- a/example/kalman/ukf-linear-example.cpp +++ b/example/kalman/ukf-linear-example.cpp @@ -270,7 +270,7 @@ int main(const int argc, const char *argv[]) #else int main() { - std::cout << "vpUnscentedKalman is only available if you compile ViSP in C++11 standard or higher." << std::endl; + std::cout << "This example 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 index 4b5d06ee92..6ae7fadb34 100644 --- a/example/kalman/ukf-nonlinear-complex-example.cpp +++ b/example/kalman/ukf-nonlinear-complex-example.cpp @@ -677,7 +677,7 @@ int main(const int argc, const char *argv[]) #else int main() { - std::cout << "vpUnscentedKalman is only available if you compile ViSP in C++11 standard or higher." << std::endl; + std::cout << "This example 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 index e6dce7377f..c185c8d8ef 100644 --- a/example/kalman/ukf-nonlinear-example.cpp +++ b/example/kalman/ukf-nonlinear-example.cpp @@ -472,7 +472,7 @@ int main(const int argc, const char *argv[]) #else int main() { - std::cout << "vpUnscentedKalman is only available if you compile ViSP in C++11 standard or higher." << std::endl; + std::cout << "This example is only available if you compile ViSP in C++11 standard or higher." << std::endl; return 0; } #endif diff --git a/example/particle-filter/CMakeLists.txt b/example/particle-filter/CMakeLists.txt new file mode 100644 index 0000000000..7931be8865 --- /dev/null +++ b/example/particle-filter/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5) + +project(example-kalman) + +find_package(VISP REQUIRED visp_core visp_gui) + +set(example_cpp) + +list(APPEND example_cpp pf-nonlinear-example.cpp) + +foreach(cpp ${example_cpp}) + visp_add_target(${cpp}) + if(COMMAND visp_add_dependency) + visp_add_dependency(${cpp} "examples") + endif() +endforeach() + +visp_add_test(pf-nonlinear-example-monothread pf-nonlinear-example --nb-particles 500 --ampli-max-X 0.02 --ampli-max-Y 0.02 --ampli-max-Z 0.01 --nb-steps-main 300 --max-distance-likelihood 10 --nb-threads 1 --seed 4224 ${OPTION_TO_DESACTIVE_DISPLAY}) +visp_add_test(pf-nonlinear-example-multithread pf-nonlinear-example --nb-particles 500 --ampli-max-X 0.02 --ampli-max-Y 0.02 --ampli-max-Z 0.01 --nb-steps-main 300 --max-distance-likelihood 10 --nb-threads -1 --seed 4224 ${OPTION_TO_DESACTIVE_DISPLAY}) diff --git a/example/particle-filter/pf-nonlinear-example.cpp b/example/particle-filter/pf-nonlinear-example.cpp new file mode 100644 index 0000000000..af68521158 --- /dev/null +++ b/example/particle-filter/pf-nonlinear-example.cpp @@ -0,0 +1,780 @@ +/**************************************************************************** + * + * 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 pf-nonlinear-example.cpp + * Example on how to use a Particle Filter (PF) 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 PF 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 +#endif +//! [Display_includes] +#include + +//! [PF_includes] +#include +//! [PF_includes] + +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + +#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] sigmaDistance Standard deviation of the Gaussian function used for the computation of the likelihood. + * An error greater than 3 times this standard deviation will lead to a likelihood equal to 0. + * \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 &sigmaDistance, + 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) + { + double sigmaDistanceSquared = sigmaDistance * sigmaDistance; + m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared); + m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared); + } + + //! [Likelihood_function] + /** + * \brief Compute the likelihood of a particle compared to the measurements. + * The likelihood equals zero if the particle is completely different of + * the measurements and equals one if it matches completely. + * The chosen likelihood is a Gaussian function that penalizes the mean distance + * between the projection of the markers corresponding to the particle position + * and the measurements of the markers in the image. + * + * \param[in] x The particle. + * \param[in] meas The measurement vector. meas[2i] = u_i meas[2i + 1] = v_i . + * \return double The likelihood of the particle. + */ + double likelihoodParticle(const vpColVector &x, const vpColVector &meas) + { + unsigned int nbMarkers = m_markers.size(); + double likelihood = 0.; + vpHomogeneousMatrix wMo; + vpTranslationVector wTo(x[0], x[1], x[2]); + wMo.build(wTo, m_wRo); + const unsigned int sizePt2D = 2; + const unsigned int idX = 0, idY = 1, idZ = 2; + double sumError = 0.; + for (unsigned int i = 0; i < nbMarkers; ++i) { + vpColVector cX = m_cMw * wMo * m_markers[i]; + vpImagePoint projParticle; + vpMeterPixelConversion::convertPoint(m_cam, cX[idX] / cX[idZ], cX[idY] / cX[idZ], projParticle); + vpImagePoint measPt(meas[sizePt2D * i + 1], meas[sizePt2D * i]); + double error = vpImagePoint::sqrDistance(projParticle, measPt); + sumError += error; + } + likelihood = std::exp(m_constantExpDenominator * sumError / static_cast(nbMarkers)) * m_constantDenominator; + likelihood = std::min(likelihood, 1.0); // Clamp to have likelihood <= 1. + likelihood = std::max(likelihood, 0.); // Clamp to have likelihood >= 0. + return likelihood; + } + //! [Likelihood_function] + + /** + * \brief Convert the state of the PF into the measurement space. + * + * \param[in] x The state of the PF. + * \return vpColVector The state 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.build(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] + /** + * \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.build(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. + double m_constantDenominator; // Denominator of the Gaussian function used for the likelihood computation. + double m_constantExpDenominator; // Denominator of the exponential of the Gaussian function used for the likelihood computation. + 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] + +struct SoftwareArguments +{ + // --- Main loop parameters--- + static const int SOFTWARE_CONTINUE = 42; + bool m_useDisplay; //!< If true, activate the plot and the renderer if VISP_HAVE_DISPLAY is defined. + unsigned int m_nbStepsWarmUp; //!< Number of steps for the warmup phase. + unsigned int m_nbSteps; //!< ?umber of steps for the main loop. + // --- PF parameters--- + unsigned int m_N; //!< The number of particles. + double m_maxDistanceForLikelihood; //!< The maximum allowed distance between a particle and the measurement, leading to a likelihood equal to 0.. + double m_ampliMaxX; //!< Amplitude max of the noise for the state component corresponding to the X coordinate. + double m_ampliMaxY; //!< Amplitude max of the noise for the state component corresponding to the Y coordinate. + double m_ampliMaxZ; //!< Amplitude max of the noise for the state component corresponding to the Z coordinate. + double m_ampliMaxOmega; //!< Amplitude max of the noise for the state component corresponding to the pulsation. + long m_seedPF; //!< Seed for the random generators of the PF. + unsigned int m_nbThreads; //!< Number of thread to use in the Particle Filter. + + SoftwareArguments() + : m_useDisplay(true) + , m_nbStepsWarmUp(200) + , m_nbSteps(300) + , m_N(500) + , m_maxDistanceForLikelihood(10.) + , m_ampliMaxX(0.02) + , m_ampliMaxY(0.02) + , m_ampliMaxZ(0.01) + , m_ampliMaxOmega(0.02) + , m_seedPF(4224) + , m_nbThreads(1) + { } + + int parseArgs(const int argc, const char *argv[]) + { + int i = 1; + while (i < argc) { + std::string arg(argv[i]); + if ((arg == "--nb-steps-main") && ((i+1) < argc)) { + m_nbSteps = std::atoi(argv[i + 1]); + ++i; + } + else if ((arg == "--nb-steps-warmup") && ((i+1) < argc)) { + m_nbStepsWarmUp = std::atoi(argv[i + 1]); + ++i; + } + else if ((arg == "--max-distance-likelihood") && ((i+1) < argc)) { + m_maxDistanceForLikelihood = std::atof(argv[i + 1]); + ++i; + } + else if (((arg == "-N") || (arg == "--nb-particles")) && ((i+1) < argc)) { + m_N = std::atoi(argv[i + 1]); + ++i; + } + else if ((arg == "--seed") && ((i+1) < argc)) { + m_seedPF = std::atoi(argv[i + 1]); + ++i; + } + else if ((arg == "--nb-threads") && ((i+1) < argc)) { + m_nbThreads = std::atoi(argv[i + 1]); + ++i; + } + else if ((arg == "--ampli-max-X") && ((i+1) < argc)) { + m_ampliMaxX = std::atof(argv[i + 1]); + ++i; + } + else if ((arg == "--ampli-max-Y") && ((i+1) < argc)) { + m_ampliMaxY = std::atof(argv[i + 1]); + ++i; + } + else if ((arg == "--ampli-max-Z") && ((i+1) < argc)) { + m_ampliMaxZ = std::atof(argv[i + 1]); + ++i; + } + else if ((arg == "--ampli-max-omega") && ((i+1) < argc)) { + m_ampliMaxOmega = std::atof(argv[i + 1]); + ++i; + } + else if (arg == "-d") { + m_useDisplay = false; + } + else if ((arg == "-h") || (arg == "--help")) { + printUsage(std::string(argv[0])); + SoftwareArguments defaultArgs; + defaultArgs.printDetails(); + return 0; + } + else { + std::cout << "WARNING: unrecognised argument \"" << arg << "\""; + if (i + 1 < argc) { + std::cout << " with associated value(s) { "; + int nbValues = 0; + int j = i + 1; + bool hasToRun = true; + while ((j < argc) && hasToRun) { + std::string nextValue(argv[j]); + if (nextValue.find("--") == std::string::npos) { + std::cout << nextValue << " "; + ++nbValues; + } + else { + hasToRun = false; + } + ++j; + } + std::cout << "}" << std::endl; + i += nbValues; + } + } + ++i; + } + return SOFTWARE_CONTINUE; + } + +private: + void printUsage(const std::string &softName) + { + std::cout << "SYNOPSIS" << std::endl; + std::cout << " " << softName << " [--nb-steps-main ] [--nb-steps-warmup ]" << std::endl; + std::cout << " [--max-distance-likelihood ] [-N, --nb-particles ] [--seed ] [--nb-threads ]" << std::endl; + std::cout << " [--ampli-max-X ] [--ampli-max-Y ] [--ampli-max-Z ] [--ampli-max-omega ]" << std::endl; + std::cout << " [-d, --no-display] [-h]" << std::endl; + } + + void printDetails() + { + std::cout << std::endl << std::endl; + std::cout << "DETAILS" << std::endl; + std::cout << " --nb-steps-main" << std::endl; + std::cout << " Number of steps in the main loop." << std::endl; + std::cout << " Default: " << m_nbSteps << std::endl; + std::cout << std::endl; + std::cout << " --nb-steps-warmup" << std::endl; + std::cout << " Number of steps in the warmup loop." << std::endl; + std::cout << " Default: " << m_nbStepsWarmUp << std::endl; + std::cout << std::endl; + std::cout << " --max-distance-likelihood" << std::endl; + std::cout << " Maximum mean distance of the projection of the markers corresponding" << std::endl; + std::cout << " to a particle with the measurements. Above this value, the likelihood of the particle is 0." << std::endl; + std::cout << " Default: " << m_maxDistanceForLikelihood << std::endl; + std::cout << std::endl; + std::cout << " -N, --nb-particles" << std::endl; + std::cout << " Number of particles of the Particle Filter." << std::endl; + std::cout << " Default: " << m_N << std::endl; + std::cout << std::endl; + std::cout << " --seed" << std::endl; + std::cout << " Seed to initialize the Particle Filter." << std::endl; + std::cout << " Use a negative value makes to use the current timestamp instead." << std::endl; + std::cout << " Default: " << m_seedPF << std::endl; + std::cout << std::endl; + std::cout << " --nb-threads" << std::endl; + std::cout << " Set the number of threads to use in the Particle Filter (only if OpenMP is available)." << std::endl; + std::cout << " Use a negative value to use the maximum number of threads instead." << std::endl; + std::cout << " Default: " << m_nbThreads << std::endl; + std::cout << std::endl; + std::cout << " --ampli-max-X" << std::endl; + std::cout << " Maximum amplitude of the noise added to a particle along the X-axis." << std::endl; + std::cout << " Default: " << m_ampliMaxX << std::endl; + std::cout << std::endl; + std::cout << " --ampli-max-Y" << std::endl; + std::cout << " Maximum amplitude of the noise added to a particle along the Y-axis." << std::endl; + std::cout << " Default: " << m_ampliMaxY << std::endl; + std::cout << std::endl; + std::cout << " --ampli-max-Z" << std::endl; + std::cout << " Maximum amplitude of the noise added to a particle along the Z-axis." << std::endl; + std::cout << " Default: " << m_ampliMaxZ << std::endl; + std::cout << std::endl; + std::cout << " --ampli-max-omega" << std::endl; + std::cout << " Maximum amplitude of the noise added to a particle affecting the pulsation of the motion." << std::endl; + std::cout << " Default: " << m_ampliMaxOmega << std::endl; + std::cout << std::endl; + std::cout << " -d, --no-display" << std::endl; + std::cout << " Deactivate display." << std::endl; + std::cout << " Default: display is "; +#ifdef VISP_HAVE_DISPLAY + std::cout << "ON" << std::endl; +#else + std::cout << "OFF" << std::endl; +#endif + std::cout << std::endl; + std::cout << " -h, --help" << std::endl; + std::cout << " Display this help." << std::endl; + std::cout << std::endl; + } +}; + +int main(const int argc, const char *argv[]) +{ + SoftwareArguments args; + int returnCode = args.parseArgs(argc, argv); + if (returnCode != SoftwareArguments::SOFTWARE_CONTINUE) { + return returnCode; + } + + //! [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 long seedMeasurements = 42; // Seed for the measurements random generator + 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])); + } + + 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 + const double px = 600., py = 600., u0 = 320., v0 = 240.; + vpCameraParameters cam; + cam.initPersProjWithoutDistortion(px, py, u0, v0); + //! [Camera_for_measurements] + + // Initialize the attributes of the PF + //! [Constants_for_the_PF] + const double maxDistanceForLikelihood = args.m_maxDistanceForLikelihood; // The maximum allowed distance between a particle and the measurement, leading to a likelihood equal to 0.. + const double sigmaLikelihood = maxDistanceForLikelihood / 3.; // The standard deviation of likelihood function. + const unsigned int nbParticles = args.m_N; // Number of particles to use + const double ampliMaxX = args.m_ampliMaxX, ampliMaxY = args.m_ampliMaxY, ampliMaxZ = args.m_ampliMaxZ; + const double ampliMaxOmega = args.m_ampliMaxOmega; + const std::vector stdevsPF = { ampliMaxX/3., ampliMaxY/3., ampliMaxZ/3., ampliMaxOmega/3. }; // Standard deviation for each state component + const long seedPF = args.m_seedPF; // Seed for the random generators of the PF + const unsigned int nbThread = args.m_nbThreads; + //! [Constants_for_the_PF] + + //! [Initial_estimates] + vpColVector X0(4U); // 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.95 * w * dt; // Wrong estimation of the pulsation: error of 5% + //! [Initial_estimates] + + //! [Init_functions] + vpParticleFilter::vpProcessFunction processFunc = fx; + vpMarkersMeasurements markerMeas(cam, cMw, wRo, markers, sigmaLikelihood, sigmaMeasurements, seedMeasurements); + using std::placeholders::_1; + using std::placeholders::_2; + vpParticleFilter::vpLikelihoodFunction likelihoodFunc = std::bind(&vpMarkersMeasurements::likelihoodParticle, &markerMeas, _1, _2); + vpParticleFilter::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter::simpleResamplingCheck; + vpParticleFilter::vpResamplingFunction resamplingFunc = vpParticleFilter::simpleImportanceResampling; + //! [Init_functions] + + //! [Init_PF] + // Initialize the PF + vpParticleFilter filter(nbParticles, stdevsPF, seedPF, nbThread); + filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc); + //! [Init_PF] + + //! [Init_plot] +#ifdef VISP_HAVE_DISPLAY + // Initialize the plot + vpPlot *plot = nullptr; + if (args.m_useDisplay) { + plot = new vpPlot(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 + std::shared_ptr d; + vpImage Idisp(800, 800, vpRGBa(static_cast(255))); + if (args.m_useDisplay) { + d = vpDisplayFactory::createDisplay(Idisp, 800, 50, "Projection of the markers"); + } +#endif + //! [Init_renderer] + + //! [Init_simu] + // Initialize the simulation + vpObjectSimulator object(radius, w, phi, wZ); + vpColVector object_pos(4U, 0.); + object_pos[3] = 1.; + //! [Init_simu] + + double averageFilteringTime = 0.; + + //! [Warmup_loop] + const unsigned int nbStepsWarmUp = args.m_nbStepsWarmUp; + for (unsigned int i = 0; i < nbStepsWarmUp; ++i) { + // Update object pose + object_pos = object.move(dt * static_cast(i)); + + // Perform the measurement + vpColVector z = markerMeas.measureWithNoise(object_pos); + + // Use the UKF to filter the measurement + double t0 = vpTime::measureTimeMicros(); + filter.filter(z, dt); + averageFilteringTime += vpTime::measureTimeMicros() - t0; + } + //! [Warmup_loop] + + //! [Simu_loop] + const unsigned int nbSteps = args.m_nbSteps; + const double invNbSteps = 1. / static_cast(nbSteps); + double meanErrorFilter = 0.; + double meanErrorNoise = 0.; + + for (unsigned int i = 0; i < nbSteps; ++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] + + double t0 = vpTime::measureTimeMicros(); + //! [Perform_filtering] + // Use the UKF to filter the measurement + filter.filter(z, dt); + //! [Perform_filtering] + averageFilteringTime += vpTime::measureTimeMicros() - t0; + + //! [Get_filtered_state] + vpColVector Xest = filter.computeFilteredState(); + //! [Get_filtered_state] + + //! [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_displays] +#ifdef VISP_HAVE_DISPLAY + if (args.m_useDisplay) { + //! [Update_plot] + // Plot the ground truth + plot->plot(0, 0, object_pos[0], object_pos[1]); + + // Plot the filtered state + 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] + + //! [Compute_error] + double error = std::sqrt(std::pow(Xest[0] - object_pos[0], 2) + std::pow(Xest[1] - object_pos[1], 2) + std::pow(Xest[2] - object_pos[2], 2)); + meanErrorFilter += invNbSteps * error; + error = std::sqrt(std::pow(wMo_noisy[0][3] - object_pos[0], 2) + std::pow(wMo_noisy[1][3] - object_pos[1], 2) + std::pow(wMo_noisy[2][3] - object_pos[2], 2)); + meanErrorNoise += invNbSteps * error; + //! [Compute_error] + } + //! [Simu_loop] + + averageFilteringTime = averageFilteringTime / (static_cast(nbSteps) + static_cast(nbStepsWarmUp)); + std::cout << "Mean error filter = " << meanErrorFilter << std::endl; + std::cout << "Mean error noise = " << meanErrorNoise << std::endl; + std::cout << "Mean filtering time = " << averageFilteringTime << "us" << std::endl; + + if (args.m_useDisplay) { + std::cout << "Press Enter to quit..." << std::endl; + std::cin.get(); + } + + //! [Delete_displays] +#ifdef VISP_HAVE_DISPLAY + // Delete the plot if it was allocated + if (plot != nullptr) { + delete plot; + } +#endif + //! [Delete_displays] + + if (meanErrorFilter > meanErrorNoise) { + return EXIT_FAILURE; + } + + return EXIT_SUCCESS; +} +#else +int main() +{ + std::cout << "This example is only available if you compile ViSP in C++11 standard or higher." << std::endl; + return EXIT_SUCCESS; +} +#endif diff --git a/modules/core/include/visp3/core/vpParticleFilter.h b/modules/core/include/visp3/core/vpParticleFilter.h new file mode 100644 index 0000000000..5423282aa7 --- /dev/null +++ b/modules/core/include/visp3/core/vpParticleFilter.h @@ -0,0 +1,747 @@ +/* + * 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 VP_PARTICLE_FILTER_H +#define VP_PARTICLE_FILTER_H + +#include + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +#include +#include +#include + +#include // std::function + +#ifdef VISP_HAVE_OPENMP +#include +#endif + +BEGIN_VISP_NAMESPACE +/*! + \class vpParticleFilter + \tparam MeasurementsType The class that corresponds to the measurements used to compute + the weights of the Particle Filter + \brief The class permits to use a Particle Filter. +*/ +template +class vpParticleFilter +{ +public: + /** + * \brief Structure of vectors for which the i^th element of the weights vector is + * associated to the i^th element of the particles vectors. + */ + typedef struct vpParticlesWithWeights + { + std::vector m_particles; /*!< Particles vector*/ + std::vector m_weights; /*!< Weights vector.*/ + } vpParticlesWithWeights; + + /** + * \brief Function that computes either the equivalent of an addition in the state space. + * The first argument is the vector to which we must add something + * and the second argument is the thing to be added. The return is the + * result of this operation. + */ + typedef std::function vpStateAddFunction; + + /** + * \brief Process model function, which projects a particle forward in time. + * The first argument is a particle, the second is the period and the return is the + * particle projected in the future. + */ + typedef std::function vpProcessFunction; + + /** + * \brief Command model function, which projects a particle forward in time according to + * the command and its previous state. + * The first argument is the command(s), the second is the particle and the third is the period. + * The return is the updated particle after period seconds. + */ + typedef std::function vpCommandStateFunction; + + /** + * \brief Likelihood function, which evaluates the likelihood of a particle with regard to the measurements. + * The first argument is the particle that is evaluated. + * The second argument is the measurements vector. + * The return is the likelihood of the particle, which equals to 0 when the particle does not match + * at all the measurements and to 1 when it matches completely. + */ + typedef std::function vpLikelihoodFunction; + + /** + * \brief Filter function, which computes the filtered state of the particle filter. + * The first argument is the vector containing the particles, + * the second argument is the associated vector of weights and the third argument is a function + * to perform an addition in the state space. + * The return is the corresponding filtered state. + */ + typedef std::function &, const std::vector &, const vpStateAddFunction &)> vpFilterFunction; + + /** + * \brief Function that takes as argument the number of particles and the vector of weights + * associated to each particle and returns true if the resampling must be performed and false + * otherwise. + */ + typedef std::function &)> vpResamplingConditionFunction; + + /** + * \brief Function that takes as argument the vector of particles and the vector of + * associated weights. It returns a pair < new_vector_particles, new_weights >. + */ + typedef std::function &, const std::vector &)> vpResamplingFunction; + + /** + * \brief Construct a new vpParticleFilter object. + * + * \param[in] N The number of particles. + * \param[in] stdev The standard deviations of the noise, each item correspond to one component of the state. + * \param[in] seed The seed to use to create the noise generators. A negative value makes the seed to + * be based on the current time. + * \param[in] nbThreads The number of threads the user would like to use. Negative value to set the maximum number + * of threads available when using OpenMP. + */ + VP_EXPLICIT vpParticleFilter(const unsigned int &N, const std::vector &stdev, const long &seed = -1, const int &nbThreads = -1); + + inline virtual ~vpParticleFilter() { } + + /** + * \brief Set the guess of the initial state. + * + * \param[in] x0 Guess of the initial state. + * \param[in] f Process model function, which projects the particles forward in time. + * The first argument is a particle, the second is the period and the return is the + * particle projected in the future. + * \param[in] l Likelihood function, that evaluates how much a particle matches the measurements. + * 0 means that the particle does not match the measurement at all and 1 that it maches completely. + * \param[in] checkResamplingFunc The function that returns true when the filter starts to degenerate + * and false otherwise. + * \param[in] resamplingFunc The resampling function that generate new particles and associated weights + * when the filter starts to degenerate. + * \param[in] filterFunc The function to compute the filtered state from the particles and their weights. + * \param[in] addFunc The function that permits to perform an addition in the state space. + */ + void init(const vpColVector &x0, const vpProcessFunction &f, + const vpLikelihoodFunction &l, + const vpResamplingConditionFunction &checkResamplingFunc, const vpResamplingFunction &resamplingFunc, + const vpFilterFunction &filterFunc = weightedMean, + const vpStateAddFunction &addFunc = simpleAdd); + + /** + * \brief Set the guess of the initial state. + * + * \param[in] x0 Guess of the initial state. + * \param[in] bx Process model function, which projects the particles forward in time based on the previous + * state and on the input commands. The first argument is the command vector, the second is a particle and + * the last is the period. The return is the particle projected in the future. + * \param[in] l Likelihood function, that evaluates how much a particle matches the measurements. + * 0 means that the particle does not match the measurement at all and 1 that it maches completely. + * \param[in] checkResamplingFunc The function that returns true when the filter starts to degenerate + * and false otherwise. + * \param[in] resamplingFunc The resampling function that generate new particles and associated weights + * when the filter starts to degenerate. + * \param[in] filterFunc The function to compute the filtered state from the particles and their weights. + * \param[in] addFunc The function that permits to perform an addition in the state space. + */ + void init(const vpColVector &x0, const vpCommandStateFunction &bx, + const vpLikelihoodFunction &l, + const vpResamplingConditionFunction &checkResamplingFunc, const vpResamplingFunction &resamplingFunc, + const vpFilterFunction &filterFunc = weightedMean, + const vpStateAddFunction &addFunc = simpleAdd); + + /** + * \brief Perform first the prediction step and then the update step. + * If needed, resampling will also be performed. + * + * \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, use the dedicated constructor or call + * vpUnscentedKalman::setCommandStateFunction beforehand. In the second case, the process + * function will be ignored. + */ + void filter(const MeasurementsType &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, use the dedicated constructor or call + * vpUnscentedKalman::setCommandStateFunction beforehand. In the second case, the process + * function will be ignored. + */ + void predict(const double &dt, const vpColVector &u = vpColVector()); + + /** + * \brief Update the weights of the particles based on a new measurement. + * The weights will be normalized (i.e. each weight will be divided by the sum of the weights). + * + * \param[in] z The measurements at the current timestep. + */ + void update(const MeasurementsType &z); + + /** + * \brief Compute the filtered state from the particles and their associated weights. + * + * \return vpColVector The filtered state. + */ + vpColVector computeFilteredState(); + + /** + * \brief Set the process function to use when projecting the particles in the future. + * + * \param f The process function to use. + * + * \warning It will deactive the command function. + */ + inline void setProcessFunction(const vpProcessFunction &f) + { + m_f = f; + m_useCommandStateFunction = false; + m_useProcessFunction = true; + } + + /** + * \brief Set the command function to use when projecting the particles in the future. + * + * \param bx The command function to use. + * + * \warning It will deactivate the process function. + */ + inline void setCommandStateFunction(const vpCommandStateFunction &bx) + { + m_bx = bx; + m_useCommandStateFunction = true; + m_useProcessFunction = false; + } + + /** + * \brief Set the likelihood function that updates the weights of the particles + * based on the new measurements. + * + * \param likelihood The likelihood function. + */ + inline void setLikelihoodFunction(const vpLikelihoodFunction &likelihood) + { + m_likelihood = likelihood; + } + + /** + * \brief Set the filter function that compute the filtered state from the particles + * and their associated weights. + * + * \param filterFunc The filtering function to use. + */ + inline void setFilterFunction(const vpFilterFunction &filterFunc) + { + m_stateFilterFunc = filterFunc; + } + + /** + * \brief Set the function that returns true when the filter starts to degenerate + * and false otherwise. + * + * \param resamplingCondFunc The evaluation function to use. + */ + inline void setCheckResamplingFunction(const vpResamplingConditionFunction &resamplingCondFunc) + { + m_checkIfResample = resamplingCondFunc; + } + + /** + * \brief Set the resampling function that generate new particles and associated weights + * when the filter starts to degenerate. + * + * \param resamplingFunc The resampling function to use. + */ + inline void setResamplingFunction(const vpResamplingFunction &resamplingFunc) + { + m_resampling = resamplingFunc; + } + + /** + * \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 weighted mean, which just does + * \f$ \textbf{res} = \sum^{N-1}_{i=0} weights[i] \textbf{particles}[i] \f$ + * + * \param[in] particles Vector that contains all the particles. + * \param[in] weights Vector that contains the weights associated to the particles. + * \param[in] addFunc How to perform the addition. + * \return vpColVector \f$ \textbf{res} = \sum^{N-1}_{i=0} weights[i] \textbf{particles}[i] \f$ + */ + static vpColVector weightedMean(const std::vector &particles, const std::vector &weights, const vpStateAddFunction &addFunc); + + /** + * \brief Returns true if the following condition is fulfilled, or if all the particles diverged: + * \f$ \frac{2}{\sum_i (\frac{w_i}{\sum_j w_j})^2} < N \f$ + * + * \param[in] N The number of particles. + * \param[in] weights The weights associated to each particle. + * \return true Resampling must be performed. + * \return false Resampling is not needed. + */ + static bool simpleResamplingCheck(const unsigned int &N, const std::vector &weights); + + /** + * \brief Function implementing the resampling of a Simple Importance Resampling Particle Filter. + * + * \param[in] particles Vector containing the particles. + * \param[in] weights Vector containing the associated weights. + * \return vpParticlesWithWeights A pair of vector of particles and + * vector of associated weights. + */ + static vpParticlesWithWeights simpleImportanceResampling(const std::vector &particles, const std::vector &weights); + +private: + void initParticles(const vpColVector &x0); +#ifdef VISP_HAVE_OPENMP + void predictMultithread(const double &dt, const vpColVector &u); + void updateMultithread(const MeasurementsType &z); +#endif + + void predictMonothread(const double &dt, const vpColVector &u); + void updateMonothread(const MeasurementsType &z); + + static vpUniRand sampler; + static vpUniRand samplerRandomIdx; + + unsigned int m_N; /*!< Number of particles.*/ + unsigned int m_nbMaxThreads; /*!< Maximum number of threads to use.*/ + std::vector> m_noiseGenerators; /*!< The noise generator adding noise to the particles at each time step.*/ + std::vector m_particles; /*!< The particles.*/ + std::vector m_w; /*!< The weights associated to each particles.*/ + + vpColVector m_Xest; /*!< The estimated (i.e. filtered) state variables.*/ + + vpProcessFunction m_f; /*!< Process model function, which projects the sigma points forward in time.*/ + vpLikelihoodFunction m_likelihood; /*!< Likelihood function, which evaluates how much a particle matches the measurements.*/ + vpCommandStateFunction m_bx; /*!< Function that permits to compute the effect of the commands on the prior, with knowledge of the state.*/ + vpFilterFunction m_stateFilterFunc; /*!< Function to compute a weighted mean in the state space.*/ + vpResamplingConditionFunction m_checkIfResample; /*!< Return true if resampling must be performed, false otherwise.*/ + vpResamplingFunction m_resampling; /*!< Performs resampling, i.e. samples particles and weights when the particle filter degenerates.*/ + vpStateAddFunction m_stateAdd; /*!< Function to performs an addition in the state space.*/ + + bool m_useProcessFunction; /*!< Set to true when the Particle filter should use the process function.*/ + bool m_useCommandStateFunction; /*!< Set to true when the Particle filter should use the command function.*/ +}; + +template +vpUniRand vpParticleFilter::sampler; + +template +vpUniRand vpParticleFilter::samplerRandomIdx; + +template +vpParticleFilter::vpParticleFilter(const unsigned int &N, const std::vector &stdev, const long &seed, const int &nbThreads) + : m_N(N) + , m_particles(N) + , m_w(N, 1./static_cast(N)) + , m_useProcessFunction(false) + , m_useCommandStateFunction(false) +{ +#ifndef VISP_HAVE_OPENMP + m_nbMaxThreads = 1; + if (nbThreads > 1) { + std::cout << "[vpParticleFilter::vpParticleFilter] WARNING: OpenMP is not available, maximum number of threads to use clamped to 1" << std::endl; + } +#else + int maxThreads = omp_get_max_threads(); + if (nbThreads <= 0) { + m_nbMaxThreads = maxThreads; + } + else if (nbThreads > maxThreads) { + m_nbMaxThreads = maxThreads; + std::cout << "[vpParticleFilter::vpParticleFilter] WARNING: maximum number of threads to use clamped to " + << maxThreads << " instead of " << nbThreads << " due to OpenMP restrictions." << std::endl; + std::cout << "[vpParticleFilter::vpParticleFilter] If you want more, consider to use omp_set_num_threads before." << std::endl; + } + else { + m_nbMaxThreads = nbThreads; + } +#endif + // Generating the random generators + unsigned int sizeState = stdev.size(); + m_noiseGenerators.resize(m_nbMaxThreads); + unsigned long long seedForGenerator; + if (seed > 0) { + seedForGenerator = seed; + } + else { + seedForGenerator = vpTime::measureTimeMicros(); + } + + // Sampler for the simpleImportanceResampling method + sampler.setSeed(seed, 0x123465789ULL); + samplerRandomIdx.setSeed(seed + 4224, 0x123465789ULL); + + vpUniRand seedGenerator(seedForGenerator); + for (unsigned int threadId = 0; threadId < m_nbMaxThreads; ++threadId) { + for (unsigned int stateId = 0; stateId < sizeState; ++stateId) { + m_noiseGenerators[threadId].push_back(vpGaussRand(stdev[stateId], 0., seedGenerator.uniform(0., 1e9))); + } + } +} + +template +void vpParticleFilter::init(const vpColVector &x0, const vpProcessFunction &f, + const vpLikelihoodFunction &l, + const vpResamplingConditionFunction &checkResamplingFunc, const vpResamplingFunction &resamplingFunc, + const vpFilterFunction &filterFunc, const vpStateAddFunction &addFunc) +{ + m_f = f; + m_stateFilterFunc = filterFunc; + m_likelihood = l; + m_checkIfResample = checkResamplingFunc; + m_resampling = resamplingFunc; + m_stateAdd = addFunc; + m_useProcessFunction = true; + m_useCommandStateFunction = false; + + // Initialize the different particles + initParticles(x0); +} + +template +void vpParticleFilter::init(const vpColVector &x0, const vpCommandStateFunction &bx, + const vpLikelihoodFunction &l, + const vpResamplingConditionFunction &checkResamplingFunc, const vpResamplingFunction &resamplingFunc, + const vpFilterFunction &filterFunc, const vpStateAddFunction &addFunc) +{ + m_bx = bx; + m_stateFilterFunc = filterFunc; + m_likelihood = l; + m_checkIfResample = checkResamplingFunc; + m_resampling = resamplingFunc; + m_stateAdd = addFunc; + m_useProcessFunction = false; + m_useCommandStateFunction = true; + + // Initialize the different particles + initParticles(x0); +} + +template +void vpParticleFilter::filter(const MeasurementsType &z, const double &dt, const vpColVector &u) +{ + predict(dt, u); + update(z); +} + +template +void vpParticleFilter::predict(const double &dt, const vpColVector &u) +{ + if (m_nbMaxThreads == 1) { + predictMonothread(dt, u); + } +#ifdef VISP_HAVE_OPENMP + else { + predictMultithread(dt, u); + } +#endif +} + +template +void vpParticleFilter::update(const MeasurementsType &z) +{ + if (m_nbMaxThreads == 1) { + updateMonothread(z); + } +#ifdef VISP_HAVE_OPENMP + else { + updateMultithread(z); + } +#endif + bool shouldResample = m_checkIfResample(m_N, m_w); + if (shouldResample) { + vpParticlesWithWeights particles_weights = m_resampling(m_particles, m_w); + m_particles = std::move(particles_weights.m_particles); + m_w = std::move(particles_weights.m_weights); + } +} + +template +vpColVector vpParticleFilter::computeFilteredState() +{ + return m_stateFilterFunc(m_particles, m_w, m_stateAdd); +} + +template +vpColVector vpParticleFilter::weightedMean(const std::vector &particles, const std::vector &weights, const vpStateAddFunction &addFunc) +{ + size_t nbParticles = particles.size(); + if (nbParticles == 0) { + throw(vpException(vpException::dimensionError, "No particles to add when computing the mean")); + } + vpColVector res = particles[0] * weights[0]; + for (size_t i = 1; i < nbParticles; ++i) { + res = addFunc(res, particles[i] * weights[i]); + } + return res; +} + +template +bool vpParticleFilter::simpleResamplingCheck(const unsigned int &N, const std::vector &weights) +{ + double sumSquare = 0.; + for (unsigned int i = 0; i < N; ++i) { + sumSquare += weights[i] * weights[i]; + } + if (sumSquare < std::numeric_limits::epsilon()) { + // All the particles diverged + return true; + } + double N_eff = 1.0 / sumSquare; + return (N_eff < (N / 2.0)); +} + +template +typename vpParticleFilter::vpParticlesWithWeights vpParticleFilter::simpleImportanceResampling(const std::vector &particles, const std::vector &weights) +{ + unsigned int nbParticles = particles.size(); + double x = 0.; + double sumWeights = 0.; + std::vector idx(nbParticles); + + // Draw indices of the randomly chosen particles from the vector of particles + for (unsigned int i = 0; i < nbParticles; ++i) { + x = sampler(); + sumWeights = 0.0; + int index = samplerRandomIdx.uniform(0, nbParticles); // In case all the weights are null + for (unsigned int j = 0; j < nbParticles; ++j) { + if (x < sumWeights + weights[j]) { + index = j; + break; + } + sumWeights += weights[j]; + } + idx[i] = index; + } + + // Draw the randomly chosen particles corresponding to the indices + vpParticlesWithWeights newParticlesWeights; + newParticlesWeights.m_particles.resize(nbParticles); + for (unsigned int i = 0; i < nbParticles; ++i) { + newParticlesWeights.m_particles[i] = particles[idx[i]]; + } + + // Reinitialize the weights + newParticlesWeights.m_weights.resize(nbParticles, 1.0/ static_cast(nbParticles)); + return newParticlesWeights; +} + +template +void vpParticleFilter::initParticles(const vpColVector &x0) +{ + unsigned int sizeState = x0.size(); + unsigned int chunkSize = m_N / m_nbMaxThreads; + double uniformWeight = 1. / static_cast(m_N); + for (unsigned int i = 0; i < m_nbMaxThreads; ++i) { + unsigned int idStart = chunkSize * i; + unsigned int idStop = chunkSize * (i + 1); + // Last chunk must go until the end + if (i == m_nbMaxThreads - 1) { + idStop = m_N; + } + for (unsigned int id = idStart; id < idStop; ++id) { + // Generating noise + vpColVector noise(sizeState); + for (unsigned int idState = 0; idState < sizeState; ++idState) { + noise[idState] = m_noiseGenerators[i][idState](); + } + // Adding noise to the initial state + m_particles[id] = m_stateAdd(x0, noise); + + // (Re)initializing its weight + m_w[id] = uniformWeight; + } + } +} + +#ifdef VISP_HAVE_OPENMP +template +void vpParticleFilter::predictMultithread(const double &dt, const vpColVector &u) +{ + int iam, nt, ipoints, istart, npoints(m_N); + unsigned int sizeState = m_particles[0].size(); +#pragma omp parallel default(shared) private(iam, nt, ipoints, istart) + { + iam = omp_get_thread_num(); + nt = omp_get_num_threads(); + ipoints = npoints / nt; + // size of partition + istart = iam * ipoints; // starting array index + if (iam == nt-1) { + // last thread may do more + ipoints = npoints - istart; + } + + for (int i = istart; i< istart + ipoints; ++i) { + // Updating the particles following the process (or command) function + if (m_useCommandStateFunction) { + m_particles[i] = m_bx(u, m_particles[i], dt); + } + else if (m_useProcessFunction) { + m_particles[i] = m_f(m_particles[i], dt); + } + + // Generating noise to add to the particle + vpColVector noise(sizeState); + for (unsigned int j = 0; j < sizeState; ++j) { + noise[j] = m_noiseGenerators[iam][j](); + } + + // Adding the noise to the particle + m_particles[i] = m_stateAdd(m_particles[i], noise); + } + } +} + +template +double threadLikelihood(const typename vpParticleFilter::vpLikelihoodFunction &likelihood, const std::vector &v_particles, + const vpColVector &z, std::vector &w, const int &istart, const int &ipoints) +{ + double sum(0.0); + for (int i = istart; i< istart + ipoints; ++i) { + w[i] = w[i] * likelihood(v_particles[i], z); + sum += w[i]; + } + return sum; +} + +template +void vpParticleFilter::updateMultithread(const MeasurementsType &z) +{ + double sumWeights = 0.0; + int iam, nt, ipoints, istart, npoints(m_N); + vpColVector tempSums(m_nbMaxThreads, 0.0); + // Compute the weights depending on the likelihood of a particle with regard to the measurements +#pragma omp parallel default(shared) private(iam, nt, ipoints, istart) + { + iam = omp_get_thread_num(); + nt = omp_get_num_threads(); + ipoints = npoints / nt; + // size of partition + istart = iam * ipoints; // starting array index + if (iam == nt-1) { + // last thread may do more + ipoints = npoints - istart; + } + tempSums[iam] = threadLikelihood(m_likelihood, m_particles, z, m_w, istart, ipoints); + } + sumWeights = tempSums.sum(); + + if (sumWeights > std::numeric_limits::epsilon()) { +#pragma omp parallel default(shared) private(iam, nt, ipoints, istart) + { + iam = omp_get_thread_num(); + nt = omp_get_num_threads(); + ipoints = npoints / nt; + // size of partition + istart = iam * ipoints; // starting array index + if (iam == nt-1) { + // last thread may do more + ipoints = npoints - istart; + } + + // Normalize the weights + for (int i = istart; i < istart + ipoints; ++i) { + m_w[i] = m_w[i] / sumWeights; + } + } + } +} +#endif + +template +void vpParticleFilter::predictMonothread(const double &dt, const vpColVector &u) +{ + unsigned int sizeState = m_particles[0].size(); + for (unsigned int i = 0; i < m_N; ++i) { + // Updating the particle following the process (or command) function + if (m_useCommandStateFunction) { + m_particles[i] = m_bx(u, m_particles[i], dt); + } + else if (m_useProcessFunction) { + m_particles[i] = m_f(m_particles[i], dt); + } + else { + throw(vpException(vpException::notInitialized, "vpParticleFilter has not been initialized before calling predict")); + } + + // Generating noise to add to the particle + vpColVector noise(sizeState); + for (unsigned int j = 0; j < sizeState; ++j) { + noise[j] = m_noiseGenerators[0][j](); + } + + // Adding the noise to the particle + m_particles[i] = m_stateAdd(m_particles[i], noise); + } +} + +template +void vpParticleFilter::updateMonothread(const MeasurementsType &z) +{ + double sumWeights = 0.; + // Compute the weights depending on the likelihood of a particle with regard to the measurements + for (unsigned int i = 0; i < m_N; ++i) { + m_w[i] = m_w[i] * m_likelihood(m_particles[i], z); + sumWeights += m_w[i]; + } + + // Normalize the weights + if (sumWeights > std::numeric_limits::epsilon()) { + for (unsigned int i = 0; i < m_N; ++i) { + m_w[i] = m_w[i] / sumWeights; + } + } +} +END_VISP_NAMESPACE +#endif +#endif diff --git a/modules/core/include/visp3/core/vpUnscentedKalman.h b/modules/core/include/visp3/core/vpUnscentedKalman.h index bbdc7412e0..d3a5abcca0 100644 --- a/modules/core/include/visp3/core/vpUnscentedKalman.h +++ b/modules/core/include/visp3/core/vpUnscentedKalman.h @@ -182,15 +182,6 @@ class VISP_EXPORT vpUnscentedKalman */ typedef std::function vpAddSubFunction; - /** - * \brief Residual function, which computes either the equivalent of the subtraction in the - * state space or the equivalent of the subtraction in the measurement space. - * The first argument is the vector to which we must subtract something - * and the second argument is the thing to be subtracted. The return is the - * result of this "subtraction". - */ - typedef std::function vpAddFunction; - /** * \brief Construct a new vpUnscentedKalman object. * diff --git a/modules/core/src/math/misc/vpStatisticalTestAbstract.cpp b/modules/core/src/math/misc/vpStatisticalTestAbstract.cpp index 3034fcd812..4ffa61febc 100644 --- a/modules/core/src/math/misc/vpStatisticalTestAbstract.cpp +++ b/modules/core/src/math/misc/vpStatisticalTestAbstract.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * diff --git a/modules/core/src/math/misc/vpStatisticalTestEWMA.cpp b/modules/core/src/math/misc/vpStatisticalTestEWMA.cpp index 78be0ed89f..167753b4ba 100644 --- a/modules/core/src/math/misc/vpStatisticalTestEWMA.cpp +++ b/modules/core/src/math/misc/vpStatisticalTestEWMA.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * diff --git a/modules/core/src/math/misc/vpStatisticalTestHinkley.cpp b/modules/core/src/math/misc/vpStatisticalTestHinkley.cpp index 73c9dd186c..d5b1856986 100644 --- a/modules/core/src/math/misc/vpStatisticalTestHinkley.cpp +++ b/modules/core/src/math/misc/vpStatisticalTestHinkley.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * diff --git a/modules/core/src/math/misc/vpStatisticalTestMeanAdjustedCUSUM.cpp b/modules/core/src/math/misc/vpStatisticalTestMeanAdjustedCUSUM.cpp index ca113164d5..6f347774e2 100644 --- a/modules/core/src/math/misc/vpStatisticalTestMeanAdjustedCUSUM.cpp +++ b/modules/core/src/math/misc/vpStatisticalTestMeanAdjustedCUSUM.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * diff --git a/modules/core/src/math/misc/vpStatisticalTestShewhart.cpp b/modules/core/src/math/misc/vpStatisticalTestShewhart.cpp index 74460225da..c32afc8412 100644 --- a/modules/core/src/math/misc/vpStatisticalTestShewhart.cpp +++ b/modules/core/src/math/misc/vpStatisticalTestShewhart.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * diff --git a/modules/core/src/math/misc/vpStatisticalTestSigma.cpp b/modules/core/src/math/misc/vpStatisticalTestSigma.cpp index cfb0d3cfbe..a577faa802 100644 --- a/modules/core/src/math/misc/vpStatisticalTestSigma.cpp +++ b/modules/core/src/math/misc/vpStatisticalTestSigma.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * diff --git a/modules/gui/include/visp3/gui/vpDisplayFactory.h b/modules/gui/include/visp3/gui/vpDisplayFactory.h index 6d672353c4..16c68f3e10 100644 --- a/modules/gui/include/visp3/gui/vpDisplayFactory.h +++ b/modules/gui/include/visp3/gui/vpDisplayFactory.h @@ -43,6 +43,10 @@ #include #include +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) +#include +#endif + BEGIN_VISP_NAMESPACE /** * \ingroup group_gui_display @@ -56,7 +60,7 @@ namespace vpDisplayFactory * \return A newly allocated vpDisplay specialization * if a GUI library is available or nullptr otherwise. */ -vpDisplay *displayFactory() +vpDisplay *allocateDisplay() { #if defined(VISP_HAVE_DISPLAY) #ifdef VISP_HAVE_X11 @@ -81,38 +85,87 @@ vpDisplay *displayFactory() * * \tparam T : Any type that an image can handle and that can be displayed. * \param[in] I : The image the display must be initialized with. + * \param[in] winx : The horizontal position of the display on the screen. + * \param[in] winy : The vertical position of the display on the screen. + * \param[in] title : The title of the display. + * \param[in] scaleType : If this parameter is set to: + * - vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image is fully displayed in the screen; + * - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size. + * - vpDisplay::SCALE_2, the display size is down scaled by 2 along the lines and the columns. + * - vpDisplay::SCALE_3, the display size is down scaled by 3 along the lines and the columns. + * - vpDisplay::SCALE_4, the display size is down scaled by 4 along the lines and the columns. + * - vpDisplay::SCALE_5, the display size is down scaled by 5 along the lines and the columns. * * \return A newly allocated vpDisplay specialization initialized with \b I * if a GUI library is available or nullptr otherwise. + * \warning The user must free the memory when the display is not used anymore. */ template -vpDisplay *displayFactory(vpImage &I) +vpDisplay *allocateDisplay(vpImage &I, const int winx = -1, const int winy = -1, const std::string &title = "", + const vpDisplay::vpScaleType &scaleType = vpDisplay::SCALE_DEFAULT) { #if defined(VISP_HAVE_DISPLAY) #ifdef VISP_HAVE_X11 - return new vpDisplayX(I); + return new vpDisplayX(I, winx, winy, title, scaleType); #elif defined(VISP_HAVE_GDI) - return new vpDisplayGDI(I); + return new vpDisplayGDI(I, winx, winy, title, scaleType); #elif defined(HAVE_OPENCV_HIGHGUI) - return new vpDisplayOpenCV(I); + return new vpDisplayOpenCV(I, winx, winy, title, scaleType); #elif defined(VISP_HAVE_GTK) - return new vpDisplayGTK(I); + return new vpDisplayGTK(I, winx, winy, title, scaleType); #elif defined(VISP_HAVE_D3D9) - return new vpDisplayD3D(I); + return new vpDisplayD3D(I, winx, winy, title, scaleType); #endif #else (void)I; + (void)winx; + (void)winy; + (void)title; + (void)scaleType; return nullptr; #endif } +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) /** - * \brief Return a newly allocated vpDisplay specialization initialized with \b I + * \brief Return a smart pointer vpDisplay specialization initialized with \b I + * if a GUI library is available or nullptr otherwise. + * + * \tparam T : Any type that an image can handle and that can be displayed. + * + * \return A smart pointer pointing to a vpDisplay specialization initialized with \b I + * if a GUI library is available or nullptr otherwise. + */ +template +std::shared_ptr createDisplay() +{ +#if defined(VISP_HAVE_DISPLAY) +#ifdef VISP_HAVE_X11 + return std::make_shared(); +#elif defined(VISP_HAVE_GDI) + return std::make_shared(); +#elif defined(HAVE_OPENCV_HIGHGUI) + return std::make_shared(); +#elif defined(VISP_HAVE_GTK) + return std::make_shared(); +#elif defined(VISP_HAVE_D3D9) + return std::make_shared(); +#endif +#else + return std::shared_ptr(nullptr); +#endif +} + +/** + * \brief Return a smart pointer vpDisplay specialization initialized with \b I * if a GUI library is available or nullptr otherwise. * * \tparam T : Any type that an image can handle and that can be displayed. * \param[in] I : The image the display must be initialized with. - * \param[in] scale_type : If this parameter is set to: + * \param[in] winx : The horizontal position of the display on the screen. + * \param[in] winy : The vertical position of the display on the screen. + * \param[in] title : The title of the display. + * \param[in] scaleType : If this parameter is set to: * - vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image is fully displayed in the screen; * - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size. * - vpDisplay::SCALE_2, the display size is down scaled by 2 along the lines and the columns. @@ -120,30 +173,37 @@ vpDisplay *displayFactory(vpImage &I) * - vpDisplay::SCALE_4, the display size is down scaled by 4 along the lines and the columns. * - vpDisplay::SCALE_5, the display size is down scaled by 5 along the lines and the columns. * - * \return A newly allocated vpDisplay specialization initialized with \b I + * \return A smart pointer pointing to a vpDisplay specialization initialized with \b I * if a GUI library is available or nullptr otherwise. */ template -vpDisplay *displayFactory(vpImage &I, vpDisplay::vpScaleType scale_type) +std::shared_ptr createDisplay(vpImage &I, const int winx = -1, const int winy = -1, + const std::string &title = "", const vpDisplay::vpScaleType &scaleType = vpDisplay::SCALE_DEFAULT) { #if defined(VISP_HAVE_DISPLAY) #ifdef VISP_HAVE_X11 - return new vpDisplayX(I, scale_type); + return std::make_shared(I, winx, winy, title, scaleType); #elif defined(VISP_HAVE_GDI) - return new vpDisplayGDI(I, scale_type); + return std::make_shared(I, winx, winy, title, scaleType); #elif defined(HAVE_OPENCV_HIGHGUI) - return new vpDisplayOpenCV(I, scale_type); + return std::make_shared(I, winx, winy, title, scaleType); #elif defined(VISP_HAVE_GTK) - return new vpDisplayGTK(I, scale_type); + return std::make_shared(I, winx, winy, title, scaleType); #elif defined(VISP_HAVE_D3D9) - return new vpDisplayD3D(I, scale_type); + return std::make_shared(I, winx, winy, title, scaleType); #endif #else (void)I; - (void)scale_type; + (void)winx; + (void)winy; + (void)title; + (void)scaleType; return nullptr; + return std::shared_ptr(nullptr); #endif } +#endif + } END_VISP_NAMESPACE #endif diff --git a/tutorial/grabber/tutorial-grabber-1394-writer.cpp b/tutorial/grabber/tutorial-grabber-1394-writer.cpp index 9200e9d847..3a51467b60 100644 --- a/tutorial/grabber/tutorial-grabber-1394-writer.cpp +++ b/tutorial/grabber/tutorial-grabber-1394-writer.cpp @@ -12,7 +12,7 @@ int main(int argc, char **) using namespace VISP_NAMESPACE_NAME; #endif #ifdef VISP_HAVE_DISPLAY - vpDisplay *d = vpDisplayFactory::displayFactory(); + vpDisplay *d = vpDisplayFactory::allocateDisplay(); #else std::cout << "No image viewer is available..." << std::endl; #endif diff --git a/tutorial/grabber/tutorial-grabber-v4l2-threaded.cpp b/tutorial/grabber/tutorial-grabber-v4l2-threaded.cpp index a2d5c49357..ca1bf9f916 100644 --- a/tutorial/grabber/tutorial-grabber-v4l2-threaded.cpp +++ b/tutorial/grabber/tutorial-grabber-v4l2-threaded.cpp @@ -59,7 +59,7 @@ void displayFunction(std::mutex &mutex_capture, vpImage &frame, t t_CaptureState capture_state_; bool display_initialized_ = false; - vpDisplay *d_ = vpDisplayFactory::displayFactory(); + vpDisplay *d_ = vpDisplayFactory::allocateDisplay(); do { mutex_capture.lock(); diff --git a/tutorial/grabber/tutorial-video-reader.cpp b/tutorial/grabber/tutorial-video-reader.cpp index 5dec916e84..ba0412ec5e 100644 --- a/tutorial/grabber/tutorial-video-reader.cpp +++ b/tutorial/grabber/tutorial-video-reader.cpp @@ -47,7 +47,7 @@ int main(int argc, char **argv) std::cout << "Video dimension: " << I.getWidth() << " " << I.getHeight() << std::endl; #ifdef VISP_HAVE_DISPLAY - vpDisplay *d = vpDisplayFactory::displayFactory(I); + vpDisplay *d = vpDisplayFactory::allocateDisplay(I); #else std::cout << "No image viewer is available..." << std::endl; #endif diff --git a/tutorial/grabber/tutorial-video-recorder.cpp b/tutorial/grabber/tutorial-video-recorder.cpp index 32b3e79de9..336a8c16dc 100644 --- a/tutorial/grabber/tutorial-video-recorder.cpp +++ b/tutorial/grabber/tutorial-video-recorder.cpp @@ -80,7 +80,7 @@ int main(int argc, const char *argv[]) std::cout << "Image size: " << I.getWidth() << " " << I.getHeight() << std::endl; #if defined(VISP_HAVE_DISPLAY) - vpDisplay *d = vpDisplayFactory::displayFactory(); + vpDisplay *d = vpDisplayFactory::allocateDisplay(); d->init(I, 0, 0, "Camera view"); #endif vpVideoWriter writer; diff --git a/tutorial/image/tutorial-draw-circle.cpp b/tutorial/image/tutorial-draw-circle.cpp index 02e5893277..8e529d2672 100644 --- a/tutorial/image/tutorial-draw-circle.cpp +++ b/tutorial/image/tutorial-draw-circle.cpp @@ -16,7 +16,7 @@ int main() try { { #if defined(VISP_HAVE_DISPLAY) - vpDisplay *d = vpDisplayFactory::displayFactory(I, vpDisplay::SCALE_AUTO); + vpDisplay *d = vpDisplayFactory::allocateDisplay(I, vpDisplay::SCALE_AUTO); #else std::cout << "No gui available to display gray level image..." << std::endl; #endif @@ -64,7 +64,7 @@ int main() //! [Circle draw color] #if defined(VISP_HAVE_DISPLAY) - vpDisplay *d = vpDisplayFactory::displayFactory(I_rgb, vpDisplay::SCALE_AUTO); + vpDisplay *d = vpDisplayFactory::allocateDisplay(I_rgb, vpDisplay::SCALE_AUTO); #else std::cout << "No gui available to display color image..." << std::endl; #endif diff --git a/tutorial/kalman/tutorial-ukf.cpp b/tutorial/kalman/tutorial-ukf.cpp index 2f552719a2..095aeaea84 100644 --- a/tutorial/kalman/tutorial-ukf.cpp +++ b/tutorial/kalman/tutorial-ukf.cpp @@ -488,7 +488,7 @@ int main(/*const int argc, const char *argv[]*/) #else int main() { - std::cout << "vpUnscentedKalman is only available if you compile ViSP in C++11 standard or higher." << std::endl; + std::cout << "This example is only available if you compile ViSP in C++11 standard or higher." << std::endl; return 0; } #endif diff --git a/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-ibvs.cpp b/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-ibvs.cpp index 1c43c012dc..49addfa573 100644 --- a/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-ibvs.cpp +++ b/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-ibvs.cpp @@ -121,7 +121,7 @@ int main(int argc, const char **argv) vpImage O; #ifdef VISP_HAVE_DISPLAY if (display_on) { - d = vpDisplayFactory::displayFactory(I); + d = vpDisplayFactory::allocateDisplay(I); } #endif diff --git a/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-pbvs.cpp b/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-pbvs.cpp index 57328f50d7..9ed3e50003 100644 --- a/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-pbvs.cpp +++ b/tutorial/robot/mbot/raspberry/visp/mbot-apriltag-pbvs.cpp @@ -110,7 +110,7 @@ int main(int argc, const char **argv) vpImage O; #ifdef VISP_HAVE_DISPLAY if (display_on) { - d = vpDisplayFactory::displayFactory(I); + d = vpDisplayFactory::allocateDisplay(I); } #endif