GaussianMixturePositionFusion.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@kit.edu>
* @copyright  2013 Alexey Kozlov
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <string>
#include <ArmarXCore/core/system/ImportExport.h>
#include <MemoryX/libraries/workingmemory/fusion/EntityFusionMethod.h>
 
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/core/entity/ProbabilityMeasures.h>
 
namespace memoryx
{
    /**
     * @class GaussianMixturePositionFusion
     * @ingroup CommonPlacesLearner
     */
    class ARMARXCORE_IMPORT_EXPORT GaussianMixturePositionFusion :
        virtual public EntityFusionMethod
    {
    public:
        /**
         * Creates a new GaussianMixturePositionFusion
         */
        GaussianMixturePositionFusion(float agingFactor, float pruningThreshold, const GaussianMixtureAssociationMethodBasePtr& assMethod) :
            EntityFusionMethod("GaussianMixturePositionFusion"), agingFactor(agingFactor), pruningThreshold(pruningThreshold),
            associationMethod(assMethod)
        {
        }
 
        void setAgingFactor(float factor)
        {
            agingFactor = factor;
        }
 
        /**
        *
        * @param updateEntity entity
        * @return entity with position as GM, ready to be stored in LTM
        */
        EntityBasePtr initEntity(const EntityBasePtr& updateEntity, const ::Ice::Current& = Ice::emptyCurrent) override
        {
            ObjectInstancePtr instance = ObjectInstancePtr::dynamicCast(updateEntity);
 
            if (!instance)
            {
                return EntityBasePtr();    // TODO exception
            }
 
            MultivariateNormalDistributionBasePtr posGaussian = instance->getPositionUncertainty();
 
            if (posGaussian)
            {
                ObjectInstancePtr fusedInstance = ObjectInstancePtr::dynamicCast(instance->ice_clone());
 
                // clear id to allow it to be auto-generated (ids in snapshots are ints, not MongoIds!)
                fusedInstance->setId("");
 
                // convert position uncertainty to GM
                GaussianMixtureDistributionPtr posGM = GaussianMixtureDistribution::FromProbabilityMeasure(posGaussian);
                //                posGM->scaleComponents(0.8);
                fusedInstance->getPositionAttribute()->setValueWithUncertainty(instance->getPositionAttribute()->getValue(), posGM);
                return fusedInstance;
            }
            else
            {
                return updateEntity;
            }
        };
 
        /**
         *
         * @param oldEntity entity before update
         * @param newEntity entity after update
         *
         * @return entity with fusioned position
         */
        EntityBasePtr fuseEntity(const EntityBasePtr& oldEntity, const EntityBasePtr& newEntity, const ::Ice::Current& = Ice::emptyCurrent) override
        {
            ObjectInstancePtr fusedInstance = ObjectInstancePtr::dynamicCast(oldEntity->ice_clone());
            EntityAttributeBasePtr posAttr = fusedInstance->getPositionAttribute();
            GaussianMixtureDistributionPtr posGM = GaussianMixtureDistributionPtr::dynamicCast(posAttr->getUncertainty());
 
            if (posGM)
            {
                ObjectInstancePtr newInstance = ObjectInstancePtr::dynamicCast(newEntity);
 
                // apply aging
                posGM->scaleComponents(agingFactor);
 
                MultivariateNormalDistributionBasePtr newUncertainty = newInstance->getPositionUncertainty();
                GaussianMixtureComponent newComp;
                newComp.gaussian = newUncertainty;
                newComp.weight = 1.;
 
                GaussianMixtureDistributionPtr normGM = GaussianMixtureDistributionPtr::dynamicCast(posGM->clone());
                //                normGM->normalize();
                int associatedCompIndex = associationMethod->getAssociatedComponentIndex(normGM, newComp);
 
                if (associatedCompIndex >= 0)
                {
                    GaussianMixtureComponent associatedComp = posGM->getComponent(associatedCompIndex);
                    GaussianMixtureComponent associatedNormComp = normGM->getComponent(associatedCompIndex);
                    GaussianMixtureComponent newNormComp = normGM->getComponent(posGM->size() - 1);
                    GaussianMixtureComponent fusedComp = fuseGaussianComponents(associatedNormComp, newNormComp);
                    fusedComp.weight = newComp.weight + associatedComp.weight;
                    posGM->setComponent(associatedCompIndex, fusedComp);
                }
                else
                {
                    posGM->addComponent(newComp);
                }
 
                // perform pruning
                if (pruningThreshold > 0)
                {
                    posGM->pruneComponents(pruningThreshold);
                }
 
                // normalize weights
                //                posGM->normalize();
 
                // set mean and uncertainty back to entity
                GaussianMixtureComponent modalComp = posGM->getModalComponent();
 
                if (modalComp.gaussian)
                {
                    NormalDistributionPtr gaussian = NormalDistributionPtr::dynamicCast(modalComp.gaussian);
                    armarx::FramedPositionPtr posMean = new armarx::FramedPosition(Eigen::Vector3f(gaussian->toEigenMean()), fusedInstance->getPosition()->getFrame(), fusedInstance->getPosition()->agent);
                    fusedInstance->setPosition(posMean);
                    fusedInstance->getPositionAttribute()->setValueWithUncertainty(fusedInstance->getPositionAttribute()->getValue(), posGM);
                    return fusedInstance;
                }
                else
                {
                    return EntityBasePtr();    // TODO exception
                }
            }
            else
            {
                return EntityBasePtr();    // TODO exception
            }
        };
 
    private:
        float agingFactor;
        float pruningThreshold;
        GaussianMixtureAssociationMethodBasePtr associationMethod;
 
        GaussianMixtureComponent fuseGaussianComponents(const GaussianMixtureComponent& oldComp, const GaussianMixtureComponent& newComp)
        {
            NormalDistributionPtr oldGaussian = NormalDistributionPtr::dynamicCast(oldComp.gaussian);
            NormalDistributionPtr newGaussian = NormalDistributionPtr::dynamicCast(newComp.gaussian);
 
            const Eigen::VectorXf x1 = oldGaussian->toEigenMean();
            const Eigen::VectorXf x2 = newGaussian->toEigenMean();
            const Eigen::MatrixXf p1 = oldGaussian->toEigenCovariance();
            const Eigen::MatrixXf p2 = newGaussian->toEigenCovariance();
 
            const float w1 = oldComp.weight;
            const float w2 = newComp.weight;
 
            const float k =  1. / (w1 + w2);
            const Eigen::VectorXf d =  x1 - x2;
 
            const Eigen::VectorXf mean = k * (w1 * x1 + w2 * x2);
            const Eigen::MatrixXf cov = k * (w1 * p1 + w2 * p2 + k * w1 * w2 * d * d.transpose());
 
            GaussianMixtureComponent result;
            result.gaussian = new MultivariateNormalDistribution(mean, cov);
            result.weight = w1 + w2;
            return result;
        }
    };
 
    using GaussianMixturePositionFusionPtr = IceInternal::Handle<GaussianMixturePositionFusion>;
}