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 <cfloat>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
 
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/EarlyVisionConverters.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/NoMotionFilter.h>
 
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.";
    }
 
} // namespace memoryx