MultidimensionalGaussian.h

Go to the documentation of this file.

#pragma once

#include <boost/shared_ptr.hpp>
#include <iostream>
#include <iomanip>
#include <string>
#include <map>
#include <random>
#include <cmath>
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include "Gaussian.h"

using namespace Eigen;
using namespace std;

namespace ASR
{

    class MultidimensionalGaussian
    {
    private:
        std::vector<float> means;
        MatrixXf covariance;
        MatrixXf L;
        GaussianPtr normal_dist;
        int dimension;

    public:
        MultidimensionalGaussian(std::vector<float> means, MatrixXf covariance)
        {
            this->means = means;
            this->covariance = covariance;
            this->dimension = means.size();

            normal_dist = GaussianPtr(new ASR::Gaussian(0,1));

            // Calculate Cholesky decomposition
            LLT<MatrixXf> lltOfA(covariance); // compute the Cholesky decomposition of A
            this->L = lltOfA.matrixL(); // retrieve factor L  in the decomposition

            // For debugging only
            //test();
        }

        void sampleRandomValues(std::vector<float> &x)
        {
            //ROS_ASSERT(x != nullptr);

            // 1. A = L * L.transpose()
            // 2. Erzeuge N-dim Vector mit standartnormalverteilten Zufallszahlen
            std::vector<float> z;
            normal_dist->sampleRandomValues(z, dimension);
            // 3. transform x = means + L * random
            VectorXf Z = initVectorXf(z);
            VectorXf mu = initVectorXf(means);
            VectorXf X = mu + L * Z;

            // Get the valus of the VectorXf and convert them to a std::vector
            float *t1 = X.data();
            for(int i=0; i < dimension; i++)
            {
                x.push_back(*(t1 + i));
            }
        }

        std::vector<float> getMean() {return means;}

    private:
        VectorXf initVectorXf(std::vector<float> values)
        {
            VectorXf x(values.size());
            for(unsigned int i=0; i<values.size();i++)
            {
                x(i) = values.at(i);
            }
            return x;
        }


        MatrixXf initMatrixXf(std::vector<float> values, unsigned int rows, unsigned int cols)
        {
            assert (rows > 0);
            assert (cols > 0);

            MatrixXf x(rows, cols);
            for(unsigned int i=0; i < rows;i++)
            {
                for(unsigned int j=0; j < cols; j++)
                {
                    x(i,j) = values.at(i*cols + j);
                }
            }
            return x;
        }

        void initVectorFromCSVString(std::vector<float> &x, std::string csv)
        {
            std::stringstream ss(csv);
            float i;
            while (ss >> i)
            {
                x.push_back(i);

                if (ss.peek() == ',' || ss.peek() == ' ')
                    ss.ignore();
            }
        }


        void test()
        {
            cout << "The means are " << endl << initVectorXf(this->means) << endl;
            cout << "The matrix A is" << endl << covariance << endl;
            cout << "The Cholesky factor L is" << endl << L << endl;
            cout << "To check this, let us compute L * L.transpose()" << endl;
            cout << L * L.transpose() << endl;
            cout << "This should equal the matrix A" << endl;
        }

    };

    typedef boost::shared_ptr<MultidimensionalGaussian> MultidimensionalGaussianPtr;

}