deterministicparticleresampler.cpp

dysii是一款非常出色的滤波函数库

//#if defined(__GNUC__) && defined(GCC_PCH)
//  #include "../aux/aux.hpp"
//#endif

#include "DeterministicParticleResampler.hpp"

namespace aux = indii::ml::aux;

using namespace indii::ml::filter;

DeterministicParticleResampler::DeterministicParticleResampler(
    const unsigned int P) : P(P) {
  //
}

DeterministicParticleResampler::~DeterministicParticleResampler() {
  //
}

void DeterministicParticleResampler::setNumParticles(const unsigned int P) {
  this->P = P;
}

aux::DiracMixturePdf DeterministicParticleResampler::resample(
    aux::DiracMixturePdf& p) {
  boost::mpi::communicator world;
  int rank = world.rank();

  aux::DiracMixturePdf resampled(p.getDimensions());
  aux::DiracMixturePdf::weighted_component_vector sorted;
  aux::DiracMixturePdf::weighted_component_iterator iter, end;
  double alpha, u, w, j, W_s;

  unsigned int P = this->P;
  if (P == 0) {
    P = p.getDistributedNumComponents();
  }

  /* redistribute by weight, mitigates memory issues, as one node may end
     up with most particles otherwise */
  p.redistributeByWeight();

  /* sort particles by weight */
  p.sort(); // allows optimisations if already sorted, etc
  sorted = p.getComponents();

  /* scan sum weights across nodes */
  W_s = boost::mpi::scan(world, p.getTotalWeight(), std::plus<double>());

  /* generate common random alpha across nodes */
  if (rank == 0) {
    alpha = aux::Random::uniform(0.0, 1.0);
  }
  boost::mpi::broadcast(world, alpha, 0);

  /* resample */
  w = p.getDistributedTotalWeight() / P;
  u = W_s - p.getTotalWeight();
  if (u >= alpha*w) {
    j = alpha + ceil((u - alpha*w) / w);
  } else {
    j = alpha;
  }

  iter = sorted.begin();
  end = sorted.end();
  while (iter != end) {
    u += iter->w;
    while (u >= w * j) {
      resampled.addComponent(iter->x);
      j += 1.0;
    }
    iter++;
  }

  resampled.redistributeByCount();

  return resampled;
}