densitytreepdf.cpp

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

#include "DensityTreePdf.hpp"

#include "DensityTreeInternal.hpp"
#include "DensityTreeLeaf.hpp"

#include <typeinfo>

using namespace indii::ml::aux;

DensityTreePdf::DensityTreePdf() : N(0), mu(0), sigma(0), haveMu(false),
    haveSigma(false), root(NULL) {
  //
}

DensityTreePdf::DensityTreePdf(DiracMixturePdf& p) : N(p.getDimensions()),
    mu(p.getDimensions()), sigma(p.getDimensions()), haveMu(false),
    haveSigma(false), root(NULL) {
  DensityTreeFactory factory(p.getNumComponents());
  root = factory.create(p);
}

DensityTreePdf::DensityTreePdf(DiracMixturePdf& p,
    const DensityTreeFactory& factory) : N(p.getDimensions()),
    mu(p.getDimensions()), sigma(p.getDimensions()), haveMu(false),
    haveSigma(false), root(NULL) {
  root = factory.create(p);
}

DensityTreePdf::DensityTreePdf(const DensityTreePdf& o) : N(o.N), mu(o.mu),
    sigma(o.sigma), haveMu(o.haveMu), haveSigma(o.haveSigma), root(NULL) {
  if (o.root != NULL) {
    if (typeid(*o.root) == typeid(DensityTreeInternal)) {
      root = new DensityTreeInternal(
          static_cast<DensityTreeInternal&>(*o.root));
    } else if (typeid(*o.root) == typeid(DensityTreeLeaf)) {
      root = new DensityTreeLeaf(
          static_cast<DensityTreeLeaf&>(*o.root));
    } else {
      assert(false);
    }
    assert (root != NULL);
  }
}

DensityTreePdf::~DensityTreePdf() {
  delete root;
}

DensityTreePdf& DensityTreePdf::operator=(const DensityTreePdf& o) {
  /* pre-condition */
  assert (N == o.N);

  haveMu = o.haveMu;
  haveSigma = o.haveSigma;
  if (haveMu) {
    mu = o.mu;
  }
  if (haveSigma) {
    sigma = o.sigma;
  }
  
  if (root != NULL) {
    delete root;
    root = NULL;
  }
  if (o.root != NULL) {
    if (typeid(*o.root) == typeid(DensityTreeInternal)) {
      root = new DensityTreeInternal(
          static_cast<DensityTreeInternal&>(*o.root));
    } else if (typeid(*o.root) == typeid(DensityTreeLeaf)) {
      root = new DensityTreeLeaf(
          static_cast<DensityTreeLeaf&>(*o.root));
    } else {
      assert(false);
    }
    assert (root != NULL);
  }
    
  return *this;
}

const vector& DensityTreePdf::getLower() const {
  /* pre-condition */
  assert (root != NULL);

  return root->getLower();
}

const vector& DensityTreePdf::getUpper() const {
  /* pre-condition */
  assert (root != NULL);

  return root->getUpper();
}

unsigned int DensityTreePdf::getDimensions() const {
  return N;
}

void DensityTreePdf::setDimensions(const unsigned int N,
    const bool preserve) {
  /* not supported */
  assert (false);
}

const vector& DensityTreePdf::getExpectation() {
  if (!haveMu) {
    calculateExpectation();
  }
  return mu;
}

const symmetric_matrix& DensityTreePdf::getCovariance() {
  if (!haveSigma) {
    calculateCovariance();
  }
  return sigma;
}

vector DensityTreePdf::sample() {
  /* pre-condition */
  assert (root != NULL);

  return root->sample(Random::uniform(0.0, root->getWeight()));
}

double DensityTreePdf::densityAt(const vector& x) {
  /* pre-condition */
  assert (x.size() == N);

  double p = 0.0;
  
  if (root != NULL) {
    const vector& lower = root->getLower();
    const vector& upper = root->getUpper();
    bool inside = true;
    unsigned int i;
  
    for (i = 0; i < N && inside; i++) {
      inside = inside && lower(i) <= x(i) && x(i) < upper(i);
    }
    if (inside) {
      p = root->densityAt(x) / root->getWeight();
    }
  }
  
  return p;
}

void DensityTreePdf::calculateExpectation() {
  /* pre-condition */
  assert (root != NULL);

  noalias(mu) = root->sumExpectation() / root->getWeight();
  haveMu = true;

  /* post-condition */
  assert (haveMu);
}

void DensityTreePdf::calculateCovariance() {
  /* pre-condition */
  assert (root != NULL);

  const vector& mu = getExpectation();
  noalias(sigma) = root->sumCovariance() / root->getWeight()
      - outer_prod(mu, mu);
  haveSigma = true;
  
  /* post-condition */
  assert (haveSigma);
}