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()));
}