Go to the documentation of this file.00001 #include <stdlib.h>
00002 #include <iostream>
00003 #include <fstream>
00004 #include <utils/point.h>
00005 #include "particlefilter.h"
00006
00007 using namespace std;
00008 using namespace GMapping;
00009
00010 #define test(s) {cout << s << " " << flush;}
00011 #define testOk() {cout << "OK" << endl;}
00012
00013 struct Particle{
00014 Particle(): p(0,0), w(0){}
00015 Point p;
00016 double w;
00017 operator double() const {return w; }
00018 void setWeight(double _w) {w=_w;}
00019 };
00020
00021 ostream& printParticles(ostream& os, const vector<Particle>& p)
00022 {
00023 for (vector<Particle>::const_iterator it=p.begin(); it!=p.end(); ++it) {
00024 os << it->p.x << " " << it->p.y << endl;
00025 }
00026 return os;
00027 }
00028
00029 struct EvolutionModel{
00030 Particle evolve(const Particle& p){
00031 Particle pn(p);
00032 pn.p.x+=10*(drand48()-.5);
00033 pn.p.y+=10*(drand48()-.5);
00034 return pn;
00035 }
00036 };
00037
00038
00039 struct LikelyhoodModel{
00040 std::vector<Point> observerVector;
00041 std::vector<double> observations;
00042 double sigma;
00043 double likelyhood(const Particle& p) const{
00044 double v=1;
00045 std::vector<double>::const_iterator oit=observations.begin();
00046 for (std::vector<Point>::const_iterator it=observerVector.begin(); it!=observerVector.end();it++){
00047 v*=exp(-pow(((p.p-*it)*(p.p-*it)-*oit*(*oit))/sigma, 2));
00048 oit++;
00049 }
00050 cout << "#v=" << v << endl;
00051 return v;
00052 }
00053 };
00054
00055 int main (unsigned int argc, const char * const * argv){
00056 vector<Particle> particles(1000);
00057 LikelyhoodModel likelyhoodModel;
00058 uniform_resampler<Particle, double> resampler;
00059 evolver <Particle, EvolutionModel> evolver;
00060
00061 for (vector<Particle>::iterator it=particles.begin(); it!=particles.end(); it++){
00062 it->w=1;
00063 it->p.x=400*(drand48()-.5);
00064 it->p.y=400*(drand48()-.5);
00065 }
00066
00067 vector<Point> sensors;
00068
00069 sensors.push_back(Point(-50,0));
00070 sensors.push_back(Point(50,0));
00071 sensors.push_back(Point(0,100));
00072
00073 likelyhoodModel.sigma=1000;
00074 likelyhoodModel.observations.push_back(70);
00075 likelyhoodModel.observations.push_back(70);
00076 likelyhoodModel.observations.push_back(70);
00077
00078 likelyhoodModel.observerVector=sensors;
00079 while (1){
00080 char buf[2];
00081 cin.getline(buf,2);
00082 vector<Particle> newgeneration;
00083
00084 cout << "# SIR step" << endl;
00085 evolver.evolve(particles);
00086 for (vector<Particle>::iterator it=particles.begin(); it!=particles.end(); it++){
00087 it->w*=likelyhoodModel.likelyhood(*it);
00088 }
00089
00090 ofstream os("sir.dat");
00091 printParticles(os, particles);
00092 os.close();
00093 vector<Particle> newpart=resampler.resample(particles);
00094 particles=newpart;
00095
00096 cout << "plot [-200:200][-200:200]\"sir.dat\" w p" << endl;
00097 }
00098 }
00099