additivenoiseparticleresampler.hpp

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

#ifndef INDII_ML_FILTER_ADDITIVENOISEPARTICLERESAMPLER_HPP
#define INDII_ML_FILTER_ADDITIVENOISEPARTICLERESAMPLER_HPP

#include "ParticleResampler.hpp"
#include "../aux/GaussianPdf.hpp"

namespace indii {
  namespace ml {
    namespace filter {
/**
 * Particle resampler with independent additive noise source.
 *
 * @author Lawrence Murray <[e-mail]>
 * @version $Rev: 404 $
 * @date $Date: 2008-03-05 14:52:55 +0000 (Wed, 05 Mar 2008) $
 *
 * @param P Type of independent resampling noise source.
 *
 * Produces a new approximation of a weighted sample set by adding
 * noise from an independent source. This would usually be used after
 * resampling with a weight-based resampler such as
 * DeterministicParticleResampler, a la Condensation @ref Isard1998
 * "(Isard & Blake 1998)".
 */
template <class P = indii::ml::aux::GaussianPdf>
class AdditiveNoiseParticleResampler : public ParticleResampler {
public:
  /**
   * Constructor.
   *
   * @param r Independent resampling noise source.
   */
  AdditiveNoiseParticleResampler(const P& r);
  
  /**
   * Destructor.
   */
  virtual ~AdditiveNoiseParticleResampler();

  /**
   * Resample particle set.
   *
   * @param p Particle set to resample.
   *
   * The resampled particle set is constructed by taking each weighted
   * sample \f$(\mathbf{s}^{(i)},\pi^{(i)})\f$ of @p p and adding a
   * sample \f$\mathbf{r}^{(i)}\f$ from the independent reampling
   * noise source, giving
   * \f$(\mathbf{s}^{(i)}+\mathbf{r}^{(i)},\pi^{(i)})\f$.
   *
   * @return Resampled particle set.
   */
  virtual indii::ml::aux::DiracMixturePdf resample(
      indii::ml::aux::DiracMixturePdf& p);

private:
  /**
   * Independent resampling noise source.
   */
  P r;

};

    }
  }
}

template <class P>
indii::ml::filter::AdditiveNoiseParticleResampler<P>::AdditiveNoiseParticleResampler(
    const P& r) : r(r) {
  //
}

template <class P>
indii::ml::filter::AdditiveNoiseParticleResampler<P>::~AdditiveNoiseParticleResampler() {
  //
}

template <class P>
indii::ml::aux::DiracMixturePdf
    indii::ml::filter::AdditiveNoiseParticleResampler<P>::resample(
    indii::ml::aux::DiracMixturePdf& p) {
  indii::ml::aux::DiracMixturePdf q(p.getDimensions());
  indii::ml::aux::DiracMixturePdf::weighted_component_const_iterator iter, end;

  /**
   * @todo Can be more efficient. Get at underlying weighted_component
   * structure, copy and edit weights. That way don't have to
   * heap_push each time a resampled component is added, as the weight
   * is the same and the order can be maintained from the original
   * distribution.
   */
  iter = p.getComponents().begin();
  end = p.getComponents().end();
  while (iter != end) {
    q.addComponent(aux::DiracPdf(iter->x + r.sample()), iter->w);
    iter++;
  }

  return q;
}

#endif