ProbabilityMeasures.h

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    MemoryX::Core
* @author     Alexey Kozlov ( kozlov at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <MemoryX/interface/core/ProbabilityMeasures.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <Eigen/Core>
 
namespace armarx::VariantType
{
    // variant types
    const armarx::VariantTypeId DiscreteProbability = armarx::Variant::addTypeName("::memoryx::DiscreteProbabilityBase");
    const armarx::VariantTypeId UnivariateNormalDistribution = armarx::Variant::addTypeName("::memoryx::UnivariateNormalDistributionBase");
    const armarx::VariantTypeId IsotropicNormalDistribution = armarx::Variant::addTypeName("::memoryx::IsotropicNormalDistributionBase");
    const armarx::VariantTypeId MultivariateNormalDistribution = armarx::Variant::addTypeName("::memoryx::MultivariateNormalDistributionBase");
    const armarx::VariantTypeId GaussianMixtureDistribution = armarx::Variant::addTypeName("::memoryx::GaussianMixtureDistributionBase");
}
 
namespace memoryx
{
    /**
     * @brief The DiscreteProbability class
     * @ingroup VariantsGrp
     */
    class DiscreteProbability :
        virtual public memoryx::DiscreteProbabilityBase
    {
    public:
        DiscreteProbability();
        DiscreteProbability(::Ice::Float var);
        ~DiscreteProbability() override;
 
        ::Ice::Float getProbability(const ::Ice::Current& = Ice::emptyCurrent) const override;
        void setProbability(::Ice::Float, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        // inherited from VariantDataClass
        Ice::ObjectPtr ice_clone() const override
        {
            return this->clone();
        }
        armarx::VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            return new DiscreteProbability(this->prob);
        }
        std::string output(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            std::stringstream s;
            s << prob;
            return s.str();
        }
        armarx::VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            return armarx::VariantType::DiscreteProbability;
        }
        bool validate(const Ice::Current& c = Ice::emptyCurrent) override
        {
            return true;
        }
 
    public: // serialization
        void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
        void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
 
    };
 
    class NormalDistribution :
        virtual public memoryx::NormalDistributionBase
    {
    public:
        int getDimensions(const ::Ice::Current& = Ice::emptyCurrent) const override
        {
            return dimensions;
        }
        FloatVector getMean(const ::Ice::Current& = Ice::emptyCurrent) const override
        {
            return mean;
        }
        void setMean(const FloatVector& mean, const ::Ice::Current& = Ice::emptyCurrent) override
        {
            this->mean = mean;
        }
        float getCovariance(int row, int col, const ::Ice::Current& = Ice::emptyCurrent) const override = 0;
 
        Eigen::VectorXf toEigenMean() const;
        void fromEigenMean(const Eigen::VectorXf& mean);
 
        virtual Eigen::MatrixXf toEigenCovariance() const = 0;
        virtual void fromEigenCovariance(const Eigen::MatrixXf& cov) = 0;
        virtual float getDensity(const Eigen::VectorXf& p) = 0;
 
        std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
 
    protected:
        NormalDistribution(int dimensions);
        NormalDistribution(const FloatVector& mean);
        NormalDistribution(const Eigen::VectorXf& mean);
        NormalDistribution(const NormalDistribution& other);
 
    protected: // serialization
        void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
        void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
    };
 
    using NormalDistributionPtr = IceInternal::Handle<NormalDistribution>;
 
    /**
     * @brief The UnivariateNormalDistribution class
     * @ingroup VariantsGrp
     */
    class UnivariateNormalDistribution :
        virtual public memoryx::UnivariateNormalDistributionBase,
        virtual public memoryx::NormalDistribution
    {
    public:
        UnivariateNormalDistribution();
        UnivariateNormalDistribution(float mean, float var);
        UnivariateNormalDistribution(const UnivariateNormalDistribution& other);
 
        ::Ice::Float getVariance(const ::Ice::Current& = Ice::emptyCurrent) const override;
        void setVariance(::Ice::Float, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        float getCovariance(int row, int col, const ::Ice::Current& = Ice::emptyCurrent) const override;
        Eigen::MatrixXf toEigenCovariance() const override;
        void fromEigenCovariance(const Eigen::MatrixXf& cov) override;
        float getDensity(const Eigen::VectorXf& p) override
        {
            return 0.;
        }
 
 
        // inherited from VariantDataClass
        Ice::ObjectPtr ice_clone() const override
        {
            return this->clone();
        }
        armarx::VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            return new UnivariateNormalDistribution(*this);
        }
        //        std::string output(const Ice::Current& c = Ice::emptyCurrent) const { std::stringstream s; s << variance; return s.str(); };
        armarx::VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            return armarx::VariantType::UnivariateNormalDistribution;
        }
        bool validate(const Ice::Current& c = Ice::emptyCurrent) override
        {
            return true;
        }
 
    public: // serialization
        void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
        void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
 
    };
 
    using UnivariateNormalDistributionPtr = IceInternal::Handle<UnivariateNormalDistribution>;
 
    /**
     * @brief The IsotropicNormalDistribution class
     * @ingroup VariantsGrp
     */
    class IsotropicNormalDistribution :
        virtual public memoryx::IsotropicNormalDistributionBase,
        virtual public memoryx::NormalDistribution
    {
    public:
        IsotropicNormalDistribution(int dimensions = 0);
        IsotropicNormalDistribution(const FloatVector& mean, const FloatVector& vars);
        IsotropicNormalDistribution(const Eigen::VectorXf& mean, const Eigen::VectorXf& vars);
        IsotropicNormalDistribution(const IsotropicNormalDistribution& other);
 
        ::Ice::Float getVariance(::Ice::Int dim, const ::Ice::Current& = Ice::emptyCurrent) const override;
        void setVariance(::Ice::Int dim, ::Ice::Float, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        float getCovariance(int row, int col, const ::Ice::Current& = Ice::emptyCurrent) const override;
        Eigen::MatrixXf toEigenCovariance() const override;
        void fromEigenCovariance(const Eigen::MatrixXf& cov) override;
        float getDensity(const Eigen::VectorXf& p) override
        {
            return 0.;
        }
 
        // inherited from VariantDataClass
        Ice::ObjectPtr ice_clone() const override
        {
            return this->clone();
        }
        armarx::VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            return new IsotropicNormalDistribution(*this);
        }
        armarx::VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            return armarx::VariantType::IsotropicNormalDistribution;
        }
        bool validate(const Ice::Current& c = Ice::emptyCurrent) override
        {
            return true;
        }
 
    public: // serialization
        void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
        void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
 
    };
 
    using IsotropicNormalDistributionPtr = IceInternal::Handle<IsotropicNormalDistribution>;
 
    class MultivariateNormalDistribution;
    using MultivariateNormalDistributionPtr = IceInternal::Handle<MultivariateNormalDistribution>;
    /**
     * @brief The MultivariateNormalDistribution class
     * @ingroup VariantsGrp
     */
    class MultivariateNormalDistribution :
        virtual public memoryx::MultivariateNormalDistributionBase,
        virtual public memoryx::NormalDistribution
    {
    public:
        MultivariateNormalDistribution(int dimensions = 0);
        MultivariateNormalDistribution(const FloatVector& mean, const FloatVector& vars);
        MultivariateNormalDistribution(const Eigen::VectorXf& mean, const Eigen::MatrixXf& vars);
        MultivariateNormalDistribution(const MultivariateNormalDistribution& other);
 
        /*!
         * \brief Create a distribution with uncertainty of variance in all directions (default: variance = 10000, i.e. standard deviation is 100 mm)
         * \param variance
         * \return
         */
        static MultivariateNormalDistributionPtr CreateDefaultDistribution(float variance = 10000)
        {
            Eigen::Matrix3f covar = variance * Eigen::Matrix3f::Identity();
            Eigen::Vector3f mean = Eigen::Vector3f::Zero();
            return MultivariateNormalDistributionPtr(new MultivariateNormalDistribution(mean, covar));
        }
 
        float getCovariance(int row, int col, const ::Ice::Current& = Ice::emptyCurrent) const override;
        void setCovariance(::Ice::Int row, ::Ice::Int col, ::Ice::Float cov, const ::Ice::Current& = Ice::emptyCurrent) override;
        float getVarianceScalar(const ::Ice::Current& = Ice::emptyCurrent) const override;
 
        Eigen::MatrixXf toEigenCovariance() const override;
        void fromEigenCovariance(const Eigen::MatrixXf& cov) override;
        float getDensity(const Eigen::VectorXf& point) override;
 
        // inherited from VariantDataClass
        Ice::ObjectPtr ice_clone() const override
        {
            return this->clone();
        }
        armarx::VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            return new MultivariateNormalDistribution(*this);
        }
        armarx::VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            return armarx::VariantType::MultivariateNormalDistribution;
        }
        bool validate(const Ice::Current& c = Ice::emptyCurrent) override
        {
            return true;
        }
 
    public: // serialization
        void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
        void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
    };
 
 
 
    class GaussianMixtureDistribution;
 
    using GaussianMixtureDistributionPtr = IceInternal::Handle<GaussianMixtureDistribution>;
    /**
     * @brief The GaussianMixtureDistribution class
     * @ingroup VariantsGrp
     */
    class GaussianMixtureDistribution :
        virtual public GaussianMixtureDistributionBase
    {
    public:
        GaussianMixtureDistribution();
        GaussianMixtureDistribution(const GaussianMixtureDistribution& other);
 
        void clear(const ::Ice::Current& = Ice::emptyCurrent) override;
        ::Ice::Int size(const ::Ice::Current& = Ice::emptyCurrent) const override;
 
        ::Ice::Int getDimensions(const ::Ice::Current& = Ice::emptyCurrent) const override;
        GaussianMixtureComponent getComponent(::Ice::Int index, const ::Ice::Current& = Ice::emptyCurrent) const override;
        GaussianMixtureComponent getModalComponent(const ::Ice::Current& = Ice::emptyCurrent) const override;
        void addComponent(const GaussianMixtureComponent& component, const ::Ice::Current& = Ice::emptyCurrent) override;
        void addComponent(const NormalDistributionBasePtr& gaussian, float weight);
        void addGaussian(const NormalDistributionBasePtr& gaussian, float weight, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setComponent(::Ice::Int index, const GaussianMixtureComponent& component, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeComponent(::Ice::Int index, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * Multiply weight of each GM component with a given constant
         *
         * @param factor constant to multiply with (scaling factor)
         */
        void addComponents(const GaussianMixtureDistributionBasePtr& other, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * Multiply weight of each GM component with a given constant
         *
         * @param factor constant to multiply with (scaling factor)
         */
        void scaleComponents(::Ice::Float factor, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * Remove all components with weights lower than threshold value
         *
         * @param threshold
         */
        void pruneComponents(::Ice::Float threshold, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * Normalize components weights (i.e. sum of weights = 1.0)
         */
        void normalize(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        virtual float getDensity(const Eigen::VectorXf& point);
 
 
        /**
         * Convert or approximate given ProbabilityMeasure to a gaussian mixture. To special cases implemented so far:
         * probMeasure is a gaussian (NormalDistibution) -> create GM with a single component probMeasure
         * probMeasure is a gaussian mixture -> just return probMeasure itself
         *
         * probMeasure probability measure to approximate/convert
         */
        static GaussianMixtureDistributionPtr FromProbabilityMeasure(const ProbabilityMeasureBasePtr& probMeasure);
 
        // inherited from VariantDataClass
        Ice::ObjectPtr ice_clone() const override
        {
            return this->clone();
        }
        armarx::VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            return new GaussianMixtureDistribution(*this);
        }
        armarx::VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            return armarx::VariantType::GaussianMixtureDistribution;
        }
        bool validate(const Ice::Current& c = Ice::emptyCurrent) override
        {
            return true;
        }
 
        std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
 
    public: // serialization
        void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
        void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
 
    private:
        float getTotalWeight() const;
    };
 
}