ObjectInstanceMemorySegment.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::WorkingMemory
* @author     Kai Welke ( welke at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "ObjectInstanceMemorySegment.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/EarlyVisionConverters.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/NoMotionFilter.h>
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
 
 
#include <cfloat>
 
namespace memoryx
{
    ObjectInstanceMemorySegment::ObjectInstanceMemorySegment(float matchThreshold, bool matchByClass):
        WorkingMemoryEntitySegment<ObjectInstance>::WorkingMemoryEntitySegment(),
        ObjectInstanceMemorySegmentBase::ObjectInstanceMemorySegmentBase(),
        matchThreshold(matchThreshold),
        matchByClass(matchByClass)
    {
    }
 
    ObjectInstanceBasePtr ObjectInstanceMemorySegment::getObjectInstanceById(const std::string& id, const Ice::Current&) const
    {
        ObjectInstanceBasePtr res = ObjectInstanceBasePtr::dynamicCast(getEntityById(id));
 
        if (!res)
        {
            ARMARX_WARNING_S << "Entity with id " << id << " is not of type ObjectInstance!" << std::endl;
        }
 
        return res;
    }
 
    ObjectInstanceBasePtr ObjectInstanceMemorySegment::getObjectInstanceByName(const std::string& name, const Ice::Current&) const
    {
        ObjectInstanceBasePtr res = ObjectInstanceBasePtr::dynamicCast(getEntityByName(name));
 
        if (!res)
        {
            ARMARX_WARNING_S << "Entity with name " << name << " is not of type ObjectInstance!" << std::endl;
        }
 
        return res;
    }
 
    ObjectInstanceList ObjectInstanceMemorySegment::getObjectInstancesByClass(const std::string& className, const Ice::Current& c) const
    {
        NameList classList;
        classList.push_back(className);
        return getObjectInstancesByClassList(classList, c);
    }
 
    ObjectInstanceList ObjectInstanceMemorySegment::getObjectInstancesByClassList(const NameList& classList, const ::Ice::Current& c) const
    {
        ObjectInstanceList result;
        ScopedSharedLockPtr lock(getReadLock(c));
 
        for (IdEntityMap::const_iterator it = entityMap.begin(); it != entityMap.end(); ++it)
        {
            ObjectInstanceBasePtr inst = ObjectInstanceBasePtr::dynamicCast(it->second);
            ClassProbabilityMap instClasses = inst->getClasses();
 
            for (NameList::const_iterator itCls = classList.begin(); itCls != classList.end(); ++itCls)
            {
                if (instClasses.count(*itCls))
                {
                    result.push_back(inst);
                    break;
                }
            }
        }
 
        return result;
    }
 
    ObjectInstanceBasePtr ObjectInstanceMemorySegment::getCorrespondingObjectInstance(const ObjectInstanceBasePtr& objectInstance, const ::Ice::Current& c) const
    {
        // setup filter
        NoMotionFilter filter;
        Eigen::MatrixXd i(3, 3);
        i.setIdentity();
        ARMARX_DEBUG << "Position: " << objectInstance->getPositionBase()->output() << " mean: " << objectInstance->getPositionUncertainty()->getMean();
        Gaussian measurement = EarlyVisionConverters::convertToGaussian(objectInstance->getPositionUncertainty());
        filter.setMeasurementModel(i, measurement);
        ARMARX_DEBUG_S << "measurement uncertainty: " << measurement.getCovariance().determinant();
 
        // loop through all entities
        double maxProb = -FLT_MAX;
        std::string matchingId = "";
        std::string instanceName = objectInstance->getName();
        const std::string mostProbableClass = objectInstance->getMostProbableClass();
 
        //        ARMARX_DEBUG_S << "entityMap.size() = " << entityMap.size();
        {
            ScopedSharedLockPtr lock(getReadLock(c));
 
            for (IdEntityMap::const_iterator it = entityMap.begin(); it != entityMap.end(); ++it)
            {
                ObjectInstancePtr current = ObjectInstancePtr::dynamicCast(it->second);
 
                //            ARMARX_DEBUG_S << "current: " << current->getName();
 
                // TODO account for multiple classes?
                if (!matchByClass && current->getName() != instanceName)
                {
                    continue;
                }
                else if (matchByClass && mostProbableClass != current->getMostProbableClass())
                {
                    continue;
                }
 
 
                MultivariateNormalDistributionBasePtr uncertainty = current->getPositionUncertainty();
 
                if (!uncertainty)
                {
                    ARMARX_WARNING_S << "current->getPositionUncertainty() of object " << current->getName() << " is nullptr! Creating default uncertainty";
                    uncertainty = MultivariateNormalDistribution::CreateDefaultDistribution(1000000);
                    current->setPositionUncertainty(uncertainty);
                }
 
 
                Gaussian believe = EarlyVisionConverters::convertToGaussian(uncertainty);
 
                ARMARX_DEBUG_S << "measurement cov: " << measurement.getCovariance();
                ARMARX_DEBUG_S << "measurement mean: " << measurement.getMean();
                ARMARX_DEBUG_S << "believe cov: " << believe.getCovariance();
                ARMARX_DEBUG_S << "believe mean: " << believe.getMean();
 
                ARMARX_DEBUG_S << "uncertainty: " << believe.getCovariance().determinant();
 
                // update with filter
                Gaussian posterior = filter.update(believe, measurement.getMean());
 
                ARMARX_DEBUG_S << "posterior uncertainty: " << posterior.getCovariance().determinant();
 
                // check posterior
                double prob = posterior.evaluate(measurement.getMean());
 
                ARMARX_DEBUG_S << "prob: " << prob;
 
                if (prob > maxProb)
                {
                    matchingId = current->getId();
                    maxProb = prob;
                }
 
            }
        }
        if (matchingId.empty())
        {
            ARMARX_INFO << deactivateSpam(5, objectInstance->getName()) << "Could not find any match for " << objectInstance->getName() << " with most probable class: " << objectInstance->getName();
            return 0;
        }
        // check threshold
        if (maxProb >= matchThreshold)
            //        if (maxProb >= 0)
        {
            return ObjectInstanceBasePtr::dynamicCast(getEntityById(matchingId));
        }
        else
        {
            ARMARX_IMPORTANT_S << deactivateSpam(5, objectInstance->getName()) << "Best match probability below threshold for object " << objectInstance->getName() << " " << VAROUT(maxProb) << VAROUT(matchThreshold);
 
            return 0;
        }
    }
 
 
 
    void ObjectInstanceMemorySegment::setNewMotionModel(const std::string& entityId, const MotionModelInterfacePtr& newMotionModel, const Ice::Current& c)
    {
        auto getObjectInstanceByIdUnsafe = [this](const std::string & id)
        {
            ObjectInstanceBasePtr res = ObjectInstanceBasePtr::dynamicCast(getEntityByIdUnsafe(id));
            if (!res)
            {
                ARMARX_WARNING_S << "Entity with id " << id << " is not of type ObjectInstance!" << std::endl;
            }
            return res;
        };
 
 
        ScopedSharedLockPtr lock(getReadLock(c));
 
        ObjectInstancePtr object = ObjectInstancePtr::dynamicCast(getObjectInstanceByIdUnsafe(entityId));
        MotionModelInterfacePtr oldMotionModel = object->getMotionModel();
 
        if (oldMotionModel)
        {
            //armarx::LinkedPoseBasePtr oldPose = oldMotionModel->getPoseAtLastLocalisation();
            armarx::LinkedPoseBasePtr oldPose = oldMotionModel->getPredictedPose();
 
            if (oldPose)
            {
                if (!newMotionModel->getUncertainty())
                {
                    newMotionModel->setPoseAtLastLocalisation(oldPose, nullptr, oldMotionModel->getUncertainty());
                }
                else
                {
                    newMotionModel->setPoseAtLastLocalisation(oldPose, nullptr, nullptr);
                }
            }
            else
            {
                ARMARX_ERROR_S << "Object " << object->getName() << " has an old motion model, but that motion model has no pose.";
            }
        }
        else
        {
            ARMARX_WARNING_S << "Object " << object->getName() << " didn't have a motion model before, this may cause problems - setting pose to current robot pose";
            newMotionModel->setPoseAtLastLocalisation(new armarx::LinkedPose(*object->getPose(),
                    newMotionModel->robotStateProxy->getRobotSnapshot(newMotionModel->robotStateProxy->getRobotName())), nullptr, nullptr);
        }
 
        object->setMotionModel(AbstractMotionModelPtr::dynamicCast(newMotionModel));
        ARMARX_INFO_S << "New motion model set for " << object->getName() << ": " << object->getMotionModel()->ice_id();
        //                updateEntity(object->getId(), object);
    }
 
    void ObjectInstanceMemorySegment::setObjectPose(const std::string& entityId, const armarx::LinkedPoseBasePtr& objectPose, const Ice::Current& c)
    {
        ScopedSharedLockPtr lock(getReadLock(c));
 
        ObjectInstancePtr object = ObjectInstancePtr::dynamicCast(getEntityByIdUnsafe(entityId));
        auto oldObject = object->clone();
        armarx::FramedPositionPtr position = new armarx::FramedPosition(armarx::Vector3Ptr::dynamicCast(objectPose->position)->toEigen(), objectPose->frame, objectPose->agent);
        object->setPosition(position);
        object->setPositionUncertainty(MultivariateNormalDistribution::CreateDefaultDistribution());
        armarx::FramedOrientationPtr orientation = new armarx::FramedOrientation(armarx::QuaternionPtr::dynamicCast(objectPose->orientation)->toEigen(), objectPose->frame, objectPose->agent);
        object->setOrientation(orientation);
 
        ARMARX_DEBUG_S << "New pose for object " << object->getName() << ": " << objectPose->output();
 
        if (object->getMotionModel())
        {
            object->getMotionModel()->setPoseAtLastLocalisation(objectPose, nullptr, MultivariateNormalDistribution::CreateDefaultDistribution());
            if (listenerProxy)
            {
                listenerProxy->reportEntityUpdated(getSegmentName(),
                                                   oldObject,
                                                   object);
            }
 
        }
    }
 
    void ObjectInstanceMemorySegment::setObjectPoseWithoutMotionModel(const std::string& entityId, const armarx::FramedPoseBasePtr& objectPose, const Ice::Current& c)
    {
        ScopedUniqueLockPtr lock(getWriteLock(c));
 
        ObjectInstancePtr object = ObjectInstancePtr::dynamicCast(getEntityByIdUnsafe(entityId));
        auto oldObject = object->clone();
        armarx::FramedPositionPtr position = new armarx::FramedPosition(armarx::Vector3Ptr::dynamicCast(objectPose->position)->toEigen(), objectPose->frame, objectPose->agent);
        object->setPosition(position);
        object->setPositionUncertainty(MultivariateNormalDistribution::CreateDefaultDistribution());
        armarx::FramedOrientationPtr orientation = new armarx::FramedOrientation(armarx::QuaternionPtr::dynamicCast(objectPose->orientation)->toEigen(), objectPose->frame, objectPose->agent);
        object->setOrientation(orientation);
 
 
        if (object->getMotionModel())
        {
            ARMARX_WARNING_S << object->getName() << " has a motion model - You should not call setObjectPoseWithoutMotionModel() when a motion model is set. Use set setObjectPose() instead!";
        }
        if (listenerProxy)
        {
            listenerProxy->reportEntityUpdated(getSegmentName(),
                                               oldObject,
                                               object);
        }
    }
 
    std::string ObjectInstanceMemorySegment::addObjectInstance(const std::string& instanceName, const std::string& className, const armarx::LinkedPoseBasePtr& objectPose, const MotionModelInterfacePtr& motionModel, const Ice::Current&)
    {
        ObjectInstancePtr object = new ObjectInstance(instanceName);
        armarx::FramedPositionPtr position = new armarx::FramedPosition(armarx::Vector3Ptr::dynamicCast(objectPose->position)->toEigen(), objectPose->frame, objectPose->agent);
        object->setPosition(position);
        object->setLocalizationTimestamp(objectPose->referenceRobot->getTimestamp());
        armarx::FramedOrientationPtr orientation = new armarx::FramedOrientation(armarx::QuaternionPtr::dynamicCast(objectPose->orientation)->toEigen(), objectPose->frame, objectPose->agent);
        object->setOrientation(orientation);
        Eigen::Vector3f mean = {objectPose->position->x, objectPose->position->y, objectPose->position->z};
        Eigen::Matrix3f variances;
        variances << 10000, 0, 0,  0, 10000, 0,  0, 0, 10000;
        MultivariateNormalDistributionPtr uncertainty = new MultivariateNormalDistribution(mean, variances);
        object->setPositionUncertainty(uncertainty);
        object->addClass(className, 1.0);
        object->setExistenceCertainty(1.0);
 
        motionModel->setPoseAtLastLocalisation(objectPose, nullptr, nullptr);
        object->setMotionModel(AbstractMotionModelPtr::dynamicCast(motionModel));
 
        return addEntity(object);
    }
 
    void ObjectInstanceMemorySegment::updateEntity(const std::string& entityId, const EntityBasePtr& update, const Ice::Current&)
    {
        ARMARX_ERROR_S << "You may not call the updateEntity() function for object instances. Use the setObjectPose(), setMotionModel(), setEntityAttribute(), setEntityAttributes()  etc. functions to manually modify the object instance.";
    }
 
}