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/core/entity/ProbabilityMeasures.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/workingmemory/fusion/EntityFusionMethod.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>;
} // namespace memoryx