pf
resamplers.h
Go to the documentation of this file.
1 #ifndef RESAMPLERS_H
2 #define RESAMPLERS_H
3 
4 #include <chrono>
5 #include <array>
6 #include <random>
7 #include <numeric> // accumulate, partial_sum
8 #include <cmath> //floor
9 #include <Eigen/Dense>
10 
11 
13 
24 template<size_t nparts, size_t dimx, typename float_t >
25 class rbase
26 {
27 public:
28 
30  using ssv = Eigen::Matrix<float_t,dimx,1>;
32  using arrayVec = std::array<ssv, nparts>;
34  using arrayFloat = std::array<float_t,nparts>;
35 
36 
40  rbase();
41 
42 
47  rbase(unsigned long seed);
48 
49 
55  virtual void resampLogWts(arrayVec &oldParts, arrayFloat &oldLogUnNormWts) = 0;
56 
57 protected:
58 
60  std::mt19937 m_gen;
61 
62 };
63 
64 
65 template<size_t nparts, size_t dimx, typename float_t>
67  : m_gen{static_cast<std::uint32_t>(
68  std::chrono::high_resolution_clock::now().time_since_epoch().count()
69  )}
70 {
71 }
72 
73 
74 template<size_t nparts, size_t dimx, typename float_t>
76  : m_gen{static_cast<std::uint32_t>(seed)}
77 {
78 }
79 
80 
81 
91 template<size_t nparts, size_t dimx, typename float_t>
92 class mn_resampler : private rbase<nparts, dimx, float_t>
93 {
94 public:
95 
97  using ssv = Eigen::Matrix<float_t,dimx,1>;
99  using arrayVec = std::array<ssv, nparts>;
101  using arrayFloat = std::array<float_t,nparts>;
103  using arrayInt = std::array<unsigned int,nparts>;
104 
108  mn_resampler() = default;
109 
110 
115  mn_resampler(unsigned long seed);
116 
117 
123  void resampLogWts(arrayVec &oldParts, arrayFloat &oldLogUnNormWts);
124 
125 };
126 
127 
128 template<size_t nparts, size_t dimx, typename float_t>
130  : rbase<nparts, dimx, float_t>(seed)
131 {
132 }
133 
134 
135 template<size_t nparts, size_t dimx, typename float_t>
137 {
138  // these log weights may be very negative. If that's the case, exponentiating them may cause underflow
139  // so we use the "log-exp-sum" trick
140  // actually not quite...we just shift the log-weights because after they're exponentiated
141  // they have the same normalized probabilities
142 
143  // Create the distribution with exponentiated log-weights
144  arrayFloat w;
145  float_t m = *std::max_element(oldLogUnNormWts.begin(), oldLogUnNormWts.end());
146  std::transform(oldLogUnNormWts.begin(), oldLogUnNormWts.end(), w.begin(),
147  [&m](float_t& d) -> float_t { return std::exp( d - m ); } );
148  std::discrete_distribution<> idxSampler(w.begin(), w.end());
149 
150  // create temporary particle vector and weight vector
151  arrayVec tmpPartics = oldParts;
152 
153  // sample from the original parts and store in tmpParts
154  unsigned int whichPart;
155  for(size_t part = 0; part < nparts; ++part)
156  {
157  whichPart = idxSampler(this->m_gen);
158  tmpPartics[part] = oldParts[whichPart];
159  }
160 
161  //overwrite olds with news
162  oldParts = std::move(tmpPartics);
163  std::fill(oldLogUnNormWts.begin(), oldLogUnNormWts.end(), 0.0); // change back
164 }
165 
166 
167 
178 template<size_t nparts, size_t dimsampledx, typename cfModT, typename float_t>
180 {
181 public:
182 
184  using ssv = Eigen::Matrix<float_t,dimsampledx,1>;
186  using arrayVec = std::array<ssv, nparts>;
188  using arrayFloat = std::array<float_t,nparts>;
190  using arrayMod = std::array<cfModT,nparts>;
191 
196 
197 
201  mn_resampler_rbpf(unsigned long seed);
202 
203 
210  void resampLogWts(arrayMod &oldMods, arrayVec &oldParts, arrayFloat &oldLogUnNormWts);
211 
212 
213 private:
214 
216  std::mt19937 m_gen;
217 };
218 
219 
220 template<size_t nparts, size_t dimsampledx, typename cfModT, typename float_t>
222  : m_gen{static_cast<std::uint32_t>(
223  std::chrono::high_resolution_clock::now().time_since_epoch().count()
224  )}
225 {
226 }
227 
228 
229 template<size_t nparts, size_t dimsampledx, typename cfModT, typename float_t>
231  : m_gen{ static_cast<std::uint32_t>(seed) }
232 {
233 }
234 
235 
236 template<size_t nparts, size_t dimsampledx, typename cfModT, typename float_t>
238 {
239  // Create the distribution with exponentiated log-weights
240  arrayFloat w;
241  float_t m = *std::max_element(oldLogUnNormWts.begin(), oldLogUnNormWts.end());
242  std::transform(oldLogUnNormWts.begin(), oldLogUnNormWts.end(), w.begin(),
243  [&m](float_t& d) -> float_t { return std::exp( d - m ); } );
244  std::discrete_distribution<> idxSampler(w.begin(), w.end());
245 
246  // create temporary vectors for samps and mods
247  arrayVec tmpSamps;
248  arrayMod tmpMods;
249 
250  // sample from the original parts and store in temporary
251  unsigned int whichPart;
252  for(size_t part = 0; part < nparts; ++part)
253  {
254  whichPart = idxSampler(m_gen);
255  tmpSamps[part] = oldSamps[whichPart];
256  tmpMods[part] = oldMods[whichPart];
257  }
258 
259  //overwrite olds with news
260  oldMods = std::move(tmpMods);
261  oldSamps = std::move(tmpSamps);
262  std::fill(oldLogUnNormWts.begin(), oldLogUnNormWts.end(), 0.0);
263 
264 }
265 
266 
277 template<size_t nparts, size_t dimx, typename float_t>
278 class resid_resampler : private rbase<nparts, dimx, float_t>
279 {
280 public:
281 
283  using ssv = Eigen::Matrix<float_t,dimx,1>;
285  using arrayVec = std::array<ssv, nparts>;
287  using arrayFloat = std::array<float_t,nparts>;
289  using arrayInt = std::array<unsigned int, nparts>;
290 
291 
295  resid_resampler() = default;
296 
297 
302  resid_resampler(unsigned long seed);
303 
304 
310  void resampLogWts(arrayVec &oldParts, arrayFloat &oldLogUnNormWts);
311 
312 };
313 
314 
315 template<size_t nparts, size_t dimx, typename float_t>
317  : rbase<nparts, dimx, float_t>(seed)
318 {
319 }
320 
321 
322 template<size_t nparts, size_t dimx, typename float_t>
324 {
325 
326  // calculate normalized weights
327  arrayFloat w;
328  float_t m = *std::max_element(oldLogUnNormWts.begin(), oldLogUnNormWts.end());
329  std::transform(oldLogUnNormWts.begin(), oldLogUnNormWts.end(), w.begin(),
330  [&m](const float_t& d) -> float_t { return std::exp( d - m ); } );
331  float_t norm_const (0.0);
332  norm_const = std::accumulate(w.begin(), w.end(), norm_const);
333  for( auto& weight : w)
334  weight = weight/norm_const;
335 
336  // calc unNormWBars and numRandomSamples (N-R using IIHMM notation)
337  size_t i;
338  arrayFloat unNormWBar;
339  float_t numRandomSamples(0.0);
340  for(i = 0; i < nparts; ++i) {
341  unNormWBar[i] = w[i]*nparts - std::floor(w[i]*nparts);
342  numRandomSamples += unNormWBar[i];
343  }
344 
345  // make multinomial distribution for residuals
346  std::discrete_distribution<> idxSampler(unNormWBar.begin(), unNormWBar.end());
347 
348  // start resampling by producing a count vector
349  arrayInt sampleCounts;
350  for(i = 0; i < nparts; ++i) {
351  sampleCounts[i] = static_cast<unsigned int>(std::floor(nparts*w[i])); // initial
352  }
353  for(i = 0; i < static_cast<unsigned int>(numRandomSamples); ++i) {
354  sampleCounts[idxSampler(this->m_gen)]++;
355  }
356 
357  // now resample the particles using the counts
358  arrayVec tmpPartics;
359  unsigned int c(0);
360  for(i = 0; i < nparts; ++i) { // over count container
361  unsigned int num_replicants = sampleCounts[i];
362  if( num_replicants > 0) {
363  for(size_t j = 0; j < num_replicants; ++j) { // assign the same thing several times
364  tmpPartics[c] = oldParts[i];
365  c++;
366  }
367  }
368  }
369 
370  //overwrite olds with news
371  oldParts = std::move(tmpPartics);
372  std::fill(oldLogUnNormWts.begin(), oldLogUnNormWts.end(), 0.0); // change back
373 }
374 
375 
386 template<size_t nparts, size_t dimx, typename float_t>
387 class stratif_resampler : private rbase<nparts, dimx, float_t>
388 {
389 public:
390 
392  using ssv = Eigen::Matrix<float_t,dimx,1>;
394  using arrayVec = std::array<ssv, nparts>;
396  using arrayFloat = std::array<float_t,nparts>;
398  using arrayInt = std::array<unsigned int, nparts>;
399 
400 
404  stratif_resampler() = default;
405 
406 
411  stratif_resampler(unsigned long seed);
412 
413 
419  void resampLogWts(arrayVec &oldParts, arrayFloat &oldLogUnNormWts);
420 
421 };
422 
423 
424 template<size_t nparts, size_t dimx, typename float_t>
426  : rbase<nparts, dimx, float_t>(seed)
427 {
428 }
429 
430 
431 template<size_t nparts, size_t dimx, typename float_t>
433 {
434 
435  // calculate normalized weights
436  arrayFloat w;
437  float_t m = *std::max_element(oldLogUnNormWts.begin(), oldLogUnNormWts.end());
438  std::transform(oldLogUnNormWts.begin(), oldLogUnNormWts.end(), w.begin(),
439  [&m](const float_t& d) -> float_t { return std::exp( d - m ); } );
440  float_t norm_const (0.0);
441  norm_const = std::accumulate(w.begin(), w.end(), norm_const);
442  for( auto& weight : w)
443  weight = weight/norm_const;
444 
445  // calculate the cumulative sums of the weights
446  // TODO: possible bug
447  arrayFloat cumsums;
448  std::partial_sum(w.begin(), w.end(), cumsums.begin());
449 
450  // samplethe Uis
451  std::uniform_real_distribution<float_t> u_sampler(0.0, 1.0/nparts);
452  arrayFloat u_samples;
453  for(size_t i = 0; i < nparts; ++i) {
454  u_samples[i] = i/nparts + u_sampler(this->m_gen);
455  }
456 
457  // resample
458  arrayVec tmpPartics;
459  for(size_t i = 0; i < nparts; ++i){ // tmpPartics, Uis
460 
461  // find which index
462  unsigned int idx;
463  for(unsigned int j = 0; j < nparts; ++j){
464 
465  // get the first time it gets covered by a cumsum
466  if(cumsums[j] >= u_samples[i]){
467  idx = j;
468  break;
469  }
470  }
471 
472  // assign
473  tmpPartics[i] = oldParts[idx];
474  }
475 
476  //overwrite olds with news
477  oldParts = std::move(tmpPartics);
478  std::fill(oldLogUnNormWts.begin(), oldLogUnNormWts.end(), 0.0); // change back
479 }
480 
481 
492 template<size_t nparts, size_t dimx, typename float_t>
493 class systematic_resampler : private rbase<nparts, dimx, float_t>
494 {
495 public:
496 
498  using ssv = Eigen::Matrix<float_t,dimx,1>;
500  using arrayVec = std::array<ssv, nparts>;
502  using arrayFloat = std::array<float_t,nparts>;
504  using arrayInt = std::array<unsigned int, nparts>;
505 
506 
510  systematic_resampler() = default;
511 
512 
517  systematic_resampler(unsigned long seed);
518 
519 
525  void resampLogWts(arrayVec &oldParts, arrayFloat &oldLogUnNormWts);
526 
527 };
528 
529 
530 template<size_t nparts, size_t dimx, typename float_t>
532  : rbase<nparts, dimx, float_t>(seed)
533 {
534 }
535 
536 
537 template<size_t nparts, size_t dimx, typename float_t>
539 {
540 
541  // calculate normalized weights
542  arrayFloat w;
543  float_t m = *std::max_element(oldLogUnNormWts.begin(), oldLogUnNormWts.end());
544  std::transform(oldLogUnNormWts.begin(), oldLogUnNormWts.end(), w.begin(),
545  [&m](const float_t& d) -> float_t { return std::exp( d - m ); } );
546  float_t norm_const (0.0);
547  norm_const = std::accumulate(w.begin(), w.end(), norm_const);
548  for( auto& weight : w)
549  weight = weight/norm_const;
550 
551  // calculate the cumulative sums of the weights
552  arrayFloat cumsums;
553  std::partial_sum(w.begin(), w.end(), cumsums.begin());
554 
555  // samplethe Uis
556  std::uniform_real_distribution<float_t> u_sampler(0.0, 1.0/nparts);
557  arrayFloat u_samples;
558  u_samples[0] = u_sampler(this->m_gen);
559  for(size_t i = 1; i < nparts; ++i) {
560  u_samples[i] = u_samples[i-1] + 1.0/nparts;
561  }
562 
563  // resample (same code from here on as stratified)
564  arrayVec tmpPartics;
565  for(size_t i = 0; i < nparts; ++i){ // tmpPartics, Uis
566 
567  // find which index
568  unsigned int idx;
569  for(unsigned int j = 0; j < nparts; ++j){
570 
571  // get the first time it gets covered by a cumsum
572  if(cumsums[j] >= u_samples[i]){
573  idx = j;
574  break;
575  }
576  }
577 
578  // assign
579  tmpPartics[i] = oldParts[idx];
580  }
581 
582  //overwrite olds with news
583  oldParts = std::move(tmpPartics);
584  std::fill(oldLogUnNormWts.begin(), oldLogUnNormWts.end(), 0.0); // change back
585 }
586 
587 
597 template<size_t nparts, size_t dimx, typename float_t>
598 class mn_resamp_fast1 : private rbase<nparts, dimx, float_t>
599 {
600 public:
601 
603  using ssv = Eigen::Matrix<float_t,dimx,1>;
605  using arrayVec = std::array<ssv, nparts>;
607  using arrayFloat = std::array<float_t,nparts>;
609  using arrayInt = std::array<unsigned int,nparts>;
610 
614  mn_resamp_fast1() = default;
615 
616 
620  mn_resamp_fast1(unsigned long seed);
621 
622 
628  void resampLogWts(arrayVec &oldParts, arrayFloat &oldLogUnNormWts);
629 
630 };
631 
632 
633 template<size_t nparts, size_t dimx, typename float_t>
635  : rbase<nparts, dimx, float_t>(seed)
636 {
637 }
638 
639 
640 template<size_t nparts, size_t dimx, typename float_t>
642 {
643  // these log weights may be very negative. If that's the case, exponentiating them may cause underflow
644  // so we use the "log-exp-sum" trick
645  // actually not quite...we just shift the log-weights because after they're exponentiated
646  // they have the same normalized probabilities
647 
648  // Also, we're using a fancier algorthm detailed on page 244 of IHMM
649 
650  // Create unnormalized weights
651  arrayFloat unnorm_weights;
652  float_t m = *std::max_element(oldLogUnNormWts.begin(), oldLogUnNormWts.end());
653  std::transform(oldLogUnNormWts.begin(), oldLogUnNormWts.end(), unnorm_weights.begin(),
654  [&m](float_t& d) -> float_t { return std::exp( d - m ); } );
655 
656  // get a uniform rv sampler
657  std::uniform_real_distribution<float_t> u_sampler(0.0, 1.0);
658 
659  // two things:
660  // 1.) calculate normalizing constant for weights, and
661  // 2.) generate all these exponentials to help with getting order statistics
662  // NB: you never need to store E_{N+1}! (this is subtle)
663  float_t weight_norm_const(0.0);
664  arrayFloat exponentials;
665  float_t G(0.0);
666  for(size_t i = 0; i < nparts; ++i) {
667  weight_norm_const += unnorm_weights[i];
668  exponentials[i] = -std::log(u_sampler(this->m_gen));
669  G += exponentials[i];
670  }
671  G+= std::log(u_sampler(this->m_gen)); // E_{N+1}
672 
673  // see Fig 7.15 in IHMM on page 243
674  arrayVec tmpPartics = oldParts; // the new particles
675  float_t uniform_order_stat(0.0); // U_{(i)} in the notation of IHMM
676  float_t running_sum_normalized_weights(unnorm_weights[0]/weight_norm_const); // \sum_{j=1}^I \omega^j in the notation of IHMM
677  float_t one_less_summand(0.0); // \sum_{j=1}^{I-1} \omega^j
678  unsigned int idx = 0;
679  for(size_t i = 0; i < nparts; ++i){
680  uniform_order_stat += exponentials[i]/G; // add a spacing E_i/G
681  do {
682  if( one_less_summand < uniform_order_stat <= running_sum_normalized_weights ) {
683  // select index idx
684  tmpPartics[i] = oldParts[idx];
685  break;
686  }else{
687  // increment idx because it will never be chosen (all the other order statistics are even higher)
688  idx++;
689  running_sum_normalized_weights += unnorm_weights[idx]/weight_norm_const;
690  one_less_summand += unnorm_weights[idx-1]/weight_norm_const;
691  }
692  }while(true);
693  }
694 
695  //overwrite olds with news
696  oldParts = std::move(tmpPartics);
697  std::fill(oldLogUnNormWts.begin(), oldLogUnNormWts.end(), 0.0);
698 }
699 
700 #endif // RESAMPLERS_H
resid_resampler()=default
Default constructor.
void resampLogWts(arrayVec &oldParts, arrayFloat &oldLogUnNormWts)
resamples particles.
Definition: resamplers.h:136
Definition: resamplers.h:179
mn_resampler_rbpf()
Default constructor.
Definition: resamplers.h:221
rbase()
The default constructor gets called by default, and it sets the seed with the clock.
Definition: resamplers.h:66
Eigen::Matrix< float_t, dimx, 1 > ssv
Definition: resamplers.h:30
void resampLogWts(arrayMod &oldMods, arrayVec &oldParts, arrayFloat &oldLogUnNormWts)
resamples particles.
Definition: resamplers.h:237
Definition: resamplers.h:598
std::array< unsigned int, nparts > arrayInt
Definition: resamplers.h:609
Definition: resamplers.h:493
Base class for all resampler types.
Definition: resamplers.h:25
mn_resamp_fast1()=default
Default constructor.
Definition: resamplers.h:278
std::array< float_t, nparts > arrayFloat
Definition: resamplers.h:34
void resampLogWts(arrayVec &oldParts, arrayFloat &oldLogUnNormWts)
resamples particles.
Definition: resamplers.h:323
std::array< cfModT, nparts > arrayMod
Definition: resamplers.h:190
void resampLogWts(arrayVec &oldParts, arrayFloat &oldLogUnNormWts)
resamples particles.
Definition: resamplers.h:641
std::array< ssv, nparts > arrayVec
Definition: resamplers.h:32
std::mt19937 m_gen
prng
Definition: resamplers.h:60
std::mt19937 m_gen
prng
Definition: resamplers.h:216
stratif_resampler()=default
Default constructor.
void resampLogWts(arrayVec &oldParts, arrayFloat &oldLogUnNormWts)
resamples particles.
Definition: resamplers.h:432
std::array< unsigned int, nparts > arrayInt
Definition: resamplers.h:289
mn_resampler()=default
Default constructor.
std::array< unsigned int, nparts > arrayInt
Definition: resamplers.h:103
std::array< unsigned int, nparts > arrayInt
Definition: resamplers.h:398
Eigen::Matrix< float_t, dimsampledx, 1 > ssv
Definition: resamplers.h:184
Definition: resamplers.h:92
std::array< float_t, nparts > arrayFloat
Definition: resamplers.h:188
Definition: resamplers.h:387
std::array< unsigned int, nparts > arrayInt
Definition: resamplers.h:504
systematic_resampler()=default
Default constructor.
void resampLogWts(arrayVec &oldParts, arrayFloat &oldLogUnNormWts)
resamples particles.
Definition: resamplers.h:538
std::array< ssv, nparts > arrayVec
Definition: resamplers.h:186
virtual void resampLogWts(arrayVec &oldParts, arrayFloat &oldLogUnNormWts)=0
Function to resample from log unnormalized weights.