more particle filter stuff

3 files changed, 96 insertions, 30 deletions

diff --git a/gi/pf/pf.h b/gi/pf/pf.h
new file mode 100644
index 00000000..ede7cda8
--- /dev/null
+++ b/gi/pf/pf.h
@@ -0,0 +1,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
diff --git a/gi/pf/pfdist.cc b/gi/pf/pfdist.cc
index 81abd61b..aae5f798 100644
--- a/gi/pf/pfdist.cc
+++ b/gi/pf/pfdist.cc
@@ -6,6 +6,7 @@
 #include <boost/program_options.hpp>
 #include <boost/program_options/variables_map.hpp>
 
+#include "pf.h"
 #include "base_measures.h"
 #include "reachability.h"
 #include "viterbi.h"
@@ -413,20 +414,6 @@ ostream& operator<<(ostream& o, const Particle& p) {
   return o;
 }
 
-void FilterCrapParticlesAndReweight(vector<Particle>* pps) {
-  vector<Particle>& ps = *pps;
-  SampleSet<prob_t> ss;
-  for (int i = 0; i < ps.size(); ++i)
-    ss.add(ps[i].weight);
-  vector<Particle> 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[prng->SelectSample(ss)]);
-    nps[i].weight = uniform_weight;
-  }
-  nps.swap(ps);
-}
-
 int main(int argc, char** argv) {
   po::variables_map conf;
   InitCommandLine(argc, argv, &conf);
@@ -466,6 +453,7 @@ int main(int argc, char** argv) {
   MyJointModel m(lp0);
 #endif
 
+  MultinomialResampleFilter<Particle> filter(&rng);
   cerr << "Initializing reachability limits...\n";
   vector<Particle> ps(corpusf.size());
   vector<Reachability> reaches; reaches.reserve(corpusf.size());
@@ -500,7 +488,7 @@ int main(int argc, char** argv) {
 
         // all particles have now been extended a bit, we will reweight them now
         if (lps[0].trg_cov > 0)
-          FilterCrapParticlesAndReweight(&lps);
+          filter(&lps);
 
         // loop over all particles and extend them
         bool done_nothing = true;
diff --git a/gi/pf/pfnaive.cc b/gi/pf/pfnaive.cc
index d967958c..728ec00d 100644
--- a/gi/pf/pfnaive.cc
+++ b/gi/pf/pfnaive.cc
@@ -6,6 +6,7 @@
 #include <boost/program_options.hpp>
 #include <boost/program_options/variables_map.hpp>
 
+#include "pf.h"
 #include "base_measures.h"
 #include "monotonic_pseg.h"
 #include "reachability.h"
@@ -135,20 +136,6 @@ ostream& operator<<(ostream& o, const Particle& p) {
   return o;
 }
 
-void FilterCrapParticlesAndReweight(vector<Particle>* pps) {
-  vector<Particle>& ps = *pps;
-  SampleSet<prob_t> ss;
-  for (int i = 0; i < ps.size(); ++i)
-    ss.add(ps[i].weight);
-  vector<Particle> 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[prng->SelectSample(ss)]);
-    nps[i].weight = uniform_weight;
-  }
-  nps.swap(ps);
-}
-
 int main(int argc, char** argv) {
   po::variables_map conf;
   InitCommandLine(argc, argv, &conf);
@@ -204,6 +191,8 @@ int main(int argc, char** argv) {
   cerr << "Sampling...\n"; 
   vector<Particle> tmp_p(10000);  // work space
   SampleSet<prob_t> pfss;
+  SystematicResampleFilter<Particle> filter(&rng);
+  // MultinomialResampleFilter<Particle> filter(&rng);
   for (int SS=0; SS < samples; ++SS) {
     for (int ci = 0; ci < corpusf.size(); ++ci) {
       vector<int>& src = corpusf[ci];
@@ -223,7 +212,7 @@ int main(int argc, char** argv) {
 
         // all particles have now been extended a bit, we will reweight them now
         if (lps[0].trg_cov > 0)
-          FilterCrapParticlesAndReweight(&lps);
+          filter(&lps);
 
         // loop over all particles and extend them
         bool done_nothing = true;
@@ -273,6 +262,11 @@ int main(int argc, char** argv) {
           }
         } // loop over particles (pi = 0 .. particles)
         if (done_nothing) all_complete = true;
+        prob_t wv = prob_t::Zero();
+        for (int pp = 0; pp < lps.size(); ++pp)
+          wv += lps[pp].weight;
+        for (int pp = 0; pp < lps.size(); ++pp)
+          lps[pp].weight /= wv;
       }
       pfss.clear();
       for (int i = 0; i < lps.size(); ++i)