densitytreeparticleresampler.cpp

来自「dysii是一款非常出色的滤波函数库」· C++ 代码 · 共 118 行

#include "DensityTreeParticleResampler.hpp"

#include "../aux/DensityTreeMixturePdf.hpp"

#include <vector>
#include <set>

namespace aux = indii::ml::aux;

using namespace indii::ml::filter;

DensityTreeParticleResampler::DensityTreeParticleResampler(
    const unsigned int P, const aux::DensityTreeFactory& factory) : P(P),
    factory(factory) {
  //
}

DensityTreeParticleResampler::DensityTreeParticleResampler(
    const unsigned int P) : P(P), factory(P) {
  //
}

DensityTreeParticleResampler::~DensityTreeParticleResampler() {
  //
}

aux::DiracMixturePdf DensityTreeParticleResampler::resample(
    aux::DiracMixturePdf& p) {
  /* degenerate case */
  if (p.getNumComponents() == 0) {
    return p;
  }
  
  p.redistributeByCount();
  p.distributedNormalise();

  const unsigned int N = p.getDimensions();
  unsigned int i, j;
  double w, qw, pw;

  aux::DiracMixturePdf resampled(N);

  /* create density tree mixture from particles, one tree per node */
  aux::DensityTreePdf tree(p, factory);
  aux::DensityTreeMixturePdf mixture(N);
  mixture.addComponent(tree, p.getTotalWeight());

  /* create density tree importance distribution */
  aux::DensityTreeFactory importanceFactory(0.25*sqrt(P), 4.0*log(P)/log(2.0), 0.3,
      aux::DensityTreeFactory::SPLIT_VARIANCE);
  aux::DensityTreePdf importance(p, importanceFactory);
  aux::DensityTreeMixturePdf importanceMixture(N);
  importanceMixture.addComponent(importance, p.getTotalWeight());
  
  /* determine extreme particles in each dimension */
  const aux::DiracMixturePdf::weighted_component_vector& particles = 
      p.getComponents();
  aux::vector lower(particles[0].x), upper(particles[0].x);
  std::vector<unsigned int> lowerIndex(N), upperIndex(N);
  lowerIndex.clear();
  upperIndex.clear();
  
  for (i = 1; i < P; i++) {
    for (j = 0; j < N; j++) {
      if (particles[i].x(j) < lower(j)) {
        lower(j) = particles[i].x(j);
        lowerIndex[j] = i;
      } else if (particles[i].x(j) > upper(j)) {
        upper(j) = particles[i].x(j);
        upperIndex[j] = i;
      }
    }
  }
  std::set<unsigned int> extremes;
  std::set<unsigned int>::iterator extremesIter, extremesEnd;
  for (i = 0; i < N; i++) {
    extremes.insert(lowerIndex[i]);
    extremes.insert(upperIndex[i]);
  }
  
  /* sample extreme particles */
  extremesIter = extremes.begin();
  extremesEnd = extremes.end();
  while (extremesIter != extremesEnd) {
    pw = mixture.densityAt(particles[*extremesIter].x);
    qw = importanceMixture.densityAt(particles[*extremesIter].x);
    w = P * pw / ((P - extremes.size()) * qw + 1);
    resampled.addComponent(particles[*extremesIter].x, w);
    extremesIter++;
  }
  
  /* importance sample remaining particles from tree */
  
  std::vector<aux::vector> xs(importanceMixture.distributedSample(P - extremes.size()));
  std::vector<aux::vector>::iterator xsIter, xsEnd;
  xsIter = xs.begin();
  xsEnd = xs.end();
  while (xsIter != xsEnd) {
    pw = mixture.densityAt(*xsIter);
    qw = importanceMixture.densityAt(*xsIter);
    w = P * pw / ((P - extremes.size()) * qw);
    resampled.addComponent(*xsIter, w);
    xsIter++;
  }

  /*std::vector<aux::vector> xs(mixture.distributedSample(P));
  std::vector<aux::vector>::iterator xsIter, xsEnd;
  xsIter = xs.begin();
  xsEnd = xs.end();
  while (xsIter != xsEnd) {
    resampled.addComponent(*xsIter);
    xsIter++;
  }*/

  return resampled;
}