ObjectLocalizationSaliency.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    RobotComponents::ArmarXObjects::ObjectLocalizationSaliency
 * @author     David Schiebener (schiebener at kit dot edu)
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ObjectLocalizationSaliency.h"
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
using namespace armarx;
 
 
void ObjectLocalizationSaliency::onInitComponent()
{
    usingProxy(getProperty<std::string>("ViewSelectionName").getValue());
    usingTopic(getProperty<std::string>("ViewSelectionName").getValue() + "Observer");
    usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
    usingProxy(getProperty<std::string>("HeadIKUnitName").getValue());
    usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
    usingProxy(getProperty<std::string>("PriorKnowledgeName").getValue());
 
    randomNoiseLevel = getProperty<float>("RandomNoiseLevel").getValue();
 
    cameraFrameName = getProperty<std::string>("CameraFrameName").getValue();
    headFrameName = getProperty<std::string>("HeadFrameName").getValue();
    centralHeadTiltAngle = getProperty<float>("CentralHeadTiltAngle").getValue();
 
    halfCameraOpeningAngle = getProperty<float>("HalfCameraOpeningAngle").getValue();
    deviationFromCameraCenterFactor = 0.5f;
 
    std::string graphFileName = "RobotAPI/ViewSelection/graph40k.gra";
 
    armarx::CMakePackageFinder finder("RobotAPI");
    ArmarXDataPath::addDataPaths(finder.getDataDir());
 
    if (ArmarXDataPath::getAbsolutePath(graphFileName, graphFileName))
    {
        saliencyEgosphereGraph = new CIntensityGraph(graphFileName);
        ARMARX_VERBOSE << "Created egosphere graph with " << saliencyEgosphereGraph->getNodes()->size() << "nodes";
        graphLookupTable = new CGraphPyramidLookupTable(9, 18);
        graphLookupTable->buildLookupTable(saliencyEgosphereGraph);
        nodeVisitedForObject.resize(saliencyEgosphereGraph->getNodes()->size());
 
        randomNoiseGraph = new CIntensityGraph(*saliencyEgosphereGraph);
        randomNoiseGraph->set(0);
    }
    else
    {
        ARMARX_ERROR << "Could not find required graph file";
        handleExceptions();
    }
 
    // this is robot model specific: offset from the used head coordinate system to the actual
    // head center where the eyes are assumed to be located. Here it is correct for ARMAR-III
    offsetToHeadCenter << 0, 0, 150;
    processorTask = new PeriodicTask<ObjectLocalizationSaliency>(this, &ObjectLocalizationSaliency::process, 1000.f / getProperty<float>("UpdateFrequency").getValue(), false, "ViewSelectionCalculation", false);
    processorTask->setDelayWarningTolerance(2600);
}
 
 
void ObjectLocalizationSaliency::onConnectComponent()
{
    viewSelection = getProxy<ViewSelectionInterfacePrx>(getProperty<std::string>("ViewSelectionName").getValue());
    viewSelection = viewSelection->ice_compress(true);
    robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
 
    memoryProxy = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
    priorKnowledge = getProxy<memoryx::PriorKnowledgeInterfacePrx>(getProperty<std::string>("PriorKnowledgeName").getValue());
    objectInstancesProxy = memoryx::ObjectInstanceMemorySegmentBasePrx::uncheckedCast(memoryProxy->getSegment("objectInstances"));
    objectClassesProxy = memoryx::ObjectClassMemorySegmentBasePrx::uncheckedCast(memoryProxy->getSegment("objectClasses"));
 
    robot = RemoteRobot::createLocalClone(robotStateComponent);
 
    auto objClassSegment = priorKnowledge->getObjectClassesSegment();
    std::vector<std::string> ids = objClassSegment->getAllEntityIds();
 
 
    for (std::string& id : ids)
    {
        memoryx::ObjectClassPtr objClass = memoryx::ObjectClassPtr::dynamicCast(objClassSegment->getEntityById(id));
 
        if (objClass)
        {
            memoryx::EntityWrappers::ObjectRecognitionWrapperPtr wrapper = objClass->addWrapper(new memoryx::EntityWrappers::ObjectRecognitionWrapper());
 
            if (!wrapper->getRecognitionMethod().empty() && wrapper->getRecognitionMethod() != "<none>")
            {
                recognizableObjClasses[objClass->getName()] = objClass;
                ARMARX_INFO << objClass->getName() << " has " << wrapper->getRecognitionMethod();
            }
        }
    }
 
    if (viewSelection->isEnabledAutomaticViewSelection())
    {
        onActivateAutomaticViewSelection();
    }
}
 
 
void ObjectLocalizationSaliency::onDisconnectComponent()
{
 
    onDeactivateAutomaticViewSelection();
}
 
 
void ObjectLocalizationSaliency::onExitComponent()
{
    delete graphLookupTable;
    delete saliencyEgosphereGraph;
    delete randomNoiseGraph;
}
 
 
void ObjectLocalizationSaliency::process()
{
    std::unique_lock<std::mutex> lock(mutex);
 
    IceUtil::Time delta = next - armarx::TimeUtil::GetTime();
    delta -= lastDiff;
    delta -= IceUtil::Time::milliSeconds(50);
 
    lock.unlock();
 
    if (delta > IceUtil::Time::seconds(0))
    {
        TimeUtil::Sleep(delta);
    }
 
 
    IceUtil::Time startTime = TimeUtil::GetTime();
 
    generateObjectLocalizationSaliency();
 
    SaliencyMapBasePtr objectLocalizationSaliencyMap = new SaliencyMapBase();
    objectLocalizationSaliencyMap->name = "objectLocalizationSaliency";
    saliencyEgosphereGraph->graphToVec(objectLocalizationSaliencyMap->map);
    viewSelection->updateSaliencyMap(objectLocalizationSaliencyMap);
 
    SaliencyMapBasePtr randomNoise = new SaliencyMapBase();
    randomNoise->name = "randomNoise";
    randomNoiseGraph->graphToVec(randomNoise->map);
    viewSelection->updateSaliencyMap(randomNoise);
 
    IceUtil::Time stopTime = TimeUtil::GetTime();
 
    std::lock_guard<std::mutex> lock2(mutex);
 
    lastDiff = (stopTime - startTime);
}
 
 
void ObjectLocalizationSaliency::generateObjectLocalizationSaliency()
{
 
    // reset saliency and visited nodes
    saliencyEgosphereGraph->set(0);
 
    for (size_t i = 0; i < saliencyEgosphereGraph->getNodes()->size(); i++)
    {
        nodeVisitedForObject.at(i) = -1;
    }
 
 
    // collect localizable objects
    std::vector<memoryx::ObjectInstancePtr> localizableObjects;
    std::vector<FramedPositionPtr> localizableObjectPositions;
    float maxObjectDistance = getProperty<float>("MaxObjectDistance").getValue();
 
    RemoteRobot::synchronizeLocalClone(robot, robotStateComponent);
 
    int numberOfLostObjects = 0;
 
    memoryx::EntityBaseList objectInstances = objectInstancesProxy->getAllEntities();
 
    for (size_t i = 0; i < objectInstances.size(); i++)
    {
        const memoryx::ObjectInstancePtr object = memoryx::ObjectInstancePtr::dynamicCast(objectInstances.at(i));
 
        if (object)
        {
            if (recognizableObjClasses.count(object->getMostProbableClass()) > 0 && !object->getPosition()->getFrame().empty())
            {
                if (objectClassesProxy->hasEntityByName(object->getMostProbableClass())) // should be true if and only if object has been requested
                {
                    FramedPositionPtr position = object->getPosition();
                    position->changeFrame(robot, headFrameName);
                    float objDist = position->toEigen().norm();
 
                    if (objDist <= maxObjectDistance)
                    {
                        localizableObjects.push_back(object);
                        localizableObjectPositions.push_back(position);
 
                        if (object->getExistenceCertainty() < 0.5)
                        {
                            numberOfLostObjects++;
                        }
                    }
                    else
                    {
                        ARMARX_DEBUG << "Discarding " << object->getName() << " which is at " << position->output() <<  " ObjectDistance " << objDist;
                    }
                }
            }
        }
    }
 
 
 
    // generate new random noise
    if (randomNoiseLevel > 0)
    {
        generateRandomNoise(localizableObjects, objectInstances);
    }
 
 
    const float probabilityToAddALostObject = (numberOfLostObjects > 0) ? getProperty<float>("ProbabilityToLookForALostObject").getValue() / numberOfLostObjects : 0;
 
    // add localization necessities to sphere of possible view directions
    for (size_t i = 0; i < localizableObjects.size(); i++)
    {
        const memoryx::ObjectInstancePtr object = localizableObjects.at(i);
 
        if ((object->getExistenceCertainty() >= 0.5) || (rand() % 100000 < 100000 * probabilityToAddALostObject))
        {
            const FramedPositionPtr position = localizableObjectPositions.at(i);
 
            memoryx::MultivariateNormalDistributionPtr uncertaintyGaussian = memoryx::MultivariateNormalDistributionPtr::dynamicCast(object->getPositionUncertainty());
 
            if (!uncertaintyGaussian)
            {
                uncertaintyGaussian = memoryx::MultivariateNormalDistribution::CreateDefaultDistribution(1000000);
            }
 
            float priority = object->getLocalizationPriority() / 100.0;
            float saliency = priority * log(1.0 + uncertaintyGaussian->getVarianceScalar());
 
            if (saliency > 0)
            {
                // close objects get a smaller visibility angle to avoid problems due to the head being positioned unfortunately
                float modifiedHalfCameraOpeningAngle = halfCameraOpeningAngle;
                float objDist = position->toEigen().norm();
                float shortDistance = 0.5f * maxObjectDistance;
                if (objDist < shortDistance)
                {
                    modifiedHalfCameraOpeningAngle = (objDist - 0.1f * shortDistance) / (0.9f * shortDistance) * halfCameraOpeningAngle;
                    modifiedHalfCameraOpeningAngle = std::max(0.0f, modifiedHalfCameraOpeningAngle);
                }
 
                TSphereCoord positionInSphereCoordinates;
                Eigen::Vector3f vec = position->toEigen() - offsetToHeadCenter;
                MathTools::convert(vec, positionInSphereCoordinates);
                int closestNodeIndex = graphLookupTable->getClosestNode(positionInSphereCoordinates);
                addSaliencyRecursive(closestNodeIndex, saliency, positionInSphereCoordinates, i, modifiedHalfCameraOpeningAngle);
            }
        }
    }
}
 
 
void ObjectLocalizationSaliency::addSaliencyRecursive(const int currentNodeIndex, const float saliency, const TSphereCoord objectSphereCoord, const int objectIndex, const float maxDistanceOnArc)
{
 
    // distance on arc between object projection center and node,
    // normalized by the maximal viewing angle of the camera (=> in [0,1])
    float normalizedDistance = MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(currentNodeIndex)->getPosition()) / maxDistanceOnArc;
 
    // increase value of node
    float newValue = ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))->getIntensity()
                     + (1.0f - deviationFromCameraCenterFactor * normalizedDistance * normalizedDistance) * saliency;
    ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))->setIntensity(newValue);
 
    // mark node as visited for this object
    nodeVisitedForObject.at(currentNodeIndex) = objectIndex;
 
    // recurse on neighbours if they were not yet visited and close enough to the object projection center
    int neighbourIndex;
 
    for (size_t i = 0; i < saliencyEgosphereGraph->getNodeAdjacency(currentNodeIndex)->size(); i++)
    {
        neighbourIndex = saliencyEgosphereGraph->getNodeAdjacency(currentNodeIndex)->at(i);
 
        if (nodeVisitedForObject.at(neighbourIndex) != objectIndex)
        {
            if (MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(neighbourIndex)->getPosition()) <= maxDistanceOnArc)
            {
                addSaliencyRecursive(neighbourIndex, saliency, objectSphereCoord, objectIndex, maxDistanceOnArc);
            }
            else
            {
                nodeVisitedForObject.at(neighbourIndex) = objectIndex;
            }
        }
    }
}
 
 
void ObjectLocalizationSaliency::generateRandomNoise(std::vector<memoryx::ObjectInstancePtr>& localizableObjects, memoryx::EntityBaseList& objectInstances)
{
 
    // if there are requested objects that were not localized yet, the random noise component will be bigger
    memoryx::EntityBaseList requestedObjectClasses = objectClassesProxy->getAllEntities();
 
    bool unlocalizedObjectExists = false;
    for (size_t i = 0; i < requestedObjectClasses.size(); i++)
    {
        bool isInInstancesList = false;
        for (size_t j = 0; j < objectInstances.size(); j++)
        {
            if (requestedObjectClasses.at(i)->getName() == memoryx::ObjectInstanceBasePtr::dynamicCast(objectInstances.at(j))->getMostProbableClass())
            {
                isInInstancesList = true;
                break;
            }
        }
        if (!isInInstancesList)
        {
            unlocalizedObjectExists = true;
            break;
        }
    }
 
    if (unlocalizedObjectExists)
    {
        if (localizableObjects.size() == 0)
        {
            ARMARX_DEBUG << "There are objects requested, and none of them has been localized yet";
            setRandomNoise(centralHeadTiltAngle + 30, 3.0f);
        }
        else
        {
            ARMARX_DEBUG << "There are objects requested, and some but not all of them have been localized already";
            setRandomNoise(centralHeadTiltAngle + 20, 2.0f);
        }
    }
    else
    {
        ARMARX_DEBUG << "There are no requested objects that were not localized yet. requestedObjectClasses.size() = " << requestedObjectClasses.size()
                     << ", objectInstances.size() = " << objectInstances.size();// << ", difference.size() = " << difference.size();
        setRandomNoise(centralHeadTiltAngle, 1.0f);
    }
 
}
 
 
 
void ObjectLocalizationSaliency::nextViewTarget(Ice::Long timestamp, const Ice::Current& c)
{
    std::lock_guard<std::mutex> lock(mutex);
 
    next = IceUtil::Time::milliSeconds(timestamp);
}
 
 
void ObjectLocalizationSaliency::setRandomNoise(const float centralAngleForVerticalDirection, const float directionVariabilityFactor)
{
 
    Eigen::Vector3f currentViewTargetEigen(1000, 0, 0); // virtual central gaze goes into x direction
    FramedPosition currentViewTarget(currentViewTargetEigen, cameraFrameName, robot->getName());
    currentViewTarget.changeFrame(robot, headFrameName);
    currentViewTargetEigen = currentViewTarget.toEigen() - offsetToHeadCenter;
    TSphereCoord currentViewDirection;
    MathTools::convert(currentViewTargetEigen, currentViewDirection);
 
    TNodeList* nodes = randomNoiseGraph->getNodes();
    const int randomFactor = 100000;
    const float noiseFactor = randomNoiseLevel / randomFactor;
    const float distanceWeight = std::min(0.6f / directionVariabilityFactor, 1.0f);
    const float centralityWeight = std::min(0.1f / directionVariabilityFactor, 0.2f);
 
    for (size_t i = 0; i < nodes->size(); i++)
    {
        CIntensityNode* node = (CIntensityNode*) nodes->at(i);
        TSphereCoord nodeCoord = node->getPosition();
        float distanceOnSphere = MathTools::getDistanceOnArc(currentViewDirection, nodeCoord);
        float distanceFactor = 1.0f - distanceWeight * distanceOnSphere / M_PI; // prefer directions close to current direction
        float verticalCenterFactor = 1.0f - centralityWeight * fabs(nodeCoord.fPhi - centralAngleForVerticalDirection) / 90.0f; // prefer directions that look to the center
        float horizontalCenterFactor = 1.0f - centralityWeight * fabs(nodeCoord.fTheta) / 120.0f;
        node->setIntensity(verticalCenterFactor * horizontalCenterFactor * distanceFactor * noiseFactor * (rand() % randomFactor));
    }
 
}
 
 
armarx::PropertyDefinitionsPtr ObjectLocalizationSaliency::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(new ObjectLocalizationSaliencyPropertyDefinitions(
            getConfigIdentifier()));
}