GMM.h

Go to the documentation of this file.

#pragma once

#include <boost/random.hpp>
#include <time.h>
#include "MultidimensionalGaussian.h"
#include <Eigen/Core>

namespace ASR
{

class GMM
{
private:
    std::vector<ASR::MultidimensionalGaussianPtr> models;
    std::vector<float> weights;
    std::vector<float> histogramm;
    boost::mt19937 rng;


    float gen_random_float(float min, float max)
    {
        boost::uniform_real<float> u(min, max);
        boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > gen(rng, u);
        return gen();
    }

public:
    GMM()
    {
        // Do some initialzations
        rng.seed(time(0));
        histogramm.push_back(0.0f);
    }

    void addGaussianModel(float weight, std::vector<float> mean, MatrixXf covariance)
    {
        //ROS_INFO("Adding gaussian to gmm. weight=%.2f", weight);

        weights.push_back(weight);
        models.push_back(boost::shared_ptr<ASR::MultidimensionalGaussian>(new ASR::MultidimensionalGaussian(mean, covariance)));
        histogramm.push_back(histogramm.at(histogramm.size()-1) + weight );
    }


    void sampleRandomValues(std::vector<float> &x)
    {
        float sum = histogramm.at(histogramm.size()-1);

        if (sum < .99f || sum > 1.01f)
        {
            ROS_INFO("Sum of the gaussian weights not 1. Please normailze the weight of the distibutions!");
            return;
        }

        // Get one vector of samples
        // The models are sampled with the stored weights
        // Get xi € [0,1)
        float xi = gen_random_float(0,1);

        unsigned int index = 0;

        // find the index in the histogeramm at which data > xi
        while (histogramm.at(index) < xi)
        {
            index++;
        }

        // Decrease index by one to get the last element which is smaller than xi
        index--;

        //ROS_INFO("Sampling from model at(%i), xi=%.2f", index, xi);

        if(index > models.size()) index = models.size();

        // Now sample from the gaussian at counter
        models.at(index)->sampleRandomValues(x);
    }

    ASR::MultidimensionalGaussianPtr getModel(unsigned int index)
    {
        ROS_ASSERT(index < models.size());
        return models.at(index);
    }
};

typedef boost::shared_ptr<GMM> GMM_Ptr;

}