particlefilter
particlefilter_test.cpp
Go to the documentation of this file.
1
#include <stdlib.h>
2
#include <iostream>
3
#include <fstream>
4
#include "
gmapping/particlefilter/particlefilter.h
"
5
6
using namespace
std;
7
8
#define test(s) {cout << s << " " << flush;}
9
#define testOk() {cout << "OK" << endl;}
10
11
struct
Particle
{
12
double
p
;
13
double
w
;
14
inline
operator
double()
const
{
return
w;}
15
inline
void
setWeight
(
double
_w) {w=_w;}
16
};
17
18
ostream&
printParticles
(ostream& os,
const
vector<Particle>& p)
19
{
20
for
(vector<Particle>::const_iterator it=p.begin(); it!=p.end(); ++it) {
21
os << it->p<<
" "
<< (double)*it << endl;
22
}
23
return
os;
24
}
25
26
struct
EvolutionModel
{
27
Particle
evolve
(
const
Particle
& p){
28
Particle
pn(p);
29
pn.
p
+=.5*(drand48()-.5);
30
return
pn;
31
}
32
};
33
34
struct
QualificationModel
{
35
Particle
evolve
(
const
Particle
& p){
36
return
p;
37
}
38
};
39
40
struct
LikelyhoodModel
{
41
double
likelyhood
(
const
Particle
& p)
const
{
42
double
v = 1./(0.1+10*(p.
p
-2)*(p.
p
-2))+0.5/(0.1+10*(p.
p
-8)*(p.
p
-8));
43
return
v;
44
}
45
};
46
47
int
main
(
unsigned
int
argc,
const
char
*
const
* argv){
48
int
nparticles=100;
49
if
(argc>1)
50
nparticles=atoi(argv[1]);
51
vector<Particle> particles(nparticles);
52
LikelyhoodModel
likelyhoodModel;
53
uniform_resampler<Particle, double>
resampler;
54
auxiliary_evolver <Particle, double, QualificationModel, EvolutionModel, LikelyhoodModel>
auxevolver;
55
evolver <Particle, EvolutionModel>
evolver
;
56
57
for
(vector<Particle>::iterator it=particles.begin(); it!=particles.end(); it++){
58
it->w=1;
59
it->p=10*(drand48());
60
}
61
62
vector<Particle> sirparticles(particles);
63
vector<Particle> auxparticles(particles);
64
65
/*sir step*/
66
while
(1){
67
char
buf[2];
68
cin.getline(buf,2);
69
vector<Particle> newgeneration;
70
71
cout <<
"# SIR step"
<< endl;
72
evolver
.
evolve
(sirparticles);
73
for
(vector<Particle>::iterator it=sirparticles.begin(); it!=sirparticles.end(); it++){
74
it->setWeight(likelyhoodModel.
likelyhood
(*it));
75
}
76
ofstream os(
"sir.dat"
);
77
printParticles
(os, sirparticles);
78
os.close();
79
newgeneration=resampler.
resample
(sirparticles);
80
sirparticles=newgeneration;
81
82
cout <<
"# AUX step"
<< endl;
83
auxevolver.
evolve
(auxparticles);
84
for
(vector<Particle>::iterator it=auxparticles.begin(); it!=auxparticles.end(); it++){
85
it->setWeight(likelyhoodModel.
likelyhood
(*it));
86
}
87
os.open(
"aux.dat"
);
88
printParticles
(os, auxparticles);
89
os.close();
90
newgeneration=resampler.
resample
(auxparticles);
91
auxparticles=newgeneration;
92
cout <<
"plot [0:10][0:10]\"sir.dat\" w impulses"
<< endl;
93
cout <<
"replot 1./(0.1+10*(x-2)*(x-2))+0.5/(0.1+10*(x-8)*(x-8))"
<< endl;
94
95
// cout << "replot \"aux.dat\" w p" << endl;
96
}
97
}
98
main
int main(unsigned int argc, const char *const *argv)
Definition:
particlefilter_test.cpp:47
LikelyhoodModel::likelyhood
double likelyhood(const Particle &p) const
Definition:
particlefilter_test.cpp:41
particlefilter.h
uniform_resampler
Definition:
particlefilter.h:148
QualificationModel::evolve
Particle evolve(const Particle &p)
Definition:
particlefilter_test.cpp:35
Particle::setWeight
void setWeight(double _w)
Definition:
particlefilter_test.cpp:15
EvolutionModel
Definition:
particlefilter_test.cpp:26
Particle
Definition:
particlefilter_test.cpp:11
evolver
Definition:
particlefilter.h:263
LikelyhoodModel
Definition:
particlefilter_test.cpp:40
evolver::evolve
void evolve(std::vector< Particle > &particles)
Definition:
particlefilter.h:270
printParticles
ostream & printParticles(ostream &os, const vector< Particle > &p)
Definition:
particlefilter_test.cpp:18
Particle::w
double w
Definition:
particlefilter_test.cpp:13
Particle::p
double p
Definition:
particlefilter_test.cpp:12
uniform_resampler::resample
std::vector< Particle > resample(const std::vector< Particle > &particles, int nparticles=0) const
Definition:
particlefilter.h:192
auxiliary_evolver
Definition:
particlefilter.h:285
auxiliary_evolver::evolve
void evolve(std::vector< Particle > &particles)
Definition:
particlefilter.h:295
QualificationModel
Definition:
particlefilter_test.cpp:34
EvolutionModel::evolve
Particle evolve(const Particle &p)
Definition:
particlefilter_test.cpp:27
openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Thu Oct 19 2023 02:25:51