[TEST] Made the vpUniRand static to be able to have the same seed use…

…d every time the tests are run
lagadic · Jun 19, 2024 · c10bb08 · c10bb08
1 parent 60dd61e
commit c10bb08
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 7 deletions.
diff --git 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
@@ -305,7 +305,7 @@ class VISP_EXPORT vpParticleFilter
   static vpColVector weightedMean(const std::vector<vpColVector> &particles, const std::vector<double> &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<std::vector<vpGaussRand>> 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
@@ -46,6 +46,9 @@
 
 BEGIN_VISP_NAMESPACE
 
+vpUniRand vpParticleFilter::sampler;
+vpUniRand vpParticleFilter::samplerRandomIdx;
+
 vpParticleFilter::vpParticleFilter(const unsigned int &N, const std::vector<double> &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<doub
   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) {
@@ -187,14 +195,16 @@ bool vpParticleFilter::simpleResamplingCheck(const unsigned int &N, const std::v
   for (unsigned int i = 0; i < N; ++i) {
     sumSquare += weights[i] * weights[i];
   }
+  if (sumSquare < std::numeric_limits<double>::epsilon()) {
+    // All the particles diverged
+    return true;
+  }
   double N_eff = 1.0 / sumSquare;
-  return (N_eff < (N / 2.0)) || (sumSquare < std::numeric_limits<double>::epsilon());
+  return (N_eff < (N / 2.0));
 }
 
 vpParticleFilter::vpParticlesWithWeights vpParticleFilter::simpleImportanceResampling(const std::vector<vpColVector> &particles, const std::vector<double> &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;