CommonPlacesLearner.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    MemoryX::CommonPlacesLearner
* @author     Alexey Kozlov ( kozlov at kit dot edu)
* @date       2013
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
// memoryx interface
#include <MemoryX/interface/core/EntityBase.h>
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
 
// memoryx helpers
#include <MemoryX/libraries/helpers/GaussianMixtureHelpers/ISDDistance.h>
#include <MemoryX/libraries/helpers/GaussianMixtureHelpers/MahalanobisDistance.h>
#include <MemoryX/libraries/helpers/GaussianMixtureHelpers/RunnallsGMMReducer.h>
#include <MemoryX/libraries/helpers/GaussianMixtureHelpers/RunnallsKLDistance.h>
#include <MemoryX/libraries/helpers/GaussianMixtureHelpers/WestGMMReducer.h>
#include <MemoryX/libraries/helpers/GaussianMixtureHelpers/WilliamsGMMReducer.h>
 
// Object Factories
#include <ArmarXCore/observers/ObserverObjectFactories.h>
 
#include "CommonPlacesLearner.h"
#include "GaussianMixtureAssociationMethod.h"
#include "GaussianMixturePositionFusion.h"
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
 
namespace memoryx
{
    void
    CommonPlacesLearner::onInitComponent()
    {
        usingProxy("LongtermMemory");
        usingProxy("PriorKnowledge");
    }
 
    void
    CommonPlacesLearner::onConnectComponent()
    {
        Ice::CommunicatorPtr ic = getIceManager()->getCommunicator();
 
 
        longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>("LongtermMemory");
        priorKnowledgePrx = getProxy<PriorKnowledgeInterfacePrx>("PriorKnowledge");
 
        std::string segmentName = getProperty<std::string>("LTMSegment").getValue();
        setLTMSegmentName(segmentName);
 
        if (!ltmInstancesSegmentPrx)
        {
            ARMARX_FATAL << "LTM segment not found or has an invalid type: " << segmentName;
        }
 
        const float agingFactor = getProperty<float>("AgingFactor").getValue();
        const float pruningThreshold = getProperty<float>("PruningThreshold").getValue();
 
        const float distanceThreshold = getProperty<float>("MergingThreshold").getValue();
        const std::string distanceName = getProperty<std::string>("MergingDistanceType").getValue();
        GMMDistancePtr distance;
 
        if (distanceName == "Mahalanobis")
        {
            distance.reset(new MahalanobisDistance());
        }
        else if (distanceName == "KL")
        {
            distance.reset(new RunnallsKLDistance());
        }
        else if (distanceName == "ISD")
        {
            distance.reset(new ISDDistance);
        }
        else
        {
            ARMARX_WARNING << "Unknown MergingDistanceType: " << distanceName
                           << ". Will use Kullback-Leibler instead.";
            distance.reset(new RunnallsKLDistance());
        }
 
        assMethod = new GaussianMixtureAssociationMethod(distanceThreshold, distance);
        fusionMethod = new GaussianMixturePositionFusion(agingFactor, pruningThreshold, assMethod);
 
        const std::string reducerName = getProperty<std::string>("GMMReducerAlgorithm").getValue();
        ARMARX_INFO << "Using GMM reduction algorithm: " << reducerName;
 
        if (reducerName == "West")
        {
            gmmReducer.reset(new WestGMMReducer());
        }
        else if (reducerName == "Runnalls")
        {
            gmmReducer.reset(new RunnallsGMMReducer());
        }
        else if (reducerName == "Williams")
        {
            gmmReducer.reset(new WilliamsGMMReducer());
        }
        else
        {
            ARMARX_WARNING << "Unknown GMMReducerAlgorithm: " << reducerName
                           << ". Will use WestReducer instead.";
            gmmReducer.reset(new WestGMMReducer());
        }
    }
 
    void
    CommonPlacesLearner::onExitComponent()
    {
    }
 
    void
    CommonPlacesLearner::setLTMSegmentName(const ::std::string& segmentName,
                                           const ::Ice::Current& c)
    {
        if (!ltmInstancesSegmentPrx || ltmInstancesSegmentPrx->getSegmentName() != segmentName)
        {
            ltmInstancesSegmentPrx =
                longtermMemoryPrx->getCustomInstancesSegment(segmentName, true);
        }
    }
 
    void
    CommonPlacesLearner::setAgingFactor(float factor, const ::Ice::Current& c)
    {
        fusionMethod->setAgingFactor(factor);
    }
 
    void
    CommonPlacesLearner::setMergingThreshold(float threshold, const ::Ice::Current& c)
    {
        assMethod->setThreshold(threshold);
    }
 
    void
    CommonPlacesLearner::learnFromObjectMCA(const ::std::string& objectName,
                                            const MultivariateNormalDistributionBasePtr& posDist,
                                            const ::Ice::Current& c)
    {
        ObjectInstancePtr instance = new ObjectInstance(objectName);
        FloatVector mean = posDist->getMean();
        armarx::FramedPositionPtr posMean = new armarx::FramedPosition();
        posMean->x = mean[0];
        posMean->y = mean[1];
        posMean->z = mean[2];
        posMean->frame = "world";
 
        instance->setPosition(posMean);
        instance->setPositionUncertainty(posDist);
        instance->setClass(objectName, 1.);
        learnFromObject(instance, c);
    }
 
    void
    CommonPlacesLearner::learnFromObject(const ObjectInstanceBasePtr& newObject,
                                         const ::Ice::Current& c)
    {
        const std::string clsName = newObject->getMostProbableClass();
        ObjectInstanceList ltmObjects = ltmInstancesSegmentPrx->getObjectInstancesByClass(clsName);
 
        if (ltmObjects.size() > 0)
        {
            for (ObjectInstanceList::const_iterator it = ltmObjects.begin(); it != ltmObjects.end();
                 ++it)
            {
                // update existing
                EntityBasePtr fusedEntity = fusionMethod->fuseEntity(*it, newObject);
                ltmInstancesSegmentPrx->updateEntity((*it)->getId(), fusedEntity);
                ARMARX_INFO << "Updated existing object: " << newObject->getName();
            }
        }
        else
        {
            // add new
            EntityBasePtr fusedEntity = fusionMethod->initEntity(newObject);
            ltmInstancesSegmentPrx->addEntity(fusedEntity);
            ARMARX_INFO << "Added new object: " << newObject->getName();
        }
    }
 
    void
    CommonPlacesLearner::learnFromSnapshot(const ::std::string& snapshotName,
                                           const ::Ice::Current& c)
    {
        WorkingMemorySnapshotInterfacePrx snapshot =
            longtermMemoryPrx->openWorkingMemorySnapshot(snapshotName);
 
        if (!snapshot)
        {
            ARMARX_ERROR << "Snapshot not found in LTM: " << snapshotName;
            return;
        }
 
        const std::string segmentName = "objectInstances";
        PersistentEntitySegmentBasePrx segInstances = snapshot->getSegment(segmentName);
 
        if (!segInstances)
        {
            ARMARX_ERROR << "Segment not found in snapshot: " << segmentName;
            return;
        }
 
        EntityIdList ids = segInstances->getAllEntityIds();
 
        // sort ids
        struct EntityIdComparator
        {
            static bool
            compare(const std::string& i, const std::string& j)
            {
                return (std::stoi(i) < std::stoi(j));
            }
        };
 
        std::sort(ids.begin(), ids.end(), EntityIdComparator::compare);
 
        for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
        {
            EntityBasePtr snapEntity = segInstances->getEntityById(*it);
            ObjectInstancePtr snapInstance = ObjectInstancePtr::dynamicCast(snapEntity);
            learnFromObject(snapInstance, c);
        }
 
        ARMARX_INFO << "Processing complete! Learned objects: " << ids.size();
    }
 
    GaussianMixtureDistributionBasePtr
    CommonPlacesLearner::getPositionFull(const ::std::string& className, const ::Ice::Current&)
    {
        GaussianMixtureDistributionBasePtr result = GaussianMixtureDistributionBasePtr();
 
        // get subclassses
        NameList clsList;
        clsList.push_back(className);
        getChildClasses(className, clsList);
 
        // get all objects with the class specified or one of its subclasses
        ObjectInstanceList ltmObjects =
            ltmInstancesSegmentPrx->getObjectInstancesByClassList(clsList);
        ObjectInstanceList::const_iterator it = ltmObjects.begin();
 
        if (it != ltmObjects.end())
        {
            ObjectInstancePtr inst = ObjectInstancePtr::dynamicCast(*it);
            result = GaussianMixtureDistribution::FromProbabilityMeasure(
                inst->getPositionAttribute()->getUncertainty());
 
            for (++it; it != ltmObjects.end(); ++it)
            {
                inst = ObjectInstancePtr::dynamicCast(*it);
                ARMARX_VERBOSE_S << "Processing " << inst->getName() << "..." << std::endl;
                result->addComponents(GaussianMixtureDistribution::FromProbabilityMeasure(
                    inst->getPositionAttribute()->getUncertainty()));
            }
        }
 
        return result;
    }
 
    GaussianMixtureDistributionBasePtr
    CommonPlacesLearner::getPositionReducedByComponentCount(const ::std::string& objectName,
                                                            Ice::Int compCount,
                                                            const ::Ice::Current&)
    {
        GaussianMixtureDistributionBasePtr fullGMM = getPositionFull(objectName);
 
        if (fullGMM)
        {
            return gmmReducer->reduceByComponentCount(fullGMM, compCount);
        }
        else
        {
            return GaussianMixtureDistributionBasePtr();
        }
    }
 
    GaussianMixtureDistributionBasePtr
    CommonPlacesLearner::getPositionReducedByMaxDeviation(const ::std::string& objectName,
                                                          Ice::Float maxDeviation,
                                                          DeviationType devType,
                                                          const ::Ice::Current&)
    {
        GaussianMixtureDistributionBasePtr fullGMM = getPositionFull(objectName);
 
        if (fullGMM)
        {
            return gmmReducer->reduceByMaxDeviation(
                fullGMM, maxDeviation, convertDeviationType(devType));
        }
        else
        {
            return GaussianMixtureDistributionBasePtr();
        }
    }
 
    NormalDistributionBasePtr
    CommonPlacesLearner::getPositionAsGaussian(const ::std::string& objectName,
                                               const ::Ice::Current&)
    {
        GaussianMixtureDistributionBasePtr result =
            getPositionReducedByComponentCount(objectName, 1);
 
        if (result)
        {
            return result->getModalComponent().gaussian;
        }
        else
        {
            return NormalDistributionBasePtr();
        }
    }
 
    Cluster3DList
    CommonPlacesLearner::getPositionClustersByComponentCount(const ::std::string& objectName,
                                                             Ice::Int compCount,
                                                             const ::Ice::Current& c)
    {
        GaussianMixtureDistributionBasePtr reducedGMM =
            getPositionReducedByComponentCount(objectName, compCount, c);
        return gmmToClusterList(reducedGMM);
    }
 
    Cluster3DList
    CommonPlacesLearner::getPositionClustersByMaxDeviation(const ::std::string& objectName,
                                                           Ice::Float maxDeviation,
                                                           DeviationType devType,
                                                           const ::Ice::Current& c)
    {
        GaussianMixtureDistributionBasePtr reducedGMM =
            getPositionReducedByMaxDeviation(objectName, maxDeviation, devType, c);
        return gmmToClusterList(reducedGMM);
    }
 
    // the only purpose of this dummy function is to avoid making GaussianMixtureHelpers an Ice component or
    // making it dependent from CommonPlacesLearner
    DeviationMeasure
    CommonPlacesLearner::convertDeviationType(DeviationType devType)
    {
        switch (devType)
        {
            case eDevAABB:
                return eAABB;
 
            case eDevOrientedBBox:
                return eOrientedBBox;
 
            case eDevEqualSphere:
                return eEqualSphere;
 
            default:
                return eAABB;
        }
    }
 
    Cluster3DList
    CommonPlacesLearner::gmmToClusterList(const GaussianMixtureDistributionBasePtr& gmm)
    {
        Cluster3DList result;
 
        if (gmm)
        {
            for (int i = 0; i < gmm->size(); ++i)
            {
                Cluster3D cluster;
                FloatVector compMean = gmm->getComponent(i).gaussian->getMean();
                cluster.center.x = compMean[0];
                cluster.center.y = compMean[1];
                cluster.center.z = compMean[2];
                cluster.weight = gmm->getComponent(i).weight;
                result.push_back(cluster);
            }
        }
 
        // sort cluster with descending weights
        struct Cluster3DComparator
        {
            static bool
            compare(const Cluster3D& i, const Cluster3D& j)
            {
                return (i.weight > j.weight);
            }
        };
 
        std::sort(result.begin(), result.end(), Cluster3DComparator::compare);
 
        return result;
    }
 
    void
    CommonPlacesLearner::getChildClasses(std::string className, NameList& result)
    {
        ObjectClassList children =
            priorKnowledgePrx->getObjectClassesSegment()->getChildClasses(className);
        ARMARX_VERBOSE_S << "Found " << children.size() << " subclasses for " << className
                         << std::endl;
 
        for (ObjectClassList::const_iterator it = children.begin(); it != children.end(); ++it)
        {
            result.push_back((*it)->getName());
        }
    }
} // namespace memoryx