pf.h
Go to the documentation of this file.
1 #ifndef PARTICLEFILTER_H
2 #define PARTICLEFILTER_H
3 #include <stdlib.h>
4 #include<vector>
5 #include<utility>
7 
8 
9 
20 typedef std::pair<uint,uint> UIntPair;
21 
22 template <class OutputIterator, class Iterator>
23 double toNormalForm(OutputIterator& out, const Iterator & begin, const Iterator & end){
24  //determine the maximum
25  double lmax=-MAXDOUBLE;
26  for (Iterator it=begin; it!=end; it++){
27  lmax=lmax>((double)(*it))? lmax: (double)(*it);
28  }
29  //convert to raw form
30  for (Iterator it=begin; it!=end; it++){
31  *out=exp((double)(*it)-lmax);
32  out++;
33  }
34  return lmax;
35 }
36 
37 template <class OutputIterator, class Iterator, class Numeric>
38 void toLogForm(OutputIterator& out, const Iterator & begin, const Iterator & end, Numeric lmax){
39  //determine the maximum
40  for (Iterator it=begin; it!=end; it++){
41  *out=log((Numeric)(*it))-lmax;
42  out++;
43  }
44  return lmax;
45 }
46 
47 template <class WeightVector>
48 void resample(std::vector<int>& indexes, const WeightVector& weights, unsigned int nparticles=0){
49  double cweight=0;
50 
51  //compute the cumulative weights
52  unsigned int n=0;
53  for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it){
54  cweight+=(double)*it;
55  n++;
56  }
57 
58  if (nparticles>0)
59  n=nparticles;
60 
61  //compute the interval
62  double interval=cweight/n;
63 
64  //compute the initial target weight
65  double target=interval*::drand48();
66  //compute the resampled indexes
67 
68  cweight=0;
69  indexes.resize(n);
70 
71  n=0;
72  unsigned int i=0;
73  for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){
74  cweight+=(double)* it;
75  while(cweight>target){
76  indexes[n++]=i;
77  target+=interval;
78  }
79  }
80 }
81 
82 template <typename WeightVector>
83 void normalizeWeights(WeightVector& weights, unsigned int size, double minWeight){
84  double wmin=MAXDOUBLE;
85  double wmax=-MAXDOUBLE;
86  for (uint i=0; i<size; i++){
87  wmin=wmin<weights[i]?wmin:weights[i];
88  wmax=wmax>weights[i]?wmax:weights[i];
89  }
90  double min_normalized_value=log(minWeight);
91  double max_normalized_value=log(1.);
92  double dn=max_normalized_value-min_normalized_value;
93  double dw=wmax-wmin;
94  if (dw==0) dw=1;
95  double scale=dn/dw;
96  double offset=-wmax*scale;
97  for (uint i=0; i<size; i++){
98  double w=weights[i];
99  w=scale*w+offset;
100  weights[i]=exp(w);
101  }
102 }
103 
104 template <typename Vector>
105 void repeatIndexes(Vector& dest, const std::vector<int>& indexes, const Vector& particles){
106 /*<<<<<<< .mine
107  assert(indexes.size()==particles.size());
108  if (dest.size()!=particles.size())
109  dest.resize(particles.size());
110 =======*/
111  //assert(indexes.size()==particles.size()); //DIEGO non ne vedo il senso, anzi è sbagliata
112  //dest.resize(particles.size()); // è sbagliato anche questo
113  dest.resize(indexes.size());
114 // >>>>>>> .r2534
115  unsigned int i=0;
116  for (std::vector<int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
117  dest[i]=particles[*it];
118  i++;
119  }
120 }
121 
122 template <typename Vector>
123 void repeatIndexes(Vector& dest, const std::vector<int>& indexes2, const Vector& particles, const std::vector<int>& indexes){
124  // assert(indexes.size()==indexes2.size());
125  dest=particles;
126  unsigned int i=0;
127  for (std::vector<int>::const_iterator it=indexes2.begin(); it!=indexes2.end(); ++it){
128  dest[indexes[i]]=particles[*it];
129  i++;
130  }
131 }
132 
133 
134 template <class Iterator>
135 double neff(const Iterator& begin, const Iterator& end){
136  double sum=0;
137  for (Iterator it=begin; it!=end; ++it){
138  sum+=*it;
139  }
140  double cum=0;
141  for (Iterator it=begin; it!=end; ++it){
142  double w=*it/sum;
143  cum+=w*w;
144  }
145  return 1./cum;
146 }
147 
148 
149 
150 template <class OutputIterator, class Iterator>
151 void rle(OutputIterator& out, const Iterator & begin, const Iterator & end){
152  unsigned int current=0;
153  unsigned int count=0;
154  for (Iterator it=begin; it!=end; it++){
155  if (it==begin){
156  current=*it;
157  count=1;
158  continue;
159  }
160  if (((uint)*it) ==current)
161  count++;
162  if (((uint)*it)!=current){
163  *out=std::make_pair(current,count);
164  out++;
165  current=*it;
166  count=1;
167  }
168  }
169  if (count>0)
170  *out=std::make_pair(current,count);
171  out++;
172 }
173 
174 #endif
175 
void repeatIndexes(Vector &dest, const std::vector< int > &indexes, const Vector &particles)
Definition: pf.h:105
void normalizeWeights(WeightVector &weights, unsigned int size, double minWeight)
Definition: pf.h:83
void rle(OutputIterator &out, const Iterator &begin, const Iterator &end)
Definition: pf.h:151
std::pair< uint, uint > UIntPair
Definition: pf.h:20
void resample(std::vector< int > &indexes, const WeightVector &weights, unsigned int nparticles=0)
Definition: pf.h:48
double toNormalForm(OutputIterator &out, const Iterator &begin, const Iterator &end)
Definition: pf.h:23
void toLogForm(OutputIterator &out, const Iterator &begin, const Iterator &end, Numeric lmax)
Definition: pf.h:38
double neff(const Iterator &begin, const Iterator &end)
Definition: pf.h:135
#define n
Definition: eig3.cpp:11


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20