range_bearing.cpp
Go to the documentation of this file.
1 #include <stdlib.h>
2 #include <iostream>
3 #include <fstream>
4 #include <gmapping/utils/point.h>
6 
7 using namespace std;
8 using namespace GMapping;
9 
10 #define test(s) {cout << s << " " << flush;}
11 #define testOk() {cout << "OK" << endl;}
12 
13 struct Particle{
14  Particle(): p(0,0), w(0){}
16  double w;
17  operator double() const {return w; }
18  void setWeight(double _w) {w=_w;}
19 };
20 
21 ostream& printParticles(ostream& os, const vector<Particle>& p)
22 {
23  for (vector<Particle>::const_iterator it=p.begin(); it!=p.end(); ++it) {
24  os << it->p.x << " " << it->p.y << endl;
25  }
26  return os;
27 }
28 
29 struct EvolutionModel{
31  Particle pn(p);
32  pn.p.x+=10*(drand48()-.5);
33  pn.p.y+=10*(drand48()-.5);
34  return pn;
35  }
36 };
37 
38 
39 struct LikelyhoodModel{
40  std::vector<Point> observerVector;
41  std::vector<double> observations;
42  double sigma;
43  double likelyhood(const Particle& p) const{
44  double v=1;
45  std::vector<double>::const_iterator oit=observations.begin();
46  for (std::vector<Point>::const_iterator it=observerVector.begin(); it!=observerVector.end();it++){
47  v*=exp(-pow(((p.p-*it)*(p.p-*it)-*oit*(*oit))/sigma, 2));
48  oit++;
49  }
50  cout << "#v=" << v << endl;
51  return v;
52  }
53 };
54 
55 int main (unsigned int argc, const char * const * argv){
56  vector<Particle> particles(1000);
57  LikelyhoodModel likelyhoodModel;
60 
61  for (vector<Particle>::iterator it=particles.begin(); it!=particles.end(); it++){
62  it->w=1;
63  it->p.x=400*(drand48()-.5);
64  it->p.y=400*(drand48()-.5);
65  }
66 
67  vector<Point> sensors;
68 
69  sensors.push_back(Point(-50,0));
70  sensors.push_back(Point(50,0));
71  sensors.push_back(Point(0,100));
72 
73  likelyhoodModel.sigma=1000;
74  likelyhoodModel.observations.push_back(70);
75  likelyhoodModel.observations.push_back(70);
76  likelyhoodModel.observations.push_back(70);
77 
78  likelyhoodModel.observerVector=sensors;
79  while (1){
80  char buf[2];
81  cin.getline(buf,2);
82  vector<Particle> newgeneration;
83 
84  cout << "# SIR step" << endl;
85  evolver.evolve(particles);
86  for (vector<Particle>::iterator it=particles.begin(); it!=particles.end(); it++){
87  it->w*=likelyhoodModel.likelyhood(*it);
88  }
89 
90  ofstream os("sir.dat");
91  printParticles(os, particles);
92  os.close();
93  vector<Particle> newpart=resampler.resample(particles);
94  particles=newpart;
95 
96  cout << "plot [-200:200][-200:200]\"sir.dat\" w p" << endl;
97  }
98 }
99 
point.h
LikelyhoodModel::likelyhood
double likelyhood(const Particle &p) const
Definition: range_bearing.cpp:43
particlefilter.h
uniform_resampler
Definition: particlefilter.h:148
GMapping
Definition: configfile.cpp:34
Particle::setWeight
void setWeight(double _w)
Definition: range_bearing.cpp:18
EvolutionModel
Definition: particlefilter_test.cpp:26
Particle
Definition: particlefilter_test.cpp:11
evolver
Definition: particlefilter.h:263
printParticles
ostream & printParticles(ostream &os, const vector< Particle > &p)
Definition: range_bearing.cpp:21
LikelyhoodModel
Definition: particlefilter_test.cpp:40
LikelyhoodModel::observations
std::vector< double > observations
Definition: range_bearing.cpp:41
LikelyhoodModel::sigma
double sigma
Definition: range_bearing.cpp:42
evolver::evolve
void evolve(std::vector< Particle > &particles)
Definition: particlefilter.h:270
LikelyhoodModel::observerVector
std::vector< Point > observerVector
Definition: range_bearing.cpp:40
Particle::p
Point p
Definition: range_bearing.cpp:15
Particle::p
double p
Definition: particlefilter_test.cpp:12
GMapping::Point
point< double > Point
Definition: point.h:202
uniform_resampler::resample
std::vector< Particle > resample(const std::vector< Particle > &particles, int nparticles=0) const
Definition: particlefilter.h:192
GMapping::point< double >
main
int main(unsigned int argc, const char *const *argv)
Definition: range_bearing.cpp:55
Particle::Particle
Particle()
Definition: range_bearing.cpp:14
EvolutionModel::evolve
Particle evolve(const Particle &p)
Definition: range_bearing.cpp:30


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