00001 #ifndef PARTICLEFILTER_H
00002 #define PARTICLEFILTER_H
00003 #include <stdlib.h>
00004 #include <sys/types.h>
00005 #include <vector>
00006 #include <utility>
00007 #include <cmath>
00008 #include <limits>
00009 #include <gmapping/utils/gvalues.h>
00010
00011
00022 typedef std::pair<uint,uint> UIntPair;
00023
00024 template <class OutputIterator, class Iterator>
00025 double toNormalForm(OutputIterator& out, const Iterator & begin, const Iterator & end){
00026
00027 double lmax = -std::numeric_limits<double>::max();
00028 for (Iterator it=begin; it!=end; it++){
00029 lmax=lmax>((double)(*it))? lmax: (double)(*it);
00030 }
00031
00032 for (Iterator it=begin; it!=end; it++){
00033 *out=exp((double)(*it)-lmax);
00034 out++;
00035 }
00036 return lmax;
00037 }
00038
00039 template <class OutputIterator, class Iterator, class Numeric>
00040 void toLogForm(OutputIterator& out, const Iterator & begin, const Iterator & end, Numeric lmax){
00041
00042 for (Iterator it=begin; it!=end; it++){
00043 *out=log((Numeric)(*it))-lmax;
00044 out++;
00045 }
00046 return lmax;
00047 }
00048
00049 template <class WeightVector>
00050 void resample(std::vector<int>& indexes, const WeightVector& weights, unsigned int nparticles=0){
00051 double cweight=0;
00052
00053
00054 unsigned int n=0;
00055 for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it){
00056 cweight+=(double)*it;
00057 n++;
00058 }
00059
00060 if (nparticles>0)
00061 n=nparticles;
00062
00063
00064 double interval=cweight/n;
00065
00066
00067 double target=interval*::drand48();
00068
00069
00070 cweight=0;
00071 indexes.resize(n);
00072
00073 n=0;
00074 unsigned int i=0;
00075 for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){
00076 cweight+=(double)* it;
00077 while(cweight>target){
00078 indexes[n++]=i;
00079 target+=interval;
00080 }
00081 }
00082 }
00083
00084 template <typename Vector>
00085 void repeatIndexes(Vector& dest, const std::vector<int>& indexes, const Vector& particles){
00086 assert(indexes.size()==particles.size());
00087 dest.resize(particles.size());
00088 unsigned int i=0;
00089 for (std::vector<int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
00090 dest[i]=particles[*it];
00091 i++;
00092 }
00093 }
00094
00095
00096 template <class Iterator>
00097 double neff(const Iterator& begin, const Iterator& end){
00098 double sum=0;
00099 for (Iterator it=begin; it!=end; ++it){
00100 sum+=*it;
00101 }
00102 double cum=0;
00103 for (Iterator it=begin; it!=end; ++it){
00104 double w=*it/sum;
00105 cum+=w*w;
00106 }
00107 return 1./cum;
00108 }
00109
00110 template <class Iterator>
00111 void normalize(const Iterator& begin, const Iterator& end){
00112 double sum=0;
00113 for (Iterator it=begin; it!=end; ++it){
00114 sum+=*it;
00115 }
00116 for (Iterator it=begin; it!=end; ++it){
00117 *it=*it/sum;
00118 }
00119 }
00120
00121 template <class OutputIterator, class Iterator>
00122 void rle(OutputIterator& out, const Iterator & begin, const Iterator & end){
00123 unsigned int current=0;
00124 unsigned int count=0;
00125 for (Iterator it=begin; it!=end; it++){
00126 if (it==begin){
00127 current=*it;
00128 count=1;
00129 continue;
00130 }
00131 if (((uint)*it) ==current)
00132 count++;
00133 if (((uint)*it)!=current){
00134 *out=std::make_pair(current,count);
00135 out++;
00136 current=*it;
00137 count=1;
00138 }
00139 }
00140 if (count>0)
00141 *out=std::make_pair(current,count);
00142 out++;
00143 }
00144
00145
00146 template <class Particle, class Numeric>
00147 struct uniform_resampler{
00148 std::vector<unsigned int> resampleIndexes(const std::vector<Particle> & particles, int nparticles=0) const;
00149 std::vector<Particle> resample(const std::vector<Particle> & particles, int nparticles=0) const;
00150 Numeric neff(const std::vector<Particle> & particles) const;
00151 };
00152
00153
00154 template <class Particle, class Numeric>
00155 std::vector<unsigned int> uniform_resampler<Particle, Numeric>:: resampleIndexes(const std::vector<Particle>& particles, int nparticles) const{
00156 Numeric cweight=0;
00157
00158
00159 unsigned int n=0;
00160 for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
00161 cweight+=(Numeric)*it;
00162 n++;
00163 }
00164
00165 if (nparticles>0)
00166 n=nparticles;
00167
00168
00169 Numeric interval=cweight/n;
00170
00171
00172 Numeric target=interval*::drand48();
00173
00174
00175 cweight=0;
00176 std::vector<unsigned int> indexes(n);
00177 n=0;
00178 unsigned int i=0;
00179 for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
00180 cweight+=(Numeric)* it;
00181 while(cweight>target){
00182 indexes[n++]=i;
00183 target+=interval;
00184 }
00185 }
00186 return indexes;
00187 }
00188
00189 template <class Particle, class Numeric>
00190 std::vector<Particle> uniform_resampler<Particle,Numeric>::resample
00191 (const typename std::vector<Particle>& particles, int nparticles) const{
00192 Numeric cweight=0;
00193
00194
00195 unsigned int n=0;
00196 for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
00197 cweight+=(Numeric)*it;
00198 n++;
00199 }
00200
00201 if (nparticles>0)
00202 n=nparticles;
00203
00204
00205 double uw=1./n;
00206
00207
00208 Numeric interval=cweight/n;
00209
00210
00211 Numeric target=interval*::drand48();
00212
00213
00214 cweight=0;
00215 std::vector<Particle> resampled;
00216 n=0;
00217 unsigned int i=0;
00218 for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
00219 cweight+=(Numeric)*it;
00220 while(cweight>target){
00221 resampled.push_back(*it);
00222 resampled.back().setWeight(uw);
00223 target+=interval;
00224 }
00225 }
00226 return resampled;
00227 }
00228
00229 template <class Particle, class Numeric>
00230 Numeric uniform_resampler<Particle,Numeric>::neff(const std::vector<Particle> & particles) const{
00231 double cum=0;
00232 double sum=0;
00233 for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
00234 Numeric w=(Numeric)*it;
00235 cum+=w*w;
00236 sum+=w;
00237 }
00238 return sum*sum/cum;
00239 }
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261 template <class Particle, class EvolutionModel>
00262 struct evolver{
00263 EvolutionModel evolutionModel;
00264 void evolve(std::vector<Particle>& particles);
00265 void evolve(std::vector<Particle>& dest, const std::vector<Particle>& src);
00266 };
00267
00268 template <class Particle, class EvolutionModel>
00269 void evolver<Particle, EvolutionModel>::evolve(std::vector<Particle>& particles){
00270 for (typename std::vector<Particle>::iterator it=particles.begin(); it!=particles.end(); ++it){
00271 *it=evolutionModel.evolve(*it);
00272 }
00273 }
00274
00275 template <class Particle, class EvolutionModel>
00276 void evolver<Particle, EvolutionModel>::evolve(std::vector<Particle>& dest, const std::vector<Particle>& src){
00277 dest.clear();
00278 for (typename std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it)
00279 dest.push_back(evolutionModel.evolve(*it));
00280 }
00281
00282
00283 template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
00284 struct auxiliary_evolver{
00285 EvolutionModel evolutionModel;
00286 QualificationModel qualificationModel;
00287 LikelyhoodModel likelyhoodModel;
00288 void evolve(std::vector<Particle>& particles);
00289 void evolve(std::vector<Particle>& dest, const std::vector<Particle>& src);
00290 };
00291
00292 template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
00293 void auxiliary_evolver<Particle, Numeric, QualificationModel, EvolutionModel, LikelyhoodModel>::evolve
00294 (std::vector<Particle>&particles){
00295 std::vector<Numeric> observationWeights(particles.size());
00296 unsigned int i=0;
00297 for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){
00298 observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
00299 }
00300 uniform_resampler<Numeric, Numeric> resampler;
00301 std::vector<unsigned int> indexes(resampler.resampleIndexes(observationWeights));
00302 for (typename std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
00303 Particle & particle=particles[*it];
00304 particle=evolutionModel.evolve(particle);
00305 particle.setWeight(likelyhoodModel.likelyhood(particle)/observationWeights[*it]);
00306 }
00307 }
00308
00309 template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
00310 void auxiliary_evolver<Particle, Numeric, QualificationModel, EvolutionModel, LikelyhoodModel>::evolve
00311 (std::vector<Particle>& dest, const std::vector<Particle>& src){
00312 dest.clear();
00313 std::vector<Numeric> observationWeights(src.size());
00314 unsigned int i=0;
00315 for (typename std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it, i++){
00316 observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
00317 }
00318 uniform_resampler<Numeric, Numeric> resampler;
00319 std::vector<unsigned int> indexes(resampler.resampleIndexes(observationWeights));
00320 for (typename std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
00321 Particle & particle=src[*it];
00322 dest.push_back(evolutionModel.evolve(particle));
00323 dest.back().weight*=likelyhoodModel.likelyhood(particle)/observationWeights[*it];
00324 }
00325 }
00326
00327
00328 #endif