Gaussian.cpp

Go to the documentation of this file.

// *****************************************************************
// Filename:    Gaussian.h
// Copyright:   Kai Welke, Chair Prof. Dillmann (IAIM),
//              Institute for Computer Science and Engineering (CSE),
//              University of Karlsruhe. All rights reserved.
// Author:      Kai Welke
// Date:        27.05.2012
// *****************************************************************
 
// *****************************************************************
// includes
// *****************************************************************
#include "Gaussian.h"
#include <cstdio>
#include <iostream>
 
// *****************************************************************
// construction
// *****************************************************************
Gaussian::Gaussian()
{
    this->dimension = -1;
}
 
Gaussian::Gaussian(int dimension)
{
    this->dimension = dimension;
 
    // initialize valid distribution
    this->cov = covariance_type::Identity(dimension, dimension);
    this->mean = value_type::Zero(dimension, 1);
}
 
Gaussian::Gaussian(const Gaussian& prototype)
{
    *this = prototype;
}
 
Gaussian::Gaussian(const value_type& mean, const covariance_type& covariance)
{
    this->cov = covariance;
    this->mean = mean;
 
    dimension = cov.rows();
 
    if (mean.rows() != dimension)
    {
        throw InvalidDimensionException();
    }
}
 
// *****************************************************************
// readouts
// *****************************************************************
double Gaussian::evaluate(const value_type& point)
{
    if (dimension == -1)
    {
        throw GaussianNotInitializedException();
    }
 
    if (point.rows() != dimension)
    {
        throw InvalidDimensionException();
    }
 
    Eigen::Matrix<double, 1, 1> inner =  -0.5 * (point - mean).transpose() * cov.inverse() * (point - mean);
    double e = exp(inner(0, 0));
 
    covariance_type i = cov;
    return pow(2 * M_PI, -getDimensions() / 2.0f) * pow(i.determinant(), -0.5) * e;
}
 
double Gaussian::mahalanobis(const value_type& point)
{
    if (dimension == -1)
    {
        throw GaussianNotInitializedException();
    }
 
    if (point.rows() != dimension)
    {
        throw InvalidDimensionException();
    }
 
    Eigen::Matrix<double, 1, 1> dist;
    dist = (point - mean).transpose() * cov.inverse() * (point - mean);
 
    return std::sqrt(dist(0, 0));
}
 
// generic draw sample for arbitrary covariance matrix
Gaussian::value_type Gaussian::drawSample()
{
    if (dimension == -1)
    {
        throw GaussianNotInitializedException();
    }
 
    // go through all dimensions and calculate independen random processes on unit
    // gaussian using Box-Muller method
 
    float fRand1, fRand2;
    value_type gaussianSample(getDimensions());
 
    // calculate gaussian distributed with Cov=I and mean = 0 (ndimension independant box muller gaussians)
    for (int d = 0 ; d < getDimensions() ; d++)
    {
        fRand1 = float(rand()) / RAND_MAX;
        fRand2 = float(rand()) / RAND_MAX;
 
        // calculate standard normal distribution sample (Box-Muller)
        gaussianSample(d) = sqrt(-2 * log(fRand1)) * cos(2 * M_PI * fRand2);
    }
 
    // in order to retrieve samples distributed according to cov
    // use cholesky decomposition:
    // A * A^t = cov
    // ==> random_vector=A * gaussianSample
    // (can be proved by X=AY, D= cov(Y) (with D=I, produced by above code), C=cov(X) (which is desired cov), C= E(XX^T)=E(AY(AY)^t)=AE(YY^t)A^t=ADA^t)
    // thus only matrx A has to be found
    covariance_type A = cov.llt().matrixL();
 
    value_type sample = A * gaussianSample + mean;
 
    return sample;
}
 
// draw sample for cases where only elements in diagonal of cov (can be used with zero in diagonal)
Gaussian::value_type Gaussian::drawSampleDiagonalCovariance()
{
    if (dimension == -1)
    {
        throw GaussianNotInitializedException();
    }
 
    // go through all dimensions and calculate independen random processes on unit
    // gaussian using Box-Muller method
 
    float fRand1, fRand2;
    value_type gaussianSample(getDimensions());
 
    // calculate gaussian distributed with Cov=I and mean = 0 (ndimension independant box muller gaussians)
    for (int d = 0 ; d < getDimensions() ; d++)
    {
        fRand1 = float(rand()) / RAND_MAX;
        fRand2 = float(rand()) / RAND_MAX;
 
        // calculate standard normal distribution sample (Box-Muller)
        gaussianSample(d) = sqrt(-2 * log(fRand1)) * cos(2 * M_PI * fRand2);
    }
 
    // generate matrix A
    covariance_type A(getDimensions(), getDimensions());
 
    for (int i = 0 ; i < getDimensions() ; i++)
        for (int u = 0 ; u < getDimensions() ; u++)
            if (i == u)
            {
                A(i, u) = sqrt(cov(i, u));
            }
            else
            {
                A(i, u) = 0;
            }
 
    value_type sample = A * gaussianSample + mean;
 
    return sample;
}
 
// *****************************************************************
// content manipulation
// *****************************************************************
void Gaussian::generateFromSamples(const samples_type& samples)
{
    if (dimension == -1)
    {
        throw GaussianNotInitializedException();
    }
 
    if (samples.rows() != dimension)
    {
        throw InvalidDimensionException();
    }
 
    int numberSamples = samples.cols();
 
    // calculate mean
    mean = value_type::Zero(dimension, 1);
 
    for (int i = 0 ; i < numberSamples ; i++)
    {
        mean += samples.block(0, i, mean.rows(), 1);
    }
 
    mean = mean / numberSamples;
 
    samples_type meanadj = samples;
 
    // mean adjust
    for (int s = 0 ; s < numberSamples ; s++)
    {
        meanadj.block(0, s, meanadj.rows(), 1) -= mean;
    }
 
    // calculate covariance
    cov = meanadj * meanadj.transpose() / numberSamples;
}
 
void Gaussian::set(const Gaussian& prototype)
{
    if ((dimension != prototype.getDimensions()))
    {
        throw InvalidDimensionException();
    }
 
    this->mean = prototype.getMean();
    this->cov = prototype.getCovariance();
}
 
void Gaussian::set(const value_type& mean, const covariance_type& cov)
{
    //   isSymmetric(cov);
 
    if ((dimension != -1) && (mean.rows() != dimension))
    {
        throw InvalidDimensionException();
    }
 
    if ((dimension != -1) && (cov.rows() != dimension))
    {
        throw InvalidDimensionException();
    }
 
    if (dimension == -1)
    {
        dimension = cov.rows();
    }
 
    this->mean = mean;
    this->cov = cov;
}
 
void Gaussian::setMean(const value_type& mean)
{
    if ((dimension != -1) && (mean.rows() != dimension))
    {
        throw InvalidDimensionException();
    }
 
    if (dimension == -1)
    {
        dimension = mean.rows();
        this->cov = Eigen::MatrixXd::Identity(dimension, dimension);
    }
 
    this->mean = mean;
}
 
void Gaussian::setCovariance(const covariance_type& cov)
{
    if ((dimension != -1) && (cov.rows() != dimension))
    {
        throw InvalidDimensionException();
    }
 
    if ((dimension != -1) && (cov.cols() != dimension))
    {
        throw InvalidDimensionException();
    }
 
    if ((dimension == -1) && (cov.rows() != cov.cols()))
    {
        throw InvalidDimensionException();
    }
 
    if (dimension == -1)
    {
        dimension = cov.rows();
        this->mean = Eigen::VectorXd::Zero(dimension);
    }
 
    this->cov = cov;
}
 
// *****************************************************************
// content manipulation
// *****************************************************************
void Gaussian::isSymmetric(const covariance_type& matrix)
{
    if (matrix.rows() != matrix.cols())
    {
        throw CovarianceNotSymmetricException();
    }
 
    for (int r = 0 ; r < matrix.rows() ; r++)
    {
        for (int c = 0 ; c < matrix.cols() ; c++)
        {
            if (matrix(r, c) != matrix(c, r))
            {
                throw CovarianceNotSymmetricException();
            }
        }
    }
}