diff --git a/example/particle-filter/CMakeLists.txt b/example/particle-filter/CMakeLists.txt index 654ffb1cbf..7931be8865 100644 --- a/example/particle-filter/CMakeLists.txt +++ b/example/particle-filter/CMakeLists.txt @@ -15,5 +15,5 @@ foreach(cpp ${example_cpp}) 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 ${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 ${OPTION_TO_DESACTIVE_DISPLAY}) +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/modules/core/include/visp3/core/vpParticleFilter.h b/modules/core/include/visp3/core/vpParticleFilter.h index 4b07026e4f..64843ccb6d 100644 --- a/modules/core/include/visp3/core/vpParticleFilter.h +++ b/modules/core/include/visp3/core/vpParticleFilter.h @@ -305,7 +305,7 @@ class VISP_EXPORT vpParticleFilter static vpColVector weightedMean(const std::vector &particles, const std::vector &weights, const vpStateAddFunction &addFunc); /** - * \brief Returns true if the following condition is fulfilled, false otherwise: + * \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. @@ -335,6 +335,9 @@ class VISP_EXPORT vpParticleFilter void predictMonothread(const double &dt, const vpColVector &u); void updateMonothread(const vpColVector &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.*/ diff --git a/modules/core/src/math/misc/vpParticleFilter.cpp b/modules/core/src/math/misc/vpParticleFilter.cpp index bc03f7c1b7..5178be8b46 100644 --- a/modules/core/src/math/misc/vpParticleFilter.cpp +++ b/modules/core/src/math/misc/vpParticleFilter.cpp @@ -46,6 +46,9 @@ BEGIN_VISP_NAMESPACE +vpUniRand vpParticleFilter::sampler; +vpUniRand vpParticleFilter::samplerRandomIdx; + vpParticleFilter::vpParticleFilter(const unsigned int &N, const std::vector &stdev, const long &seed, const int &nbThreads) : m_N(N) , m_particles(N) @@ -83,6 +86,11 @@ vpParticleFilter::vpParticleFilter(const unsigned int &N, const std::vector::epsilon()) { + // All the particles diverged + return true; + } double N_eff = 1.0 / sumSquare; - return (N_eff < (N / 2.0)) || (sumSquare < std::numeric_limits::epsilon()); + return (N_eff < (N / 2.0)); } vpParticleFilter::vpParticlesWithWeights vpParticleFilter::simpleImportanceResampling(const std::vector &particles, const std::vector &weights) { - static vpUniRand sampler(vpTime::measureTimeMicros()); - static vpUniRand samplerRandomIdx(vpTime::measureTimeMicros() + 4224); unsigned int nbParticles = particles.size(); double x = 0.; double sumWeights = 0.; @@ -204,7 +214,7 @@ vpParticleFilter::vpParticlesWithWeights vpParticleFilter::simpleImportanceResam for (unsigned int i = 0; i < nbParticles; ++i) { x = sampler(); sumWeights = 0.0; - int index = samplerRandomIdx.uniform(0, nbParticles); + 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;