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 <Eigen/Core>
 
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <MemoryX/interface/core/ProbabilityMeasures.h>
 
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 armarx::VariantType
 
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;
    };
 
} // namespace memoryx