ProbabilityMeasures.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    WorkingMemory
* @author     Alexey Kozlov ( kozlov at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "ProbabilityMeasures.h"
 
#include <ArmarXCore/observers/AbstractObjectSerializer.h>
#include <Eigen/LU>
 
namespace memoryx
{
    /*
     *  DiscreteProbability class
     *
     */
    DiscreteProbability::DiscreteProbability(): DiscreteProbabilityBase::DiscreteProbabilityBase(0.) {}
 
    DiscreteProbability::DiscreteProbability(::Ice::Float prob): DiscreteProbabilityBase::DiscreteProbabilityBase(prob) {}
 
    DiscreteProbability::~DiscreteProbability() {}
 
    Ice::Float DiscreteProbability::getProbability(const Ice::Current& c) const
    {
        return prob;
    }
 
    void DiscreteProbability::setProbability(Ice::Float prob, const Ice::Current&)
    {
        this->prob = prob;
    }
 
    void DiscreteProbability::serialize(const armarx::ObjectSerializerBasePtr& serializer, const Ice::Current&) const
    {
        armarx::AbstractObjectSerializerPtr obj = armarx::AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        obj->setFloat("prob", prob);
    }
 
    void DiscreteProbability::deserialize(const armarx::ObjectSerializerBasePtr& serializer, const Ice::Current&)
    {
        armarx::AbstractObjectSerializerPtr obj = armarx::AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        prob = obj->getFloat("prob");
    }
 
 
    /*
     *  NormalDistribution class
     *
     */
    NormalDistribution::NormalDistribution(int dimensions)
    {
        this->dimensions = dimensions;
        this->mean.resize(dimensions);
    }
 
    NormalDistribution::NormalDistribution(const FloatVector& mean):
        NormalDistributionBase(mean.size(), mean)
    {
    }
 
    NormalDistribution::NormalDistribution(const Eigen::VectorXf& mean)
    {
        this->dimensions = mean.rows();
        this->mean.resize(dimensions);
        fromEigenMean(mean);
    }
 
    NormalDistribution::NormalDistribution(const NormalDistribution& other):
        IceUtil::Shared(other),
        NormalDistributionBase()
        //     NormalDistributionBase(other)
    {
        this->dimensions = other.dimensions;
        this->mean = other.mean;
    }
 
    Eigen::VectorXf NormalDistribution::toEigenMean() const
    {
        Eigen::VectorXf result = Eigen::Map<const Eigen::VectorXf>(this->mean.data(), this->dimensions);
        return result;
    }
 
    void NormalDistribution::fromEigenMean(const Eigen::VectorXf& mean)
    {
        if (this->dimensions == mean.rows())
        {
            Eigen::Map<Eigen::VectorXf>(this->mean.data(), this->dimensions) = mean;
        }
        else
        {
            throw InvalidDimensionException();
        }
    }
 
    std::string NormalDistribution::output(const Ice::Current& c) const
    {
        std::stringstream s;
        Eigen::MatrixXf cov = this->toEigenCovariance();
        if (dimensions > 0)
        {
            s << "Standard deviation(mm): " << pow(cov.determinant(), 0.5 / dimensions);
        }
 
        s << " mean: (";
 
        for (size_t i = 0; i < mean.size(); ++i)
        {
            s << mean[i] << ((i != mean.size() - 1) ? ", " : ")  ");
        }
 
        s << "cov: (";
 
        for (int i = 0; i < dimensions; ++i)
            for (int j = 0; j < dimensions; ++j)
            {
                s << getCovariance(i, j) << ((i != dimensions - 1 || j != dimensions - 1) ? ", " : ")");
            }
 
        return s.str();
    }
 
    void NormalDistribution::serialize(const armarx::ObjectSerializerBasePtr& serializer, const Ice::Current&) const
    {
        armarx::AbstractObjectSerializerPtr obj = armarx::AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        obj->setFloatArray("mean", mean);
    }
 
    void NormalDistribution::deserialize(const armarx::ObjectSerializerBasePtr& serializer, const Ice::Current&)
    {
        armarx::AbstractObjectSerializerPtr obj = armarx::AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        obj->getFloatArray("mean", mean);
        dimensions = mean.size();
    }
 
 
    /*
     *  UnivariateNormalDistribution class
     *
     */
    UnivariateNormalDistribution::UnivariateNormalDistribution() :
        UnivariateNormalDistributionBase::UnivariateNormalDistributionBase(),
        NormalDistribution::NormalDistribution(1)
    {
        variance = 0.0f;
    }
 
    UnivariateNormalDistribution::UnivariateNormalDistribution(float mean, float var) :
        UnivariateNormalDistributionBase::UnivariateNormalDistributionBase(),
        NormalDistribution::NormalDistribution(1)
    {
        variance = var;
        this->mean.push_back(mean);
    }
 
    UnivariateNormalDistribution::UnivariateNormalDistribution(const UnivariateNormalDistribution& other):
        IceUtil::Shared(other),
        NormalDistributionBase(/*other*/),
        UnivariateNormalDistributionBase(/*other*/),
        NormalDistribution(other)
    {
        variance = other.variance;
    }
 
    Ice::Float UnivariateNormalDistribution::getVariance(const Ice::Current& c) const
    {
        return variance;
    }
 
    void UnivariateNormalDistribution::setVariance(Ice::Float var, const Ice::Current&)
    {
        variance = var;
    }
 
    float UnivariateNormalDistribution::getCovariance(int row, int col, const ::Ice::Current&) const
    {
        if ((row == 0) && (col == 0))
        {
            return variance;
        }
 
        return 0;
    }
 
    Eigen::MatrixXf UnivariateNormalDistribution::toEigenCovariance() const
    {
        Eigen::MatrixXf result(1, 1);
        result << this->variance;
        return result;
    }
 
    void UnivariateNormalDistribution::fromEigenCovariance(const Eigen::MatrixXf& cov)
    {
        if (cov.rows() == 1 && cov.cols() == 1)
        {
            this->variance = cov(1, 1);
        }
        else
        {
            throw InvalidDimensionException();
        }
    }
 
    void UnivariateNormalDistribution::serialize(const armarx::ObjectSerializerBasePtr& serializer, const Ice::Current& c) const
    {
        armarx::AbstractObjectSerializerPtr obj = armarx::AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        NormalDistribution::serialize(obj, c);
        obj->setFloat("var", variance);
    }
 
    void UnivariateNormalDistribution::deserialize(const armarx::ObjectSerializerBasePtr& serializer, const Ice::Current& c)
    {
        armarx::AbstractObjectSerializerPtr obj = armarx::AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        NormalDistribution::deserialize(obj, c);
        variance = obj->getFloat("var");
    }
 
    /*
     *  IsotropicNormalDistribution class
     *
     */
    IsotropicNormalDistribution::IsotropicNormalDistribution(int dimensions) :
        NormalDistribution::NormalDistribution(dimensions)
    {
        this->varVector.resize(dimensions);
    }
 
    IsotropicNormalDistribution::IsotropicNormalDistribution(const FloatVector& mean, const FloatVector& vars) :
        NormalDistribution::NormalDistribution(mean)
    {
        this->varVector = vars;
    }
 
    IsotropicNormalDistribution::IsotropicNormalDistribution(const Eigen::VectorXf& mean, const Eigen::VectorXf& vars):
        NormalDistribution::NormalDistribution(mean)
    {
        this->varVector.resize(dimensions);
        fromEigenCovariance(vars);
    }
 
    IsotropicNormalDistribution::IsotropicNormalDistribution(const IsotropicNormalDistribution& other):
        IceUtil::Shared(other),
        NormalDistributionBase(other),
        IsotropicNormalDistributionBase(other),
        NormalDistribution(other)
    {
        this->varVector.assign(other.varVector.begin(), other.varVector.end());
    }
 
 
    Ice::Float IsotropicNormalDistribution::getVariance(Ice::Int dim, const Ice::Current&) const
    {
        return (size_t) dim < varVector.size() ? varVector[dim] : -1.;
    }
 
    void IsotropicNormalDistribution::setVariance(Ice::Int dim, Ice::Float var, const Ice::Current&)
    {
        if ((size_t) dim < varVector.size())
        {
            varVector[dim] = var;
        }
    }
 
    float IsotropicNormalDistribution::getCovariance(int row, int col, const ::Ice::Current&) const
    {
        if (row != col)
        {
            return 0.;
        }
 
        return varVector[row];
    }
 
    Eigen::MatrixXf IsotropicNormalDistribution::toEigenCovariance() const
    {
        Eigen::MatrixXf result(dimensions, dimensions);
 
        for (int i = 0; i < dimensions; ++i)
        {
            result(i, i) = varVector[i];
        }
 
        return result;
    }
 
    void IsotropicNormalDistribution::fromEigenCovariance(const Eigen::MatrixXf& cov)
    {
        if (cov.rows() == 1 && cov.cols() == dimensions)
        {
            for (int i = 0; i < dimensions; ++i)
            {
                varVector[i] = cov(0, i);
            }
        }
        else if (cov.rows() == dimensions && cov.cols() == dimensions)
        {
            for (int i = 0; i < dimensions; ++i)
            {
                varVector[i] = cov(i, i);
            }
        }
        else
        {
            throw InvalidDimensionException();
        }
    }
 
 
    void IsotropicNormalDistribution::serialize(const armarx::ObjectSerializerBasePtr& serializer, const Ice::Current& c) const
    {
        armarx::AbstractObjectSerializerPtr obj = armarx::AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        NormalDistribution::serialize(obj, c);
        obj->setFloatArray("var", varVector);
    }
 
    void IsotropicNormalDistribution::deserialize(const armarx::ObjectSerializerBasePtr& serializer, const Ice::Current& c)
    {
        armarx::AbstractObjectSerializerPtr obj = armarx::AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        NormalDistribution::deserialize(obj, c);
        obj->getFloatArray("var", varVector);
    }
 
 
    /*
     *  MultivariateNormalDistribution class
     *
     */
    MultivariateNormalDistribution::MultivariateNormalDistribution(int dimensions) :
        NormalDistribution::NormalDistribution(dimensions)
    {
        this->covMatrix.resize(dimensions * dimensions);
    }
 
    MultivariateNormalDistribution::MultivariateNormalDistribution(const FloatVector& mean, const FloatVector& vars) :
        NormalDistribution::NormalDistribution(mean)
    {
        this->covMatrix = vars;
    }
 
    MultivariateNormalDistribution::MultivariateNormalDistribution(const Eigen::VectorXf& mean, const Eigen::MatrixXf& vars):
        NormalDistribution::NormalDistribution(mean)
    {
        this->covMatrix.resize(dimensions * dimensions);
        fromEigenCovariance(vars);
    }
 
    MultivariateNormalDistribution::MultivariateNormalDistribution(const MultivariateNormalDistribution& other):
        IceUtil::Shared(other),
        NormalDistributionBase(/*other*/),
        MultivariateNormalDistributionBase(/*other*/),
        NormalDistribution(other)
    {
        this->covMatrix = other.covMatrix;
    }
 
 
    Ice::Float MultivariateNormalDistribution::getCovariance(int row, int col, const Ice::Current&) const
    {
        const int index = row * dimensions + col;
 
        if (index >= 0 && (size_t) index < covMatrix.size())
        {
            return covMatrix[index];
        }
        throw armarx::IndexOutOfBoundsException();
    }
 
    void MultivariateNormalDistribution::setCovariance(Ice::Int row, Ice::Int col, Ice::Float cov, const Ice::Current&)
    {
        const int index = row * dimensions + col;
 
        if (index >= 0 && (size_t) index < covMatrix.size())
        {
            covMatrix[index] = cov;
        }
        else
        {
            throw armarx::IndexOutOfBoundsException();
        }
    }
 
    Ice::Float MultivariateNormalDistribution::getVarianceScalar(const Ice::Current&) const
    {
        if (dimensions > 0)
        {
            Eigen::MatrixXf cov = toEigenCovariance();
            return pow(cov.determinant(), 0.5 / dimensions);
        }
        else
        {
            return 0;
        }
    }
 
    Eigen::MatrixXf MultivariateNormalDistribution::toEigenCovariance() const
    {
        Eigen::MatrixXf result = Eigen::Map<const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> >(this->covMatrix.data(), this->dimensions, this->dimensions);
        return result;
    }
 
    void MultivariateNormalDistribution::fromEigenCovariance(const Eigen::MatrixXf& cov)
    {
        if (cov.rows() == this->dimensions && cov.cols() == this->dimensions)
        {
            Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> >(this->covMatrix.data(), this->dimensions, this->dimensions) = cov;
        }
        else
        {
            throw InvalidDimensionException();
        }
    }
 
    float MultivariateNormalDistribution::getDensity(const Eigen::VectorXf& point)
    {
        if (point.rows() != dimensions)
        {
            throw InvalidDimensionException();
        }
 
        Eigen::MatrixXf cov = toEigenCovariance();
        Eigen::VectorXf m = toEigenMean();
        Eigen::Matrix<float, 1, 1> inner =  -0.5 * (point - m).transpose() * cov.inverse() * (point - m);
        float e = expf(inner(0, 0));
 
        return pow(2 * M_PI, -dimensions / 2.0f) * pow(cov.determinant(), -0.5) * e;
    }
 
    void MultivariateNormalDistribution::serialize(const armarx::ObjectSerializerBasePtr& serializer, const Ice::Current& c) const
    {
        armarx::AbstractObjectSerializerPtr obj = armarx::AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        NormalDistribution::serialize(obj, c);
        obj->setFloatArray("cov", covMatrix);
    }
 
    void MultivariateNormalDistribution::deserialize(const armarx::ObjectSerializerBasePtr& serializer, const Ice::Current& c)
    {
        armarx::AbstractObjectSerializerPtr obj = armarx::AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        NormalDistribution::deserialize(obj, c);
        obj->getFloatArray("cov", covMatrix);
    }
 
    /**
     *  GaussianMixtureDistribution implementation
     */
    GaussianMixtureDistribution::GaussianMixtureDistribution()
    {
        this->dimensions = 0;
    }
 
    GaussianMixtureDistribution::GaussianMixtureDistribution(const GaussianMixtureDistribution& other):
        IceUtil::Shared(other),
        GaussianMixtureDistributionBase(other)
    {
        clear();
 
        for (GaussianMixtureComponentList::const_iterator it = other.components.begin(); it != other.components.end(); ++it)
        {
            GaussianMixtureComponent comp;
            comp.gaussian = NormalDistributionBasePtr::dynamicCast(it->gaussian->clone());
            comp.weight = it->weight;
            addComponent(comp);
        }
    }
 
    ::Ice::Int GaussianMixtureDistribution::getDimensions(const ::Ice::Current&) const
    {
        return dimensions;
    }
 
    GaussianMixtureComponent GaussianMixtureDistribution::getComponent(::Ice::Int index, const ::Ice::Current&) const
    {
        if (index >= 0 && (size_t) index < components.size())
        {
            return components[index];
        }
        throw armarx::IndexOutOfBoundsException();
    }
 
    GaussianMixtureComponent GaussianMixtureDistribution::getModalComponent(const ::Ice::Current&) const
    {
        if (components.empty())
        {
            return GaussianMixtureComponent();
        }
 
        GaussianMixtureComponentList::const_iterator res = components.end();
 
        for (GaussianMixtureComponentList::const_iterator it = components.begin(); it != components.end(); ++it)
            if (res == components.end() || it->weight > res->weight)
            {
                res = it;
            }
 
        return *res;
    }
 
    void GaussianMixtureDistribution::addComponent(const GaussianMixtureComponent& component, const ::Ice::Current&)
    {
        if (components.empty())
        {
            dimensions = component.gaussian->getDimensions();
        }
        else if (component.gaussian->getDimensions() != dimensions)
        {
            throw InvalidDimensionException();
        }
 
        components.push_back(component);
    }
 
    void GaussianMixtureDistribution::addComponent(const NormalDistributionBasePtr& gaussian, float weight)
    {
        GaussianMixtureComponent comp;
        comp.gaussian = gaussian;
        comp.weight = weight;
        addComponent(comp);
    }
 
    void GaussianMixtureDistribution::addGaussian(const NormalDistributionBasePtr& gaussian, float weight, const ::Ice::Current& c)
    {
        addComponent(gaussian, weight);
    }
 
    void GaussianMixtureDistribution::setComponent(::Ice::Int index, const GaussianMixtureComponent& component, const ::Ice::Current&)
    {
        if (index < 0 && (size_t) index >= components.size())
        {
            throw armarx::IndexOutOfBoundsException();
        }
        else if (component.gaussian->getDimensions() != dimensions)
        {
            throw InvalidDimensionException();
        }
        else
        {
            components[index] = component;
        }
    }
 
    void GaussianMixtureDistribution::removeComponent(::Ice::Int index, const ::Ice::Current&)
    {
        if (index >= 0 && (size_t) index < components.size())
        {
            components.erase(components.begin() + index);
        }
        else
        {
            throw armarx::IndexOutOfBoundsException();
        }
    }
 
    void GaussianMixtureDistribution::clear(const ::Ice::Current&)
    {
        components.clear();
    }
 
    ::Ice::Int GaussianMixtureDistribution::size(const ::Ice::Current&) const
    {
        return (::Ice::Int) components.size();
    }
 
    float GaussianMixtureDistribution::getTotalWeight() const
    {
        float result = 0.;
 
        for (GaussianMixtureComponentList::const_iterator it = components.begin(); it != components.end(); ++it)
        {
            result += it->weight;
        }
 
        return result;
    }
 
    void GaussianMixtureDistribution::addComponents(const GaussianMixtureDistributionBasePtr& other, const ::Ice::Current& c)
    {
        for (int i = 0; i < other->size(); ++i)
        {
            addComponent(other->getComponent(i));
        }
    }
 
    void GaussianMixtureDistribution::scaleComponents(::Ice::Float factor, const ::Ice::Current&)
    {
        for (GaussianMixtureComponentList::iterator it = components.begin(); it != components.end(); ++it)
        {
            it->weight *= factor;
        }
    }
 
    void GaussianMixtureDistribution::pruneComponents(::Ice::Float threshold, const ::Ice::Current&)
    {
        const float totalWeight = getTotalWeight();
        GaussianMixtureComponentList::iterator it = components.begin();
 
        while (it != components.end())
        {
            if (it->weight / totalWeight < threshold)
            {
                it = components.erase(it);
            }
            else
            {
                ++it;
            }
        }
    }
 
    void GaussianMixtureDistribution::normalize(const ::Ice::Current&)
    {
        const float totalWeight = getTotalWeight();
 
        for (GaussianMixtureComponentList::iterator it = components.begin(); it != components.end(); ++it)
        {
            it->weight /= totalWeight;
        }
    }
 
    float GaussianMixtureDistribution::getDensity(const Eigen::VectorXf& point)
    {
        const float totalWeight = getTotalWeight();
        float result = 0.f;
 
        for (GaussianMixtureComponentList::iterator it = components.begin(); it != components.end(); ++it)
        {
            const NormalDistributionPtr gaussian = NormalDistributionPtr::dynamicCast(it->gaussian);
            result += gaussian->getDensity(point) * it->weight / totalWeight;
        }
 
        return result;
    }
 
    GaussianMixtureDistributionPtr GaussianMixtureDistribution::FromProbabilityMeasure(const ProbabilityMeasureBasePtr& probMeasure)
    {
        // check if argument is already GM - in that case, we just need to cast
        GaussianMixtureDistributionPtr gm = GaussianMixtureDistributionPtr::dynamicCast(probMeasure);
 
        if (gm)
        {
            return gm;
        }
 
        // check if argument is a guassian
        NormalDistributionBasePtr gaussian = NormalDistributionBasePtr::dynamicCast(probMeasure);
 
        if (gaussian)
        {
            // if so, create GM with a single component
            GaussianMixtureDistributionPtr result = new GaussianMixtureDistribution();
            result->addComponent(gaussian, 1.);
            return result;
        }
 
        // unable to convert -> return empty pointer
        return GaussianMixtureDistributionPtr();
    }
 
    std::string GaussianMixtureDistribution::output(const Ice::Current& c) const
    {
        std::stringstream s;
 
        for (GaussianMixtureComponentList::const_iterator it = components.begin(); it != components.end(); ++it)
        {
            s << "{ gaussian: " << it->gaussian->output() << "weight: " << it->weight << "} \n";
        }
 
        return s.str();
    }
 
 
    void GaussianMixtureDistribution::serialize(const armarx::ObjectSerializerBasePtr& serializer, const Ice::Current& c) const
    {
        armarx::AbstractObjectSerializerPtr obj = armarx::AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        obj->setElementType(armarx::ElementTypes::eArray);
 
        for (GaussianMixtureComponentList::const_iterator it = components.begin(); it != components.end(); ++it)
        {
            armarx::AbstractObjectSerializerPtr compValue = armarx::AbstractObjectSerializerPtr::dynamicCast(serializer)->createElement();
            armarx::VariantPtr gaussianVar = new armarx::Variant(armarx::VariantDataClassPtr::dynamicCast(it->gaussian));
            compValue->setVariant("gaussian", gaussianVar);
            compValue->setFloat("weight", it->weight);
            obj->append(compValue);
        }
    }
 
    void GaussianMixtureDistribution::deserialize(const armarx::ObjectSerializerBasePtr& serializer, const Ice::Current& c)
    {
        armarx::AbstractObjectSerializerPtr obj = armarx::AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        clear();
 
        for (unsigned int i = 0; i < obj->size(); ++i)
        {
            const armarx::AbstractObjectSerializerPtr compValue = obj->getElement(i);
 
            GaussianMixtureComponent comp;
            armarx::VariantPtr gaussianVar = compValue->getVariant("gaussian");
            comp.gaussian = gaussianVar->getClass<NormalDistributionBase>();
            comp.weight = compValue->getFloat("weight");
            addComponent(comp);
        }
    }
}