particlefilter.cpp
Go to the documentation of this file.
1 std::vector<unsigned int> sistematicResampler<State,Numeric>::resample(const vector<Particle>& particles) const{
2  Numeric cweight=0;
3 
4  //compute the cumulative weights
5  unsigned int n=0;
6  for (vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
7  cweight+=it->weight;
8  n++;
9  }
10 
11  //compute the interval
12  Numeric interval=cweight/n;
13 
14  //compute the initial target weight
15  Numeric target=
16  //compute the resampled indexes
17 
18  cweight=0;
19  std::vector<int> indexes(n);
20  n=0;
21  unsigned int i=0;
22  for (vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
23  cweight+=it->weight;
24  while(cweight>target){
25  indexes[n++]=i;
26  target+=interval;
27  }
28  }
29  return indexes;
30  }
31 
32 template <class Numeric>
33 std::vector<unsigned int> indexResampler<Numeric>::resample(const vector<Numeric> >& weights) const{
34  Numeric cweight=0;
35 
36  //compute the cumulative weights
37  unsigned int n=0;
38  for (vector<Numeric>::const_iterator it=weights.begin(); it!=weights.end(); ++it){
39  cweight+=*it;
40  n++;
41  }
42 
43  //compute the interval
44  Numeric interval=cweight/n;
45 
46  //compute the initial target weight
47  Numeric target=
48  //compute the resampled indexes
49 
50  cweight=0;
51  std::vector<int> indexes(n);
52  n=0;
53  unsigned int i=0;
54  for (vector<Numeric>::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){
55  cweight+=it->weight;
56  while(cweight>target){
57  indexes[n++]=i;
58  target+=interval;
59  }
60  }
61  return indexes;
62 }
63 
64 /*
65 
66 The following are patterns for the evolution and the observation classes
67 The user should implement classes having the specified meaning
68 
69 template <class State, class Numeric, class Observation>
70 struct observer{
71  Observation& observation
72  Numeric observe(const class State&) const;
73 };
74 
75 template <class State, class Numeric, class Input>
76 struct evolver{
77  Input& input;
78  State& evolve(const State& s);
79 };
80 */
81 
82 template <class State, class Numeric, class EvolutionModel>
83 void evolver<State, Numeric, EvolutionModel>::evolve(std::vector<evolver::Particle>& particles) const{
84  for (std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it)
85  *it=evolutionModel.evolve(*it);
86 }
87 
88 void evolver<State, Numeric, EvolutionModel>::evolve(std::vector<evolver::Particle>& dest, const std::vector<evolver::Particle>& src) const{
89  dest.clear();
90  for (std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it)
91  dest.push_back(evolutionModel.evolve(*it));
92 }
93 
94 template <class State, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
95 struct auxiliaryEvolver{
96  typedef particle<State, Numeric> Particle;
97 
98  EvolutionModel evolutionModel;
99  QualificationModel qualificationModel;
100  LikelyhoodModel likelyhoodModel;
101  indexResampler<Numeric> resampler;
102 
103 void auxiliaryEvolver<State, Numeric, QualificationModel, EvolutionModel, LikelyhoodModel>::evolve
104  (std::vector<auxiliaryEvolver::Particle>&particles){
105  std::vector<Numeric> observationWeights(particles.size());
106  unsigned int i=0;
107  for (std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){
108  observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
109  }
110  std::vector<unsigned int> indexes(indexResampler.resample(observationWeights));
111  for (std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); it++){
112  Particle & particle=particles[*it];
113  particle=evolutionModel.evolve(particle);
114  particle.weight*=lykelyhoodModel.lykelyhood(particle)/observationWeights[*it];
115  }
116 }
117 
118 void auxiliaryEvolver<State, Numeric, QualificationModel, EvolutionModel, LikelyhoodModel>::evolve
119  (std::vector<Particle>& dest, const std::vector<Particle>& src){
120  dest.clear();
121  std::vector<Numeric> observationWeights(particles.size());
122  unsigned int i=0;
123  for (std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it, i++){
124  observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
125  }
126  std::vector<unsigned int> indexes(indexResampler.resample(observationWeights));
127  for (std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); it++){
128  Particle & particle=src[*it];
129  dest.push_back(evolutionModel.evolve(particle));
130  dest.back().weight*=likelyhoodModel.lykelyhood(particle)/observationWeights[*it];
131  }
132  return dest();
133 }
LikelyhoodModel::likelyhood
double likelyhood(const Particle &p) const
Definition: particlefilter_test.cpp:41
n
#define n
Definition: eig3.cpp:11
QualificationModel::evolve
Particle evolve(const Particle &p)
Definition: particlefilter_test.cpp:35
EvolutionModel
Definition: particlefilter_test.cpp:26
Particle
Definition: particlefilter_test.cpp:11
LikelyhoodModel
Definition: particlefilter_test.cpp:40
evolver::evolve
void evolve(std::vector< Particle > &particles)
Definition: particlefilter.h:270
resample
void resample(std::vector< int > &indexes, const WeightVector &weights, unsigned int nparticles=0)
Definition: particlefilter.h:51
QualificationModel
Definition: particlefilter_test.cpp:34
EvolutionModel::evolve
Particle evolve(const Particle &p)
Definition: particlefilter_test.cpp:27


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Thu Oct 19 2023 02:25:51