blob: ede7cda83d5266c8cf29e4935b505fd0ee2798fe (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
|
#ifndef _PF_H_
#define _PF_H_
#include <cassert>
#include <vector>
#include "sampler.h"
#include "prob.h"
template <typename ParticleType>
struct ParticleRenormalizer {
void operator()(std::vector<ParticleType>* pv) const {
if (pv->empty()) return;
prob_t z = prob_t::Zero();
for (unsigned i = 0; i < pv->size(); ++i)
z += (*pv)[i].weight;
assert(z > prob_t::Zero());
for (unsigned i = 0; i < pv->size(); ++i)
(*pv)[i].weight /= z;
}
};
template <typename ParticleType>
struct MultinomialResampleFilter {
explicit MultinomialResampleFilter(MT19937* rng) : rng_(rng) {}
void operator()(std::vector<ParticleType>* pv) {
if (pv->empty()) return;
std::vector<ParticleType>& ps = *pv;
SampleSet<prob_t> ss;
for (int i = 0; i < ps.size(); ++i)
ss.add(ps[i].weight);
std::vector<ParticleType> nps; nps.reserve(ps.size());
const prob_t uniform_weight(1.0 / ps.size());
for (int i = 0; i < ps.size(); ++i) {
nps.push_back(ps[rng_->SelectSample(ss)]);
nps[i].weight = uniform_weight;
}
nps.swap(ps);
}
private:
MT19937* rng_;
};
template <typename ParticleType>
struct SystematicResampleFilter {
explicit SystematicResampleFilter(MT19937* rng) : rng_(rng), renorm_() {}
void operator()(std::vector<ParticleType>* pv) {
if (pv->empty()) return;
renorm_(pv);
std::vector<ParticleType>& ps = *pv;
std::vector<ParticleType> nps; nps.reserve(ps.size());
double lower = 0, upper = 0;
const double skip = 1.0 / ps.size();
double u_j = rng_->next() * skip;
//std::cerr << "u_0: " << u_j << std::endl;
int j = 0;
for (unsigned i = 0; i < ps.size(); ++i) {
upper += ps[i].weight.as_float();
//std::cerr << "lower: " << lower << " upper: " << upper << std::endl;
// how many children does ps[i] have?
while (u_j < lower) { u_j += skip; ++j; }
while (u_j >= lower && u_j <= upper) {
assert(j < ps.size());
nps.push_back(ps[i]);
u_j += skip;
//std::cerr << " add u_j=" << u_j << std::endl;
++j;
}
lower = upper;
}
//std::cerr << ps.size() << " " << nps.size() << "\n";
assert(ps.size() == nps.size());
//exit(1);
ps.swap(nps);
}
private:
MT19937* rng_;
ParticleRenormalizer<ParticleType> renorm_;
};
#endif
|
