Visual Servoing Platform  version 3.6.1 under development (2024-10-15)
vpParticleFilter.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Display a point cloud using PCL library.
32  */
33 
34 #ifndef VP_PARTICLE_FILTER_H
35 #define VP_PARTICLE_FILTER_H
36 
37 #include <visp3/core/vpConfig.h>
38 
39 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
40 #include <visp3/core/vpColVector.h>
41 #include <visp3/core/vpGaussRand.h>
42 #include <visp3/core/vpMatrix.h>
43 
44 #include <functional> // std::function
45 
46 #ifdef VISP_HAVE_OPENMP
47 #include <omp.h>
48 #endif
49 
50 BEGIN_VISP_NAMESPACE
95 template <typename MeasurementsType>
97 {
98 public:
103  typedef struct vpParticlesWithWeights
104  {
105  std::vector<vpColVector> m_particles;
106  std::vector<double> m_weights;
108 
115  typedef std::function<vpColVector(const vpColVector &, const vpColVector &)> vpStateAddFunction;
116 
122  typedef std::function<vpColVector(const vpColVector &, const double &)> vpProcessFunction;
123 
130  typedef std::function<vpColVector(const vpColVector &, const vpColVector &, const double &)> vpCommandStateFunction;
131 
139  typedef std::function<double(const vpColVector &, const MeasurementsType &)> vpLikelihoodFunction;
140 
148  typedef std::function<vpColVector(const std::vector<vpColVector> &, const std::vector<double> &, const vpStateAddFunction &)> vpFilterFunction;
149 
155  typedef std::function<bool(const unsigned int &, const std::vector<double> &)> vpResamplingConditionFunction;
156 
161  typedef std::function<vpParticlesWithWeights(const std::vector<vpColVector> &, const std::vector<double> &)> vpResamplingFunction;
162 
173  VP_EXPLICIT vpParticleFilter(const unsigned int &N, const std::vector<double> &stdev, const long &seed = -1, const int &nbThreads = -1);
174 
175  inline virtual ~vpParticleFilter() { }
176 
193  void init(const vpColVector &x0, const vpProcessFunction &f,
194  const vpLikelihoodFunction &l,
195  const vpResamplingConditionFunction &checkResamplingFunc, const vpResamplingFunction &resamplingFunc,
196  const vpFilterFunction &filterFunc = weightedMean,
197  const vpStateAddFunction &addFunc = simpleAdd);
198 
215  void init(const vpColVector &x0, const vpCommandStateFunction &bx,
216  const vpLikelihoodFunction &l,
217  const vpResamplingConditionFunction &checkResamplingFunc, const vpResamplingFunction &resamplingFunc,
218  const vpFilterFunction &filterFunc = weightedMean,
219  const vpStateAddFunction &addFunc = simpleAdd);
220 
233  void filter(const MeasurementsType &z, const double &dt, const vpColVector &u = vpColVector());
234 
245  void predict(const double &dt, const vpColVector &u = vpColVector());
246 
253  void update(const MeasurementsType &z);
254 
261 
269  inline void setProcessFunction(const vpProcessFunction &f)
270  {
271  m_f = f;
272  m_useCommandStateFunction = false;
273  m_useProcessFunction = true;
274  }
275 
284  {
285  m_bx = bx;
286  m_useCommandStateFunction = true;
287  m_useProcessFunction = false;
288  }
289 
296  inline void setLikelihoodFunction(const vpLikelihoodFunction &likelihood)
297  {
298  m_likelihood = likelihood;
299  }
300 
307  inline void setFilterFunction(const vpFilterFunction &filterFunc)
308  {
309  m_stateFilterFunc = filterFunc;
310  }
311 
318  inline void setCheckResamplingFunction(const vpResamplingConditionFunction &resamplingCondFunc)
319  {
320  m_checkIfResample = resamplingCondFunc;
321  }
322 
329  inline void setResamplingFunction(const vpResamplingFunction &resamplingFunc)
330  {
331  m_resampling = resamplingFunc;
332  }
333 
341  inline static vpColVector simpleAdd(const vpColVector &a, const vpColVector &toAdd)
342  {
343  vpColVector res = a + toAdd;
344  return res;
345  }
346 
356  static vpColVector weightedMean(const std::vector<vpColVector> &particles, const std::vector<double> &weights, const vpStateAddFunction &addFunc);
357 
367  static bool simpleResamplingCheck(const unsigned int &N, const std::vector<double> &weights);
368 
377  static vpParticlesWithWeights simpleImportanceResampling(const std::vector<vpColVector> &particles, const std::vector<double> &weights);
378 
379 private:
380  void initParticles(const vpColVector &x0);
381 #ifdef VISP_HAVE_OPENMP
382  void predictMultithread(const double &dt, const vpColVector &u);
383  void updateMultithread(const MeasurementsType &z);
384 #endif
385 
386  void predictMonothread(const double &dt, const vpColVector &u);
387  void updateMonothread(const MeasurementsType &z);
388 
389  static vpUniRand sampler;
390  static vpUniRand samplerRandomIdx;
391 
392  unsigned int m_N;
393  unsigned int m_nbMaxThreads;
394  std::vector<std::vector<vpGaussRand>> m_noiseGenerators;
395  std::vector<vpColVector> m_particles;
396  std::vector<double> m_w;
398  vpColVector m_Xest;
400  vpProcessFunction m_f;
401  vpLikelihoodFunction m_likelihood;
403  vpFilterFunction m_stateFilterFunc;
404  vpResamplingConditionFunction m_checkIfResample;
405  vpResamplingFunction m_resampling;
406  vpStateAddFunction m_stateAdd;
408  bool m_useProcessFunction;
409  bool m_useCommandStateFunction;
410 };
411 
412 template <typename MeasurementsType>
414 
415 template <typename MeasurementsType>
417 
418 template <typename MeasurementsType>
419 vpParticleFilter<MeasurementsType>::vpParticleFilter(const unsigned int &N, const std::vector<double> &stdev, const long &seed, const int &nbThreads)
420  : m_N(N)
421  , m_particles(N)
422  , m_w(N, 1./static_cast<double>(N))
423  , m_useProcessFunction(false)
424  , m_useCommandStateFunction(false)
425 {
426 #ifndef VISP_HAVE_OPENMP
427  m_nbMaxThreads = 1;
428  if (nbThreads > 1) {
429  std::cout << "[vpParticleFilter::vpParticleFilter] WARNING: OpenMP is not available, maximum number of threads to use clamped to 1" << std::endl;
430  }
431 #else
432  int maxThreads = omp_get_max_threads();
433  if (nbThreads <= 0) {
434  m_nbMaxThreads = maxThreads;
435  }
436  else if (nbThreads > maxThreads) {
437  m_nbMaxThreads = maxThreads;
438  std::cout << "[vpParticleFilter::vpParticleFilter] WARNING: maximum number of threads to use clamped to "
439  << maxThreads << " instead of " << nbThreads << " due to OpenMP restrictions." << std::endl;
440  std::cout << "[vpParticleFilter::vpParticleFilter] If you want more, consider to use omp_set_num_threads before." << std::endl;
441  }
442  else {
443  m_nbMaxThreads = nbThreads;
444  }
445  omp_set_num_threads(m_nbMaxThreads);
446 #endif
447  // Generating the random generators
448  unsigned int sizeState = static_cast<unsigned int>(stdev.size());
449  m_noiseGenerators.resize(m_nbMaxThreads);
450  unsigned long long seedForGenerator;
451  if (seed > 0) {
452  seedForGenerator = seed;
453  }
454  else {
455  seedForGenerator = static_cast<unsigned long long>(vpTime::measureTimeMicros());
456  }
457 
458  // Sampler for the simpleImportanceResampling method
459  sampler.setSeed(seed, 0x123465789ULL);
460  samplerRandomIdx.setSeed(seed + 4224, 0x123465789ULL);
461 
462  vpUniRand seedGenerator(seedForGenerator);
463  for (unsigned int threadId = 0; threadId < m_nbMaxThreads; ++threadId) {
464  for (unsigned int stateId = 0; stateId < sizeState; ++stateId) {
465  m_noiseGenerators[threadId].push_back(vpGaussRand(stdev[stateId], 0., static_cast<long>(seedGenerator.uniform(0., 1e9))));
466  }
467  }
468 }
469 
470 template <typename MeasurementsType>
472  const vpLikelihoodFunction &l,
473  const vpResamplingConditionFunction &checkResamplingFunc, const vpResamplingFunction &resamplingFunc,
474  const vpFilterFunction &filterFunc, const vpStateAddFunction &addFunc)
475 {
476  if (x0.size() != m_noiseGenerators[0].size()) {
477  throw(vpException(vpException::dimensionError, "X0 does not have the same size than the vector of stdevs used to build the object"));
478  }
479  m_f = f;
480  m_stateFilterFunc = filterFunc;
481  m_likelihood = l;
482  m_checkIfResample = checkResamplingFunc;
483  m_resampling = resamplingFunc;
484  m_stateAdd = addFunc;
485  m_useProcessFunction = true;
486  m_useCommandStateFunction = false;
487 
488  // Initialize the different particles
489  initParticles(x0);
490 }
491 
492 template <typename MeasurementsType>
494  const vpLikelihoodFunction &l,
495  const vpResamplingConditionFunction &checkResamplingFunc, const vpResamplingFunction &resamplingFunc,
496  const vpFilterFunction &filterFunc, const vpStateAddFunction &addFunc)
497 {
498  if (x0.size() != m_noiseGenerators[0].size()) {
499  throw(vpException(vpException::dimensionError, "X0 does not have the same size than the vector of stdevs used to build the object"));
500  }
501  m_bx = bx;
502  m_stateFilterFunc = filterFunc;
503  m_likelihood = l;
504  m_checkIfResample = checkResamplingFunc;
505  m_resampling = resamplingFunc;
506  m_stateAdd = addFunc;
507  m_useProcessFunction = false;
508  m_useCommandStateFunction = true;
509 
510  // Initialize the different particles
511  initParticles(x0);
512 }
513 
514 template <typename MeasurementsType>
515 void vpParticleFilter<MeasurementsType>::filter(const MeasurementsType &z, const double &dt, const vpColVector &u)
516 {
517  predict(dt, u);
518  update(z);
519 }
520 
521 template <typename MeasurementsType>
523 {
524  if (m_nbMaxThreads == 1) {
525  predictMonothread(dt, u);
526  }
527 #ifdef VISP_HAVE_OPENMP
528  else {
529  predictMultithread(dt, u);
530  }
531 #endif
532 }
533 
534 template <typename MeasurementsType>
535 void vpParticleFilter<MeasurementsType>::update(const MeasurementsType &z)
536 {
537  if (m_nbMaxThreads == 1) {
538  updateMonothread(z);
539  }
540 #ifdef VISP_HAVE_OPENMP
541  else {
542  updateMultithread(z);
543  }
544 #endif
545  bool shouldResample = m_checkIfResample(m_N, m_w);
546  if (shouldResample) {
547  vpParticlesWithWeights particles_weights = m_resampling(m_particles, m_w);
548  m_particles = std::move(particles_weights.m_particles);
549  m_w = std::move(particles_weights.m_weights);
550  }
551 }
552 
553 template <typename MeasurementsType>
555 {
556  return m_stateFilterFunc(m_particles, m_w, m_stateAdd);
557 }
558 
559 template <typename MeasurementsType>
560 vpColVector vpParticleFilter<MeasurementsType>::weightedMean(const std::vector<vpColVector> &particles, const std::vector<double> &weights, const vpStateAddFunction &addFunc)
561 {
562  size_t nbParticles = particles.size();
563  if (nbParticles == 0) {
564  throw(vpException(vpException::dimensionError, "No particles to add when computing the mean"));
565  }
566  vpColVector res = particles[0] * weights[0];
567  for (size_t i = 1; i < nbParticles; ++i) {
568  res = addFunc(res, particles[i] * weights[i]);
569  }
570  return res;
571 }
572 
573 template <typename MeasurementsType>
574 bool vpParticleFilter<MeasurementsType>::simpleResamplingCheck(const unsigned int &N, const std::vector<double> &weights)
575 {
576  double sumSquare = 0.;
577  for (unsigned int i = 0; i < N; ++i) {
578  sumSquare += weights[i] * weights[i];
579  }
580  if (sumSquare < std::numeric_limits<double>::epsilon()) {
581  // All the particles diverged
582  return true;
583  }
584  double N_eff = 1.0 / sumSquare;
585  return (N_eff < (N / 2.0));
586 }
587 
588 template <typename MeasurementsType>
589 typename vpParticleFilter<MeasurementsType>::vpParticlesWithWeights vpParticleFilter<MeasurementsType>::simpleImportanceResampling(const std::vector<vpColVector> &particles, const std::vector<double> &weights)
590 {
591  unsigned int nbParticles = static_cast<unsigned int>(particles.size());
592  double x = 0.;
593  double sumWeights = 0.;
594  std::vector<int> idx(nbParticles);
595 
596  // Draw indices of the randomly chosen particles from the vector of particles
597  for (unsigned int i = 0; i < nbParticles; ++i) {
598  x = sampler();
599  sumWeights = 0.0;
600  int index = samplerRandomIdx.uniform(0, nbParticles); // In case all the weights are null
601  for (unsigned int j = 0; j < nbParticles; ++j) {
602  if (x < sumWeights + weights[j]) {
603  index = j;
604  break;
605  }
606  sumWeights += weights[j];
607  }
608  idx[i] = index;
609  }
610 
611  // Draw the randomly chosen particles corresponding to the indices
612  vpParticlesWithWeights newParticlesWeights;
613  newParticlesWeights.m_particles.resize(nbParticles);
614  for (unsigned int i = 0; i < nbParticles; ++i) {
615  newParticlesWeights.m_particles[i] = particles[idx[i]];
616  }
617 
618  // Reinitialize the weights
619  newParticlesWeights.m_weights.resize(nbParticles, 1.0/ static_cast<double>(nbParticles));
620  return newParticlesWeights;
621 }
622 
623 template <typename MeasurementsType>
625 {
626  unsigned int sizeState = x0.size();
627  unsigned int chunkSize = m_N / m_nbMaxThreads;
628  double uniformWeight = 1. / static_cast<double>(m_N);
629  for (unsigned int i = 0; i < m_nbMaxThreads; ++i) {
630  unsigned int idStart = chunkSize * i;
631  unsigned int idStop = chunkSize * (i + 1);
632  // Last chunk must go until the end
633  if (i == m_nbMaxThreads - 1) {
634  idStop = m_N;
635  }
636  for (unsigned int id = idStart; id < idStop; ++id) {
637  // Generating noise
638  vpColVector noise(sizeState);
639  for (unsigned int idState = 0; idState < sizeState; ++idState) {
640  noise[idState] = m_noiseGenerators[i][idState]();
641  }
642  // Adding noise to the initial state
643  m_particles[id] = m_stateAdd(x0, noise);
644 
645  // (Re)initializing its weight
646  m_w[id] = uniformWeight;
647  }
648  }
649 }
650 
651 #ifdef VISP_HAVE_OPENMP
652 template <typename MeasurementsType>
654 {
655  int iam, nt, ipoints, istart, npoints(m_N);
656  unsigned int sizeState = m_particles[0].size();
657 #pragma omp parallel default(shared) private(iam, nt, ipoints, istart)
658  {
659  iam = omp_get_thread_num();
660  nt = omp_get_num_threads();
661  ipoints = npoints / nt;
662  // size of partition
663  istart = iam * ipoints; // starting array index
664  if (iam == nt-1) {
665  // last thread may do more
666  ipoints = npoints - istart;
667  }
668 
669  for (int i = istart; i< istart + ipoints; ++i) {
670  // Updating the particles following the process (or command) function
671  if (m_useCommandStateFunction) {
672  m_particles[i] = m_bx(u, m_particles[i], dt);
673  }
674  else if (m_useProcessFunction) {
675  m_particles[i] = m_f(m_particles[i], dt);
676  }
677 
678  // Generating noise to add to the particle
679  vpColVector noise(sizeState);
680  for (unsigned int j = 0; j < sizeState; ++j) {
681  noise[j] = m_noiseGenerators[iam][j]();
682  }
683 
684  // Adding the noise to the particle
685  m_particles[i] = m_stateAdd(m_particles[i], noise);
686  }
687  }
688 }
689 
690 template <typename MeasurementsType>
691 double threadLikelihood(const typename vpParticleFilter<MeasurementsType>::vpLikelihoodFunction &likelihood, const std::vector<vpColVector> &v_particles,
692  const MeasurementsType &z, std::vector<double> &w, const int &istart, const int &ipoints)
693 {
694  double sum(0.0);
695  for (int i = istart; i< istart + ipoints; ++i) {
696  w[i] = w[i] * likelihood(v_particles[i], z);
697  sum += w[i];
698  }
699  return sum;
700 }
701 
702 template <typename MeasurementsType>
703 void vpParticleFilter<MeasurementsType>::updateMultithread(const MeasurementsType &z)
704 {
705  double sumWeights = 0.0;
706  int iam, nt, ipoints, istart, npoints(m_N);
707  vpColVector tempSums(m_nbMaxThreads, 0.0);
708  // Compute the weights depending on the likelihood of a particle with regard to the measurements
709 #pragma omp parallel default(shared) private(iam, nt, ipoints, istart)
710  {
711  iam = omp_get_thread_num();
712  nt = omp_get_num_threads();
713  ipoints = npoints / nt;
714  // size of partition
715  istart = iam * ipoints; // starting array index
716  if (iam == nt-1) {
717  // last thread may do more
718  ipoints = npoints - istart;
719  }
720  tempSums[iam] = threadLikelihood<MeasurementsType>(m_likelihood, m_particles, z, m_w, istart, ipoints);
721  }
722  sumWeights = tempSums.sum();
723 
724  if (sumWeights > std::numeric_limits<double>::epsilon()) {
725 #pragma omp parallel default(shared) private(iam, nt, ipoints, istart)
726  {
727  iam = omp_get_thread_num();
728  nt = omp_get_num_threads();
729  ipoints = npoints / nt;
730  // size of partition
731  istart = iam * ipoints; // starting array index
732  if (iam == nt-1) {
733  // last thread may do more
734  ipoints = npoints - istart;
735  }
736 
737  // Normalize the weights
738  for (int i = istart; i < istart + ipoints; ++i) {
739  m_w[i] = m_w[i] / sumWeights;
740  }
741  }
742  }
743 }
744 #endif
745 
746 template <typename MeasurementsType>
748 {
749  unsigned int sizeState = m_particles[0].size();
750  for (unsigned int i = 0; i < m_N; ++i) {
751  // Updating the particle following the process (or command) function
752  if (m_useCommandStateFunction) {
753  m_particles[i] = m_bx(u, m_particles[i], dt);
754  }
755  else if (m_useProcessFunction) {
756  m_particles[i] = m_f(m_particles[i], dt);
757  }
758  else {
759  throw(vpException(vpException::notInitialized, "vpParticleFilter has not been initialized before calling predict"));
760  }
761 
762  // Generating noise to add to the particle
763  vpColVector noise(sizeState);
764  for (unsigned int j = 0; j < sizeState; ++j) {
765  noise[j] = m_noiseGenerators[0][j]();
766  }
767 
768  // Adding the noise to the particle
769  m_particles[i] = m_stateAdd(m_particles[i], noise);
770  }
771 }
772 
773 template <typename MeasurementsType>
774 void vpParticleFilter<MeasurementsType>::updateMonothread(const MeasurementsType &z)
775 {
776  double sumWeights = 0.;
777  // Compute the weights depending on the likelihood of a particle with regard to the measurements
778  for (unsigned int i = 0; i < m_N; ++i) {
779  m_w[i] = m_w[i] * m_likelihood(m_particles[i], z);
780  sumWeights += m_w[i];
781  }
782 
783  // Normalize the weights
784  if (sumWeights > std::numeric_limits<double>::epsilon()) {
785  for (unsigned int i = 0; i < m_N; ++i) {
786  m_w[i] = m_w[i] / sumWeights;
787  }
788  }
789 }
790 END_VISP_NAMESPACE
791 #endif
792 #endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:74
@ dimensionError
Bad dimension.
Definition: vpException.h:71
Class for generating random number with normal probability density.
Definition: vpGaussRand.h:117
The class permits to use a Particle Filter.
std::function< vpParticlesWithWeights(const std::vector< vpColVector > &, const std::vector< double > &)> vpResamplingFunction
Function that takes as argument the vector of particles and the vector of associated weights....
virtual ~vpParticleFilter()
void setResamplingFunction(const vpResamplingFunction &resamplingFunc)
Set the resampling function that generate new particles and associated weights when the filter starts...
void setLikelihoodFunction(const vpLikelihoodFunction &likelihood)
Set the likelihood function that updates the weights of the particles based on the new measurements.
std::function< vpColVector(const std::vector< vpColVector > &, const std::vector< double > &, const vpStateAddFunction &)> vpFilterFunction
Filter function, which computes the filtered state of the particle filter. The first argument is the ...
void filter(const MeasurementsType &z, const double &dt, const vpColVector &u=vpColVector())
Perform first the prediction step and then the update step. If needed, resampling will also be perfor...
static bool simpleResamplingCheck(const unsigned int &N, const std::vector< double > &weights)
Returns true if the following condition is fulfilled, or if all the particles diverged: .
static vpColVector weightedMean(const std::vector< vpColVector > &particles, const std::vector< double > &weights, const vpStateAddFunction &addFunc)
Simple function to compute a weighted mean, which just does .
std::function< vpColVector(const vpColVector &, const double &)> vpProcessFunction
Process model function, which projects a particle forward in time. The first argument is a particle,...
struct vpParticleFilter::vpParticlesWithWeights vpParticlesWithWeights
Structure of vectors for which the i^th element of the weights vector is associated to the i^th eleme...
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)
Set the guess of the initial state.
std::function< vpColVector(const vpColVector &, const vpColVector &, const double &)> vpCommandStateFunction
Command model function, which projects a particle forward in time according to the command and its pr...
static vpParticlesWithWeights simpleImportanceResampling(const std::vector< vpColVector > &particles, const std::vector< double > &weights)
Function implementing the resampling of a Simple Importance Resampling Particle Filter.
std::function< bool(const unsigned int &, const std::vector< double > &)> vpResamplingConditionFunction
Function that takes as argument the number of particles and the vector of weights associated to each ...
static vpColVector simpleAdd(const vpColVector &a, const vpColVector &toAdd)
Simple function to compute an addition, which just does .
void setCheckResamplingFunction(const vpResamplingConditionFunction &resamplingCondFunc)
Set the function that returns true when the filter starts to degenerate and false otherwise.
void setFilterFunction(const vpFilterFunction &filterFunc)
Set the filter function that compute the filtered state from the particles and their associated weigh...
VP_EXPLICIT vpParticleFilter(const unsigned int &N, const std::vector< double > &stdev, const long &seed=-1, const int &nbThreads=-1)
Construct a new vpParticleFilter object.
std::function< double(const vpColVector &, const MeasurementsType &)> vpLikelihoodFunction
Likelihood function, which evaluates the likelihood of a particle with regard to the measurements....
void predict(const double &dt, const vpColVector &u=vpColVector())
Predict the new state based on the last state and how far in time we want to predict.
void setCommandStateFunction(const vpCommandStateFunction &bx)
Set the command function to use when projecting the particles in the future.
void setProcessFunction(const vpProcessFunction &f)
Set the process function to use when projecting the particles in the future.
void update(const MeasurementsType &z)
Update the weights of the particles based on a new measurement. The weights will be normalized (i....
std::function< vpColVector(const vpColVector &, const vpColVector &)> vpStateAddFunction
Function that computes either the equivalent of an addition in the state space. The first argument is...
vpColVector computeFilteredState()
Compute the filtered state from the particles and their associated weights.
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:127
int uniform(int a, int b)
Definition: vpUniRand.cpp:159
void setSeed(uint64_t initstate, uint64_t initseq)
Definition: vpUniRand.cpp:202
VISP_EXPORT double measureTimeMicros()
Structure of vectors for which the i^th element of the weights vector is associated to the i^th eleme...
std::vector< vpColVector > m_particles