34 #ifndef VP_PARTICLE_FILTER_H
35 #define VP_PARTICLE_FILTER_H
37 #include <visp3/core/vpConfig.h>
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>
46 #ifdef VISP_HAVE_OPENMP
57 template <
typename MeasurementsType>
135 VP_EXPLICIT
vpParticleFilter(
const unsigned int &N,
const std::vector<double> &stdev,
const long &seed = -1,
const int &nbThreads = -1);
215 void update(
const MeasurementsType &z);
234 m_useCommandStateFunction =
false;
235 m_useProcessFunction =
true;
248 m_useCommandStateFunction =
true;
249 m_useProcessFunction =
false;
260 m_likelihood = likelihood;
271 m_stateFilterFunc = filterFunc;
282 m_checkIfResample = resamplingCondFunc;
293 m_resampling = resamplingFunc;
343 #ifdef VISP_HAVE_OPENMP
344 void predictMultithread(
const double &dt,
const vpColVector &u);
345 void updateMultithread(
const MeasurementsType &z);
348 void predictMonothread(
const double &dt,
const vpColVector &u);
349 void updateMonothread(
const MeasurementsType &z);
355 unsigned int m_nbMaxThreads;
356 std::vector<std::vector<vpGaussRand>> m_noiseGenerators;
357 std::vector<vpColVector> m_particles;
358 std::vector<double> m_w;
370 bool m_useProcessFunction;
371 bool m_useCommandStateFunction;
374 template <
typename MeasurementsType>
377 template <
typename MeasurementsType>
380 template <
typename MeasurementsType>
384 , m_w(N, 1./static_cast<double>(N))
385 , m_useProcessFunction(false)
386 , m_useCommandStateFunction(false)
388 #ifndef VISP_HAVE_OPENMP
391 std::cout <<
"[vpParticleFilter::vpParticleFilter] WARNING: OpenMP is not available, maximum number of threads to use clamped to 1" << std::endl;
394 int maxThreads = omp_get_max_threads();
395 if (nbThreads <= 0) {
396 m_nbMaxThreads = maxThreads;
398 else if (nbThreads > maxThreads) {
399 m_nbMaxThreads = maxThreads;
400 std::cout <<
"[vpParticleFilter::vpParticleFilter] WARNING: maximum number of threads to use clamped to "
401 << maxThreads <<
" instead of " << nbThreads <<
" due to OpenMP restrictions." << std::endl;
402 std::cout <<
"[vpParticleFilter::vpParticleFilter] If you want more, consider to use omp_set_num_threads before." << std::endl;
405 m_nbMaxThreads = nbThreads;
409 unsigned int sizeState =
static_cast<unsigned int>(stdev.size());
410 m_noiseGenerators.resize(m_nbMaxThreads);
411 unsigned long long seedForGenerator;
413 seedForGenerator = seed;
420 sampler.
setSeed(seed, 0x123465789ULL);
421 samplerRandomIdx.
setSeed(seed + 4224, 0x123465789ULL);
423 vpUniRand seedGenerator(seedForGenerator);
424 for (
unsigned int threadId = 0; threadId < m_nbMaxThreads; ++threadId) {
425 for (
unsigned int stateId = 0; stateId < sizeState; ++stateId) {
426 m_noiseGenerators[threadId].push_back(
vpGaussRand(stdev[stateId], 0., seedGenerator.
uniform(0., 1e9)));
431 template <
typename MeasurementsType>
438 m_stateFilterFunc = filterFunc;
440 m_checkIfResample = checkResamplingFunc;
441 m_resampling = resamplingFunc;
442 m_stateAdd = addFunc;
443 m_useProcessFunction =
true;
444 m_useCommandStateFunction =
false;
450 template <
typename MeasurementsType>
457 m_stateFilterFunc = filterFunc;
459 m_checkIfResample = checkResamplingFunc;
460 m_resampling = resamplingFunc;
461 m_stateAdd = addFunc;
462 m_useProcessFunction =
false;
463 m_useCommandStateFunction =
true;
469 template <
typename MeasurementsType>
476 template <
typename MeasurementsType>
479 if (m_nbMaxThreads == 1) {
480 predictMonothread(dt, u);
482 #ifdef VISP_HAVE_OPENMP
484 predictMultithread(dt, u);
489 template <
typename MeasurementsType>
492 if (m_nbMaxThreads == 1) {
495 #ifdef VISP_HAVE_OPENMP
497 updateMultithread(z);
500 bool shouldResample = m_checkIfResample(m_N, m_w);
501 if (shouldResample) {
503 m_particles = std::move(particles_weights.
m_particles);
504 m_w = std::move(particles_weights.
m_weights);
508 template <
typename MeasurementsType>
511 return m_stateFilterFunc(m_particles, m_w, m_stateAdd);
514 template <
typename MeasurementsType>
517 size_t nbParticles = particles.
size();
518 if (nbParticles == 0) {
522 for (
size_t i = 1; i < nbParticles; ++i) {
523 res = addFunc(res, particles[i] * weights[i]);
528 template <
typename MeasurementsType>
531 double sumSquare = 0.;
532 for (
unsigned int i = 0; i < N; ++i) {
533 sumSquare += weights[i] * weights[i];
535 if (sumSquare < std::numeric_limits<double>::epsilon()) {
539 double N_eff = 1.0 / sumSquare;
540 return (N_eff < (N / 2.0));
543 template <
typename MeasurementsType>
546 unsigned int nbParticles =
static_cast<unsigned int>(particles.size());
548 double sumWeights = 0.;
549 std::vector<int> idx(nbParticles);
552 for (
unsigned int i = 0; i < nbParticles; ++i) {
555 int index = samplerRandomIdx.uniform(0, nbParticles);
556 for (
unsigned int j = 0; j < nbParticles; ++j) {
557 if (x < sumWeights + weights[j]) {
561 sumWeights += weights[j];
568 newParticlesWeights.
m_particles.resize(nbParticles);
569 for (
unsigned int i = 0; i < nbParticles; ++i) {
570 newParticlesWeights.
m_particles[i] = particles[idx[i]];
574 newParticlesWeights.
m_weights.resize(nbParticles, 1.0/
static_cast<double>(nbParticles));
575 return newParticlesWeights;
578 template <
typename MeasurementsType>
581 unsigned int sizeState = x0.
size();
582 unsigned int chunkSize = m_N / m_nbMaxThreads;
583 double uniformWeight = 1. /
static_cast<double>(m_N);
584 for (
unsigned int i = 0; i < m_nbMaxThreads; ++i) {
585 unsigned int idStart = chunkSize * i;
586 unsigned int idStop = chunkSize * (i + 1);
588 if (i == m_nbMaxThreads - 1) {
591 for (
unsigned int id = idStart;
id < idStop; ++id) {
594 for (
unsigned int idState = 0; idState < sizeState; ++idState) {
595 noise[idState] = m_noiseGenerators[i][idState]();
598 m_particles[id] = m_stateAdd(x0, noise);
601 m_w[id] = uniformWeight;
606 #ifdef VISP_HAVE_OPENMP
607 template <
typename MeasurementsType>
610 int iam, nt, ipoints, istart, npoints(m_N);
611 unsigned int sizeState = m_particles[0].size();
612 #pragma omp parallel default(shared) private(iam, nt, ipoints, istart)
614 iam = omp_get_thread_num();
615 nt = omp_get_num_threads();
616 ipoints = npoints / nt;
618 istart = iam * ipoints;
621 ipoints = npoints - istart;
624 for (
int i = istart; i< istart + ipoints; ++i) {
626 if (m_useCommandStateFunction) {
627 m_particles[i] = m_bx(u, m_particles[i], dt);
629 else if (m_useProcessFunction) {
630 m_particles[i] = m_f(m_particles[i], dt);
635 for (
unsigned int j = 0; j < sizeState; ++j) {
636 noise[j] = m_noiseGenerators[iam][j]();
640 m_particles[i] = m_stateAdd(m_particles[i], noise);
645 template <
typename MeasurementsType>
647 const vpColVector &z, std::vector<double> &w,
const int &istart,
const int &ipoints)
650 for (
int i = istart; i< istart + ipoints; ++i) {
651 w[i] = w[i] * likelihood(v_particles[i], z);
657 template <
typename MeasurementsType>
660 double sumWeights = 0.0;
661 int iam, nt, ipoints, istart, npoints(m_N);
664 #pragma omp parallel default(shared) private(iam, nt, ipoints, istart)
666 iam = omp_get_thread_num();
667 nt = omp_get_num_threads();
668 ipoints = npoints / nt;
670 istart = iam * ipoints;
673 ipoints = npoints - istart;
675 tempSums[iam] = threadLikelihood<MeasurementsType>(m_likelihood, m_particles, z, m_w, istart, ipoints);
677 sumWeights = tempSums.sum();
679 if (sumWeights > std::numeric_limits<double>::epsilon()) {
680 #pragma omp parallel default(shared) private(iam, nt, ipoints, istart)
682 iam = omp_get_thread_num();
683 nt = omp_get_num_threads();
684 ipoints = npoints / nt;
686 istart = iam * ipoints;
689 ipoints = npoints - istart;
693 for (
int i = istart; i < istart + ipoints; ++i) {
694 m_w[i] = m_w[i] / sumWeights;
701 template <
typename MeasurementsType>
704 unsigned int sizeState = m_particles[0].size();
705 for (
unsigned int i = 0; i < m_N; ++i) {
707 if (m_useCommandStateFunction) {
708 m_particles[i] = m_bx(u, m_particles[i], dt);
710 else if (m_useProcessFunction) {
711 m_particles[i] = m_f(m_particles[i], dt);
719 for (
unsigned int j = 0; j < sizeState; ++j) {
720 noise[j] = m_noiseGenerators[0][j]();
724 m_particles[i] = m_stateAdd(m_particles[i], noise);
728 template <
typename MeasurementsType>
731 double sumWeights = 0.;
733 for (
unsigned int i = 0; i < m_N; ++i) {
734 m_w[i] = m_w[i] * m_likelihood(m_particles[i], z);
735 sumWeights += m_w[i];
739 if (sumWeights > std::numeric_limits<double>::epsilon()) {
740 for (
unsigned int i = 0; i < m_N; ++i) {
741 m_w[i] = m_w[i] / sumWeights;
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ notInitialized
Used to indicate that a parameter is not initialized.
@ dimensionError
Bad dimension.
Class for generating random number with normal probability density.
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.
int uniform(int a, int b)
void setSeed(uint64_t initstate, uint64_t initseq)
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< double > m_weights
std::vector< vpColVector > m_particles