range_bearing.cpp
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 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13