ObjectLocalizationMemoryUpdater.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::Core
* @author    Kai Welke <welke@kit.edu>
* @copyright  2012 Kai Welke
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/exceptions/local/FileIOException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <RobotAPI/libraries/core/LinkedPose.h>
 
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/EarlyVisionConverters.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
#include <MemoryX/libraries/updater/ObjectLocalization/ObjectLocalizationMemoryUpdater.h>
 
// localization strategies
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/LocalizationStrategyOnce.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/LocalizationStrategyRepeated.h>
#include <MemoryX/libraries/motionmodels/AbstractMotionModel.h>
#include <MemoryX/libraries/motionmodels/MotionModelRobotHand.h>
#include <MemoryX/libraries/motionmodels/MotionModelStaticObject.h>
 
namespace memoryx
{
    // *******************************************************
    // public members
    // *******************************************************
    ObjectLocalizationMemoryUpdater::ObjectLocalizationMemoryUpdater() :
        classSegmentName("objectClasses"),
        instanceSegmentName("objectInstances"),
        agentInstancesSegmentName("agentInstances"),
        referenceFrameName("Platform")
    {
    }
 
    void
    ObjectLocalizationMemoryUpdater::setSegmentNames(std::string classSegmentName,
                                                     std::string instanceSegmentName)
    {
        // retrieve segments
        this->classSegmentName = classSegmentName;
        this->instanceSegmentName = instanceSegmentName;
    }
 
    void
    ObjectLocalizationMemoryUpdater::setReferenceFrame(const std::string& referenceFrameName)
    {
        this->referenceFrameName = referenceFrameName;
    }
 
    // *******************************************************
    // implementation of ManagedIceObject callbacks
    // *******************************************************
    void
    ObjectLocalizationMemoryUpdater::onInitComponent()
    {
        // retrieve relevant segments
        objectClassSegment = getSegment<ObjectClassMemorySegmentBase>(classSegmentName);
        objectInstanceSegment = getSegment<ObjectInstanceMemorySegment>(instanceSegmentName);
        agentInstancesSegment = getSegment<AgentInstancesSegment>(agentInstancesSegmentName);
 
        // register for WM updates
        usingTopic(getWorkingMemory()->getListenerTopicName());
 
        // we need a remote robot proxy
        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
        usingProxy(getProperty<std::string>("LongtermMemoryName").getValue());
 
        // this will be used to measure the head motion speed, to decide whether objects can be localized
        if (!getProperty<std::string>("RobotStateObserverName").getValue().empty())
        {
            usingProxy(getProperty<std::string>("RobotStateObserverName").getValue());
        }
 
        if (!getProperty<std::string>("CommonPlacesLearnerName").getValue().empty())
        {
            usingProxy(getProperty<std::string>("CommonPlacesLearnerName").getValue());
        }
 
        workingMemoryName = getProperty<std::string>("WorkingMemoryName").getValue();
        checkFieldOfView = getProperty<bool>("CheckFieldOfView").getValue();
        cameraOpeningAngle = getProperty<float>("CameraOpeningAngle").getValue();
        robotNodeNameLeftCamera = getProperty<std::string>("RobotNodeNameLeftCamera").getValue();
        robotNodeNameRightCamera = getProperty<std::string>("RobotNodeNameRightCamera").getValue();
        handNodeNameLeft = getProperty<std::string>("HandNodeNameLeft").getValue();
        handNodeNameRight = getProperty<std::string>("HandNodeNameRight").getValue();
        headMotionVelocityLimit = getProperty<float>("HeadMotionVelocityLimit").getValue();
 
        if (checkFieldOfView)
        {
            ARMARX_IMPORTANT << "Objects will only be localized when they are assumed to be in the "
                                "field of view";
        }
        else
        {
            ARMARX_IMPORTANT
                << "Objects will always be localized, even when they are not in the field of view";
        }
    }
 
    void
    ObjectLocalizationMemoryUpdater::onConnectComponent()
    {
        robotStateInterfacePrx = getProxy<armarx::RobotStateComponentInterfacePrx>(
            getProperty<std::string>("RobotStateComponentName").getValue());
        longtermMemoryPrx = getProxy<memoryx::LongtermMemoryInterfacePrx>(
            getProperty<std::string>("LongtermMemoryName").getValue());
        auto robot = robotStateInterfacePrx->getSynchronizedRobot();
        referenceFrameName = robot->getRootNode()->getName();
        agentName = robot->getName();
 
        if (!getProperty<std::string>("RobotStateObserverName").getValue().empty())
        {
            robotStateObserverProxy = getProxy<armarx::ObserverInterfacePrx>(
                getProperty<std::string>("RobotStateObserverName").getValue());
            headTCPVelocityDatafieldID = new armarx::DataFieldIdentifier(
                getProperty<std::string>("RobotStateObserverName").getValue(),
                "TCPVelocities",
                getProperty<std::string>("GazeTCPName").getValue());
        }
 
        if (!getProperty<std::string>("CommonPlacesLearnerName").getValue().empty())
        {
            commonPlacesLearnerProxy = getProxy<memoryx::CommonPlacesLearnerInterfacePrx>(
                getProperty<std::string>("CommonPlacesLearnerName").getValue());
        }
 
        // start scheduler
        ARMARX_INFO << "Starting localization jobs";
        localizationTask = new armarx::PeriodicTask<ObjectLocalizationMemoryUpdater>(
            this, &ObjectLocalizationMemoryUpdater::runJobs, 10, false, "", false);
        localizationTask->start();
    }
 
    void
    ObjectLocalizationMemoryUpdater::onDisconnectComponent()
    {
        if (localizationTask)
        {
            localizationTask->stop();
        }
    }
 
    // *******************************************************
    // implementation of WorkingMemoryListenerInterface
    // *******************************************************
    void
    ObjectLocalizationMemoryUpdater::reportEntityUpdated(const std::string& segmentName,
                                                         const EntityBasePtr& entityOld,
                                                         const EntityBasePtr& entityNew,
                                                         const Ice::Current&)
    {
        ARMARX_DEBUG << deactivateSpam(4)
                     << "ObjectLocalizationMemoryUpdater::reportEntityUpdated()";
 
        // only updates from class segment
        if (segmentName != classSegmentName)
        {
            return;
        }
 
        // retrieve queries
        ObjectClassPtr objectClassOld = ObjectClassPtr::dynamicCast(entityOld);
        ObjectClassPtr objectClassNew = ObjectClassPtr::dynamicCast(entityNew);
 
        if (!objectClassOld || !objectClassNew)
        {
            return;
        }
 
        EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapperOld =
            objectClassOld->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
        EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrappeNew =
            objectClassNew->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
 
        LocalizationQueryList queriesOld = recognitionWrapperOld->getLocalizationQueries();
        LocalizationQueryList queriesNew = recognitionWrappeNew->getLocalizationQueries();
 
        std::unique_lock lock(jobsMutex);
 
        // start new queries
        LocalizationQueryList queriesCreated = getMissingQueries(queriesOld, queriesNew);
 
        for (LocalizationQueryList::iterator iter = queriesCreated.begin();
             iter != queriesCreated.end();
             iter++)
        {
            LocalizationQueryPtr query = LocalizationQueryPtr::dynamicCast(*iter);
            ARMARX_INFO << "Adding new query " << query->queryName;
 
            // add and create jobs
            std::vector<LocalizationJobPtr> createdjobs = query->createJobs(objectClassSegment);
 
            for (LocalizationJobPtr job : createdjobs)
            {
                ARMARX_DEBUG << deactivateSpam(1) << "Created job for "
                             << job->getRecognitionMethod();
            }
 
            std::move(createdjobs.begin(), createdjobs.end(), std::back_inserter(jobs));
        }
 
        // stop removed queries
        LocalizationQueryList queriesRemoved = getMissingQueries(queriesNew, queriesOld);
 
        for (LocalizationQueryList::iterator iter = queriesRemoved.begin();
             iter != queriesRemoved.end();
             iter++)
        {
            LocalizationQueryPtr query = LocalizationQueryPtr::dynamicCast(*iter);
 
            // remove jobs for this query
            std::vector<LocalizationJobPtr>::iterator iterJobs = jobs.begin();
 
            while (iterJobs != jobs.end())
            {
                if ((*iterJobs)->getQueryName() == query->queryName)
                {
                    iterJobs = jobs.erase(iterJobs);
                }
                else
                {
                    iterJobs++;
                }
            }
 
            ARMARX_INFO << "removed query " << query->queryName;
        }
    }
 
    // *******************************************************
    // private methods
    // *******************************************************
    void
    ObjectLocalizationMemoryUpdater::predictPoses()
    {
        std::unique_lock lock(predictionMutex);
 
        EntityIdList allEntityIDs = objectInstanceSegment->getAllEntityIds();
 
        for (EntityIdList::iterator it = allEntityIDs.begin(); it != allEntityIDs.end(); it++)
        {
            auto entity = objectInstanceSegment->getEntityById(*it);
            if (!entity)
            {
                continue;
            }
            try
            {
                ObjectInstancePtr objectInstance = ObjectInstancePtr::dynamicCast(entity);
 
                AbstractMotionModelPtr motionModel = objectInstance->getMotionModel();
                if (!motionModel)
                {
                    continue;
                }
 
                ARMARX_DEBUG << deactivateSpam(5, objectInstance->getName()) << "prediction for "
                             << objectInstance->getName()
                             << " motion model: " << objectInstance->getMotionModel()->ice_id();
 
                armarx::LinkedPosePtr predictedPose =
                    armarx::LinkedPosePtr::dynamicCast(motionModel->getPredictedPose());
                MultivariateNormalDistributionBasePtr positionUncertainty =
                    motionModel->getUncertainty();
 
                ARMARX_DEBUG << deactivateSpam(1, objectInstance->getName())
                             << "predicted pose: " << predictedPose->toEigen();
                ARMARX_DEBUG
                    << deactivateSpam(1, objectInstance->getName()) << "last localization: "
                    << objectInstance->getMotionModel()->getPoseAtLastLocalisation()->output();
 
                if (positionUncertainty)
                {
                    ARMARX_DEBUG << deactivateSpam(1, objectInstance->getName())
                                 << "positionUncertainty->getDimensions(): "
                                 << positionUncertainty->getDimensions();
                    Gaussian uncertaintyGaussian =
                        EarlyVisionConverters::convertToGaussian(positionUncertainty);
                    ARMARX_DEBUG << deactivateSpam(1, objectInstance->getName())
                                 << "uncertaintyGaussian: " << uncertaintyGaussian.getCovariance();
                }
 
                if (predictedPose->frame.empty())
                {
                    ARMARX_VERBOSE << "predicting pose of " << objectInstance->getName()
                                   << "    position: " << predictedPose->position->x << " "
                                   << predictedPose->position->y << " "
                                   << predictedPose->position->z
                                   << "  frame: " << predictedPose->frame;
                }
 
                // ObjectInstancePtr update = new ObjectInstance(objectInstance->getName());
                objectInstance->setPosition(new armarx::FramedPosition(
                    predictedPose->toEigen(), predictedPose->frame, predictedPose->agent));
                objectInstance->setOrientation(new armarx::FramedOrientation(
                    predictedPose->toEigen(), predictedPose->frame, predictedPose->agent));
 
                if (positionUncertainty)
                {
                    objectInstance->setPositionUncertainty(positionUncertainty);
                }
 
                objectInstance->setClass(
                    objectInstance->getMostProbableClass(),
                    objectInstance->getClassProbability(objectInstance->getMostProbableClass()));
                objectInstance->setExistenceCertainty(objectInstance->getExistenceCertainty());
                //        update->setMotionModel(objectInstance->getMotionModel());
                if (objectInstanceSegment->getListenerProxy() &&
                    objectInstanceSegment->hasEntityById(objectInstance->getId(),
                                                         Ice::emptyCurrent))
                {
                    auto reportClone = objectInstance->clone();
                    objectInstanceSegment->getListenerProxy()->reportEntityUpdated(
                        objectInstanceSegment->getSegmentName(), reportClone, reportClone);
                }
                //        objectInstanceSegment->updateEntity(*it, update, Ice::StringSeq{"KalmanFilterFusion"});
 
                //objectInstance = ObjectInstancePtr::dynamicCast(objectInstanceSegment->getEntityById(*it));
                //FramedPositionPtr pos = objectInstance->getPosition();
                //ARMARX_DEBUG << "new position of " << objectInstance->getName() << ": " << pos->x << " " << pos->y << " " << pos->z << "   " << pos->frame;
                //FramedOrientationPtr ori = objectInstance->getOrientation();
                //ARMARX_DEBUG << "new orientation of " << objectInstance->getName() << ": " << ori->toEigen();
            }
            catch (...)
            {
                ARMARX_INFO << deactivateSpam(2, entity->getName())
                            << "Predicting poses for object '" << entity->getName()
                            << "' failed: " << armarx::GetHandledExceptionString();
            }
        }
    }
 
    void
    ObjectLocalizationMemoryUpdater::scheduleJobs()
    {
        //use a lambda so we can quickly leave this block by calling return
        [&]
        {
            std::unique_lock lock(jobsMutex);
 
            // collect waiting jobs
            std::multimap<std::string, LocalizationJobPtr> recognizerJobs;
            std::set<std::string> recognizerNames;
 
            bool aJobIsWaiting = false;
 
            for (const LocalizationJobPtr& job : jobs)
            {
                if (job->waiting())
                {
                    recognizerJobs.insert(std::make_pair(job->getRecognitionMethod(), job));
                    recognizerNames.insert(job->getRecognitionMethod());
                    //ARMARX_DEBUG << job->getQueryName() << " is waiting";
                    aJobIsWaiting = true;
                }
                else
                {
                    ARMARX_DEBUG << deactivateSpam(1) << job->getQueryName() << " is not waiting";
                }
            }
 
            ARMARX_VERBOSE << deactivateSpam(60, armarx::ValueToString(recognizerNames.size()))
                           << "localizer jobs that should be started: " << recognizerNames
                           << " (number of jobs = " << jobs.size() << ")";
 
            if (!aJobIsWaiting)
            {
                ARMARX_DEBUG << deactivateSpam(5) << "No object needs to be localized";
                return;
            }
 
            Eigen::Vector3f tcpVelocity;
            tcpVelocity.setZero();
 
            if (robotStateObserverProxy)
            {
                if (robotStateObserverProxy->existsDataField(
                        headTCPVelocityDatafieldID->getChannelName(),
                        headTCPVelocityDatafieldID->getDataFieldName()))
                {
                    tcpVelocity =
                        armarx::VariantPtr::dynamicCast(
                            robotStateObserverProxy->getDataField(headTCPVelocityDatafieldID))
                            ->get<armarx::FramedDirection>()
                            ->toEigen();
                }
            }
 
            ARMARX_DEBUG << deactivateSpam(1) << "Head velocity: " << tcpVelocity.norm();
 
            // if head speed is too high, don't localize
            if (tcpVelocity.norm() > headMotionVelocityLimit)
            {
                ARMARX_INFO << deactivateSpam(2)
                            << "Not localizing anything because head velocity is too high: "
                            << tcpVelocity.norm();
                return;
            }
 
            //  start jobs, if recognition method is available
            for (const auto& recognitionMethod : recognizerNames)
            {
 
                // retrieve jobs for recognition method
                if (isRecognitionMethodRunning(recognitionMethod))
                {
                    ARMARX_DEBUG << deactivateSpam(0.5) << "Recognition method "
                                 << recognitionMethod << " is busy";
                    continue;
                }
 
 
                // elements contains two iterators, the first one points to the first and the second one to the last element of
                // the list containing all jobs with the given recognition method
                std::pair<std::map<std::string, LocalizationJobPtr>::iterator,
                          std::map<std::string, LocalizationJobPtr>::iterator>
                    elements = recognizerJobs.equal_range(recognitionMethod);
                std::map<std::string, LocalizationJobPtr>::iterator iterElements = elements.first;
 
                LocalizationJobContainerPtr container =
                    new LocalizationJobContainer(recognitionMethod);
                bool hasJob = false;
                std::unique_lock lockPrediction(predictionMutex, std::defer_lock);
 
                if (lockPrediction.try_lock())
                {
                    while (iterElements != elements.second)
                    {
                        LocalizationJobPtr job = iterElements->second;
 
                        // filter the objects in the job: only try to localize those that should currently be visible, have not been found yet or were lost again
                        std::vector<std::string> classNames = job->getAllClassNames();
                        std::vector<std::string> classNamesToBeLocalized;
 
                        for (size_t i = 0; i < classNames.size(); i++)
                        {
                            ObjectInstanceList instances =
                                objectInstanceSegment->getObjectInstancesByClass(classNames.at(i));
 
                            if (instances.size() != 0)
                            {
                                bool atLeastOneInstanceIsVisibleOrLost = false;
 
                                for (size_t j = 0; j < instances.size(); j++)
                                {
                                    ObjectInstancePtr instance =
                                        ObjectInstancePtr::dynamicCast(instances.at(j));
                                    float existenceCertainty = instance->getExistenceCertainty();
                                    armarx::FramedPositionPtr pos = instance->getPosition();
 
                                    if (checkObjectVisibility(pos) || existenceCertainty < 0.5f)
                                    {
                                        atLeastOneInstanceIsVisibleOrLost = true;
                                    }
                                }
 
                                if (atLeastOneInstanceIsVisibleOrLost)
                                {
                                    for (const auto& instance : instances)
                                    {
                                        if (ObjectInstancePtr::dynamicCast(instance)
                                                ->getMotionModel())
                                        {
                                            ObjectInstancePtr::dynamicCast(instance)
                                                ->getMotionModel()
                                                ->savePredictedPoseAtStartOfCurrentLocalization();
                                        }
                                    }
 
                                    classNamesToBeLocalized.push_back(classNames.at(i));
                                }
                                else
                                {
                                    ARMARX_INFO
                                        << deactivateSpam(5, classNames.at(i))
                                        << "Not trying to localize " << classNames.at(i)
                                        << " because it is currently not in the field of view";
                                }
                            }
                            else
                            {
                                // object has not been found yet, so we will try to localize it
                                classNamesToBeLocalized.push_back(classNames.at(i));
                            }
                        }
 
                        // if at least one object in the job should be visible, we start the localization
                        if (classNamesToBeLocalized.size() > 0)
                        {
                            job->setClassNamesToBeLocalized(classNamesToBeLocalized);
                            container->addJob(job);
                            hasJob = true;
                        }
 
                        iterElements++;
                    }
                }
 
                // start recognition method
                if (hasJob)
                {
                    startLocalization(container);
                }
            }
        }();
 
        try
        {
            predictPoses();
        }
        catch (...)
        {
            ARMARX_ERROR << "predicting poses caused an exception!";
            armarx::handleExceptions();
        }
    }
 
    void
    ObjectLocalizationMemoryUpdater::runJobs()
    {
        try
        {
            scheduleJobs();
        }
        catch (std::exception&)
        {
            ARMARX_ERROR << "Scheduling jobs threw an exception!";
            armarx::handleExceptions();
        }
    }
 
    void
    ObjectLocalizationMemoryUpdater::startLocalization(
        const LocalizationJobContainerPtr& jobContainer)
    {
        ObjectClassNameList objectClassNames = jobContainer->getClassNamesUnique();
        std::string recognitionMethod = jobContainer->getRecognitionMethod();
 
        ObjectLocalizerInterfacePrx proxy = getProxyCached(recognitionMethod);
 
        if (!proxy)
        {
            ARMARX_ERROR << deactivateSpam(5)
                         << "No running proxy found for recognition method: " << recognitionMethod;
            // update queries so that statcharts waiting on queries can continue
            finishJobContainer(jobContainer);
            return;
        }
 
 
        //    ARMARX_DEBUG_S << "Got shared robot proxy " << sharedRobotProxy;
        // make snapshot of robot state at latest possible point
        auto robotName = robotStateInterfacePrx->getSynchronizedRobot()->getName();
        armarx::SharedRobotInterfacePrx sharedRobotProxy =
            robotStateInterfacePrx->getRobotSnapshot(robotName);
        jobContainer->setRobotState(sharedRobotProxy);
 
        // get global pose of robot in world CS, if available
        armarx::PosePtr robotPose = new armarx::Pose(Eigen::Matrix4f::Identity());
        robotPose = armarx::PosePtr::dynamicCast(sharedRobotProxy->getGlobalPose());
 
 
        jobContainer->setRobotPose(robotPose);
 
 
        // start jobs
        setRecognitionMethodRunningState(recognitionMethod, true);
 
        jobContainer->startJobs();
        ARMARX_VERBOSE << deactivateSpam(5, jobContainer->getRecognitionMethod())
                       << "starting localization of " << objectClassNames << " with "
                       << jobContainer->getRecognitionMethod();
        proxy->begin_localizeObjectClasses(
            objectClassNames,
            Ice::newCallback(this, &ObjectLocalizationMemoryUpdater::localizationFinished),
            jobContainer);
    }
 
    bool
    ObjectLocalizationMemoryUpdater::isRecognitionMethodRunning(
        const std::string& recognitionMethod)
    {
        std::unique_lock lock(recognitionMethodStateMutex);
        std::map<std::string, bool>::iterator iter =
            recognitionMethodRunning.find(recognitionMethod);
 
        if (iter == recognitionMethodRunning.end())
        {
            return false;
        }
 
        bool result = recognitionMethodRunning[recognitionMethod];
        return result;
    }
 
    void
    ObjectLocalizationMemoryUpdater::setRecognitionMethodRunningState(
        const std::string& recognitionMethod,
        bool running)
    {
        std::unique_lock lock(recognitionMethodStateMutex);
        ARMARX_DEBUG << deactivateSpam(1) << "Setting running state of recognition method "
                     << recognitionMethod << " to " << running;
        recognitionMethodRunning[recognitionMethod] = running;
    }
 
    void
    ObjectLocalizationMemoryUpdater::finishJobContainer(LocalizationJobContainerPtr jobContainer)
    {
        // set all jobs in the container to finished
        jobContainer->finishJobs();
        setRecognitionMethodRunningState(jobContainer->getRecognitionMethod(), false);
        // update queries in object class segment
        updateQueries(jobContainer);
    }
 
    void
    ObjectLocalizationMemoryUpdater::localizationFinished(const Ice::AsyncResultPtr& r)
    {
 
        ObjectLocalizerInterfacePrx objectLocalizer =
            ObjectLocalizerInterfacePrx::uncheckedCast(r->getProxy());
        LocalizationJobContainerPtr jobContainer =
            LocalizationJobContainerPtr::dynamicCast(r->getCookie());
        ObjectLocalizationResultList resultList;
 
        try
        {
            resultList = objectLocalizer->end_localizeObjectClasses(r);
 
 
            if (jobContainer->getClassNamesUnique().empty())
            {
                ARMARX_WARNING << "There is a bug: Returned from localization, but list of class "
                                  "names is empty";
                finishJobContainer(jobContainer);
                return;
            }
            std::string allObjectNames;
 
            for (size_t i = 0; i < jobContainer->getClassNamesUnique().size(); i++)
            {
                allObjectNames.append(jobContainer->getClassNamesUnique().at(i));
                allObjectNames.append(" ");
            }
 
            ARMARX_DEBUG << deactivateSpam(1) << "Localization of " << allObjectNames
                         << " returned - number of detected instances: " << resultList.size();
 
 
            if (resultList.empty())
            {
                // if the object was not found although it should have been visible, reduce the existence certainty
                if (checkFieldOfView)
                {
                    ObjectInstanceList instances = objectInstanceSegment->getObjectInstancesByClass(
                        jobContainer->getClassNamesUnique().at(0));
 
                    if (instances.size() == 1)
                    {
                        ObjectInstancePtr instance =
                            ObjectInstancePtr::dynamicCast(instances.at(0));
                        float oldExistenceCertainty = instance->getExistenceCertainty();
                        instance->setExistenceCertainty(
                            getProperty<float>(
                                "ExistenceCertaintyReductionFactorWhenLocalizationFailed")
                                .getValue() *
                            oldExistenceCertainty);
                        //                   if(instance->getExistenceCertainty() < 0.1)
                        //                       instance->setExistenceCertainty(0.1);
                        ARMARX_INFO << deactivateSpam(3, instance->getName()) << instance->getName()
                                    << " not found in view altough it should be there - decreasing "
                                       "existence certainty to: "
                                    << instance->getExistenceCertainty();
                        if (objectInstanceSegment->getListenerProxy() &&
                            objectInstanceSegment->hasEntityById(instance->getId(),
                                                                 Ice::emptyCurrent))
                        {
                            auto reportClone = instance->clone();
                            objectInstanceSegment->getListenerProxy()->reportEntityUpdated(
                                objectInstanceSegment->getSegmentName(), reportClone, reportClone);
                        }
                    }
                    else if (instances.size() > 1)
                    {
                        ARMARX_IMPORTANT << "Dealing with failed localizations is not implemented "
                                            "for the case that multiple instances exist";
                    }
                }
            }
            else
            {
                // generate / fuse instances
                const std::string rootNodeName =
                    jobContainer->getRobotState()->getRootNode()->getName();
                for (const ObjectLocalizationResult& result : resultList)
                {
 
                    if (result.objectClassName.empty())
                    {
                        ARMARX_ERROR << "Object name in localization result is empty";
                    }
 
                    armarx::SharedRobotInterfacePrx robotState = jobContainer->getRobotState();
                    armarx::PosePtr robotPose = jobContainer->getRobotPose();
                    if (result.timeStamp)
                    {
                        armarx::TimestampVariantPtr timestamp =
                            armarx::TimestampVariantPtr::dynamicCast(result.timeStamp);
                        IceUtil::Time now = armarx::TimeUtil::GetTime();
                        ARMARX_INFO << deactivateSpam(3) << "object pose is already "
                                    << (now - timestamp->toTime()).toMilliSecondsDouble()
                                    << "ms old";
                        robotState = robotStateInterfacePrx->getRobotSnapshotAtTimestamp(
                            timestamp->getTimestamp());
                        if (!robotState)
                        {
                            ARMARX_WARNING << deactivateSpam(2)
                                           << "getRobotSnapshotAtTimestamp returned NULL, using "
                                              "current robot state "
                                           << "(possibly a bug in the simulation, time server, or "
                                              "something else)";
                            robotState = robotStateInterfacePrx->getRobotSnapshot("");
                        }
                        robotPose = armarx::PosePtr::dynamicCast(robotState->getGlobalPose());
                        jobContainer->setRobotState(robotState);
                        jobContainer->setRobotPose(robotPose);
                    }
                    else
                    {
                        ARMARX_WARNING << deactivateSpam(1, jobContainer->getRecognitionMethod())
                                       << "Recognition method "
                                       << jobContainer->getRecognitionMethod()
                                       << " did not provide a timestamp - using robotstate from "
                                          "before starting recognition";
                    }
                    ARMARX_DEBUG << deactivateSpam(1, result.objectClassName)
                                 << "Object: " << result.objectClassName;
 
                    // new instance for updating
                    ObjectInstancePtr update = new ObjectInstance(
                        result.instanceName.empty() ? result.objectClassName : result.instanceName);
                    update->setLocalizationTimestamp(result.timeStamp);
                    // position and orientation
                    armarx::FramedPositionPtr measuredPosition =
                        armarx::FramedPositionPtr::dynamicCast(result.position);
                    armarx::FramedOrientationPtr measuredOrientation =
                        armarx::FramedOrientationPtr::dynamicCast(result.orientation);
 
                    Eigen::Vector3f positionBeforeFrameChange = measuredPosition->toEigen();
                    measuredPosition->changeFrame(robotState, rootNodeName);
                    measuredOrientation->changeFrame(robotState, rootNodeName);
                    Eigen::Vector3f positionTranslation =
                        measuredPosition->toEigen() - positionBeforeFrameChange;
                    Eigen::Matrix4f measuredPose = math::Helpers::Pose(
                        measuredPosition->toEigen(), measuredOrientation->toEigen());
 
                    ARMARX_DEBUG << deactivateSpam(1, result.objectClassName)
                                 << result.objectClassName
                                 << "New pose from localizer: " << measuredPose
                                 << " (in reference frame " << measuredPosition->frame << ")";
 
                    // now we have new position and orientation in reference frame
                    armarx::FramedPositionPtr posInRef = new armarx::FramedPosition(
                        measuredPose, rootNodeName, measuredPosition->agent);
                    armarx::FramedOrientationPtr orientationInRef = new armarx::FramedOrientation(
                        measuredPose, rootNodeName, measuredPosition->agent);
                    armarx::LinkedPosePtr linkedPose =
                        new armarx::LinkedPose(measuredPose, rootNodeName, robotState);
 
                    update->setPosition(posInRef);
                    update->setOrientation(orientationInRef);
 
                    int priority = 0.0;
                    for (LocalizationQueryPtr& q : jobContainer->getQueries())
                    {
                        if (q->className == result.objectClassName)
                        {
                            priority = q->priority;
                        }
                    }
                    update->setLocalizationPriority(priority);
 
                    // it is not obvious how to rotate the covariance matrix. it seems that it is not as simple as this:
                    //         MultivariateNormalDistributionPtr uncertainty = MultivariateNormalDistributionPtr::dynamicCast(result.positionNoise);
                    //         Eigen::Matrix3f covariance = uncertainty->toEigenCovariance().block<3,3>(0,0);
                    //         Eigen::Matrix3f covarianceInRef = transformationToReferenceFrame.block<3,3>(0,0) * covariance;
                    //         MultivariateNormalDistributionPtr uncertaintyInRef = new MultivariateNormalDistribution(posInRef->toEigen(), covarianceInRef);
                    MultivariateNormalDistributionBasePtr uncertaintyInRef = result.positionNoise;
                    auto mean = uncertaintyInRef->getMean();
                    if (mean.size() == 3)
                    {
                        Eigen::Vector3f meanEigen(mean.at(0), mean.at(1), mean.at(2));
                        meanEigen += positionTranslation;
                        uncertaintyInRef->setMean(
                            memoryx::FloatVector{meanEigen(0), meanEigen(1), meanEigen(2)});
                    }
                    update->setPositionUncertainty(uncertaintyInRef);
 
                    // classes
                    update->addClass(result.objectClassName, result.recognitionCertainty);
 
                    // existence certainty
                    ARMARX_INFO << deactivateSpam(5, result.objectClassName)
                                << result.objectClassName
                                << ": existence certainty: " << result.recognitionCertainty;
                    update->setExistenceCertainty(result.recognitionCertainty);
 
                    // find correspondence in ObjectInstanceLayer (TODO: Method not using motion models and correct reference frame (LinkedCoordinate))
                    ObjectInstancePtr correspondingEntity = ObjectInstancePtr::dynamicCast(
                        objectInstanceSegment->getCorrespondingObjectInstance(update));
 
                    // add entity to or update memory segment
                    if (!correspondingEntity)
                    {
                        ARMARX_INFO << "Adding new entity " << update->getName();
 
                        if (!update->getMotionModel())
                        {
                            setMotionModel(update);
                        }
 
                        update->getMotionModel()->setPoseAtLastLocalisation(
                            linkedPose, robotPose, uncertaintyInRef);
                        objectInstanceSegment->addEntity(update);
                    }
                    else
                    {
                        ARMARX_DEBUG << "updating entity: " << update->getName();
 
                        std::unique_lock lock(predictionMutex);
 
                        if (!correspondingEntity->getMotionModel())
                        {
                            ARMARX_IMPORTANT
                                << "Corresponding entity has no motion model, creating a new one";
                            setMotionModel(correspondingEntity);
                            correspondingEntity->getMotionModel()->setPoseAtLastLocalisation(
                                linkedPose, robotPose, uncertaintyInRef);
                        }
 
                        AbstractMotionModelPtr motionModel =
                            ObjectInstancePtr::dynamicCast(correspondingEntity)->getMotionModel();
 
                        if (motionModel)
                        {
                            //                       update->setMotionModel(motionModel);
                            // insert into memory to give the fusion methods a chance
                            objectInstanceSegment->updateEntityInternal(
                                correspondingEntity->getId(), update);
                            ARMARX_DEBUG
                                << /*deactivateSpam(1, update->getName())<< */ "Ori of update: "
                                << *update->getOrientation();
 
                            // update motion model and predict current pose
                            ObjectInstancePtr objectInstance = ObjectInstancePtr::dynamicCast(
                                objectInstanceSegment->getEntityById(correspondingEntity->getId()));
                            //                        ARMARX_INFO << deactivateSpam(1) << "Object Instance localization timestamp:" << (objectInstance->hasLocalizationTimestamp() ? objectInstance->getLocalizationTimestamp().toDateTime() : "none") << " timestamp of update: " <<
                            //                                    (update->hasLocalizationTimestamp() ? update->getLocalizationTimestamp().toDateTime() : "none") ;
                            ARMARX_DEBUG /*<< deactivateSpam(1, update->getName()) */
                                << update->getName()
                                << " obj Pose from memory:  " << objectInstance->getPose()->output()
                                << " robot global pose: "
                                << jobContainer->getRobotState()->getGlobalPose()->output();
                            objectInstance->setLocalizationTimestamp(result.timeStamp);
                            armarx::LinkedPosePtr linkedObjectPose =
                                new armarx::LinkedPose(*objectInstance->getPose(), robotState);
                            ARMARX_DEBUG
                                << /*deactivateSpam(1, update->getName()) <<*/ "Ori after fusion: "
                                << *objectInstance->getOrientation();
 
                            motionModel->setPoseAtLastLocalisation(
                                linkedObjectPose,
                                robotPose,
                                objectInstance->getPositionUncertainty());
                            armarx::LinkedPosePtr predictedPose =
                                armarx::LinkedPosePtr::dynamicCast(motionModel->getPredictedPose());
                            ARMARX_DEBUG << /*deactivateSpam(1, update->getName()) <<*/ VAROUT(
                                predictedPose->output());
                            objectInstance->setPose(predictedPose);
                        }
                        else
                        {
                            ARMARX_WARNING << "no motion model for obj instance "
                                           << update->getName() << " set";
                        }
                    }
 
                    if (commonPlacesLearnerProxy)
                    {
                        ARMARX_VERBOSE << "Using object instance " << VAROUT(update)
                                       << " to train common places learner";
                        try
                        {
                            commonPlacesLearnerProxy->learnFromObject(update);
                        }
                        catch (...)
                        {
                            ARMARX_INFO << "Learning failed: "
                                        << armarx::GetHandledExceptionString();
                        }
                    }
                }
            }
        }
        catch (std::exception& e)
        {
            ARMARX_ERROR << deactivateSpam(2, jobContainer->getRecognitionMethod())
                         << "Localization failed of type " << jobContainer->getRecognitionMethod()
                         << "\nReason: " << e.what();
            //handleExceptions();
        }
 
 
        finishJobContainer(jobContainer);
    }
 
    void
    ObjectLocalizationMemoryUpdater::updateQueries(const LocalizationJobContainerPtr& jobContainer)
    {
        std::vector<LocalizationQueryPtr> queries = jobContainer->getQueries();
 
        for (std::vector<LocalizationQueryPtr>::iterator iter = queries.begin();
             iter != queries.end();
             iter++)
        {
            LocalizationQueryPtr query = *iter;
 
            if (query->getFinished())
            {
                // update working memory segment
                EntityBasePtr entity = objectClassSegment->getEntityByName(
                    query->className, Ice::emptyCurrent); // pass Ice::Current to get a safe clone
 
                if (entity)
                {
                    ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(entity);
 
                    if (objectClass)
                    {
                        EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapper =
                            objectClass->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
                        recognitionWrapper->updateLocalizationQuery(query->queryName, query);
                    }
 
                    ARMARX_DEBUG << deactivateSpam(1) << "Updating query " << query->queryName
                                 << " with state " << query->getFinished();
 
                    // update entity in segment with current state
                    objectClassSegment->updateClass(query->className, objectClass);
                }
            }
        }
    }
 
    ObjectLocalizerInterfacePrx
    ObjectLocalizationMemoryUpdater::getProxyCached(const std::string& recognitionMethod)
    {
        // find proxy in list of already obtained proxies
        std::map<std::string, ObjectLocalizerInterfacePrx>::iterator iter;
        iter = knownObjectProxies.find(recognitionMethod);
 
        if (iter != knownObjectProxies.end())
        {
            // TODO: check if proxy is still valid
            return iter->second;
        }
 
        // create new proxy if not found
        ObjectLocalizerInterfacePrx objectLocalizerProxy;
 
        try
        {
            objectLocalizerProxy =
                getWorkingMemory()->getProxy<ObjectLocalizerInterfacePrx>(recognitionMethod);
        }
        catch (...)
        {
            // exceptions only indicate an issue with the proxy
            // this is handled below when the Prx is checked for NULL
        }
 
        if (objectLocalizerProxy)
        {
            std::pair<std::string, ObjectLocalizerInterfacePrx> entry;
            entry.first = recognitionMethod;
            entry.second =
                getWorkingMemory()->getProxy<ObjectLocalizerInterfacePrx>(recognitionMethod);
 
            knownObjectProxies.insert(entry);
        }
 
        return objectLocalizerProxy;
    }
 
    LocalizationQueryList
    ObjectLocalizationMemoryUpdater::getMissingQueries(const LocalizationQueryList& queriesBase,
                                                       const LocalizationQueryList& queriesCompare)
    {
        LocalizationQueryList missing;
 
        for (LocalizationQueryList::const_iterator iter = queriesCompare.begin();
             iter != queriesCompare.end();
             iter++)
        {
            bool found = false;
 
            for (LocalizationQueryList::const_iterator iterBase = queriesBase.begin();
                 iterBase != queriesBase.end();
                 iterBase++)
            {
                if ((*iterBase)->queryName == (*iter)->queryName)
                {
                    found = true;
                }
            }
 
            if (!found)
            {
                missing.push_back(*iter);
            }
        }
 
        return missing;
    }
 
    bool
    ObjectLocalizationMemoryUpdater::checkObjectVisibility(armarx::FramedPositionPtr pos)
    {
        if (!checkFieldOfView)
        {
            return true;
        }
 
        auto robot = robotStateInterfacePrx->getSynchronizedRobot();
        armarx::FramedPositionPtr posInLeftCamCS =
            armarx::FramedPositionPtr::dynamicCast(pos->clone());
        posInLeftCamCS->changeFrame(robot, robotNodeNameLeftCamera);
        Eigen::Vector3f positionInLeftCamCS = posInLeftCamCS->toEigen();
        armarx::FramedPositionPtr posInRightCamCS =
            armarx::FramedPositionPtr::dynamicCast(pos->clone());
        posInRightCamCS->changeFrame(robot, robotNodeNameRightCamera);
        Eigen::Vector3f positionInRightCamCS = posInRightCamCS->toEigen();
 
        // if object is too far away, don't localize
        if (positionInLeftCamCS(2) > getProperty<float>("MaximalObjectDistance").getValue())
        {
            return false;
        }
 
        Eigen::Vector3f viewDirection = Eigen::Vector3f::UnitZ();
        positionInLeftCamCS.normalize();
        positionInRightCamCS.normalize();
        float angleLeft = acos(viewDirection.dot(positionInLeftCamCS));
        float angleRight = acos(viewDirection.dot(positionInRightCamCS));
 
        if (2 * angleLeft < cameraOpeningAngle && 2 * angleRight < cameraOpeningAngle)
        {
            return true;
        }
        else
        {
            return false;
        }
    }
 
    void
    ObjectLocalizationMemoryUpdater::setMotionModel(ObjectInstancePtr& objectInstance)
    {
        std::string objectClassName = objectInstance->getMostProbableClass();
        // create a motion model
        ObjectClassPtr objectClass =
            ObjectClassPtr::dynamicCast(objectClassSegment->getEntityByName(objectClassName));
        EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapper =
            objectClass->getWrapper<EntityWrappers::ObjectRecognitionWrapper>();
 
        if (!recognitionWrapper)
        {
            recognitionWrapper =
                objectClass->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
        }
 
        std::string motionModelName = recognitionWrapper->getDefaultMotionModel();
        ARMARX_INFO << "Motion model is " << motionModelName;
        AbstractMotionModel::EMotionModelType motionModelType =
            AbstractMotionModel::getMotionModelTypeByName(motionModelName);
        AbstractMotionModelPtr motionModel;
 
        switch (motionModelType)
        {
            case AbstractMotionModel::eMotionModelStaticObject:
            {
                motionModel = new MotionModelStaticObject(robotStateInterfacePrx);
                break;
            }
 
            case AbstractMotionModel::eMotionModelRobotHand:
            {
                std::string handNodeName;
 
                if (objectClassName.find("left") != std::string::npos ||
                    objectClassName.find("Left") != std::string::npos)
                {
                    handNodeName = handNodeNameLeft;
                }
                else
                {
                    handNodeName = handNodeNameRight;
                }
 
                ARMARX_INFO << "The hand node name in the kinematic model is assumed to be "
                            << handNodeName;
                motionModel = new MotionModelRobotHand(robotStateInterfacePrx, handNodeName);
                break;
            }
 
            case AbstractMotionModel::eMotionModelKBM:
            {
                if (armarx::Contains(objectClassName, "left", true))
                {
                    motionModel = new MotionModelKBM(
                        getProperty<std::string>("KBMReferenceNodeName").getValue(),
                        getProperty<std::string>("KBMLeftArmNodeSetName").getValue(),
                        robotStateInterfacePrx,
                        longtermMemoryPrx);
                }
                else
                {
                    motionModel = new MotionModelKBM(
                        getProperty<std::string>("KBMReferenceNodeName").getValue(),
                        getProperty<std::string>("KBMRightArmNodeSetName").getValue(),
                        robotStateInterfacePrx,
                        longtermMemoryPrx);
                }
 
                break;
            }
 
            default:
                ARMARX_WARNING << "Motion model type " << motionModelName
                               << " not yet implemented here!";
                motionModel = new MotionModelStaticObject(robotStateInterfacePrx);
                break;
        }
 
        objectInstance->setMotionModel(motionModel);
    }
} // namespace memoryx