ViewSelection.cpp

/*
 * 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::ViewSelection
 * @author     David Schiebener (schiebener at kit dot edu)
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */

#include "ViewSelection.h"

#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>

#include <VirtualRobot/Robot.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>


using namespace armarx;
using namespace Eigen;


void ViewSelection::onInitComponent()
{
    usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
    usingProxy(getProperty<std::string>("HeadIKUnitName").getValue());
    usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
    usingProxy(getProperty<std::string>("PriorKnowledgeName").getValue());
    offeringTopic("DebugDrawerUpdates");
    headIKKinematicChainName = getProperty<std::string>("HeadIKKinematicChainName").getValue();
    headFrameName = getProperty<std::string>("HeadFrameName").getValue();
    cameraFrameName = getProperty<std::string>("CameraFrameName").getValue();
    drawViewDirection = getProperty<bool>("VisualizeViewDirection").getValue();

    randomNoiseLevel = getProperty<float>("RandomNoiseLevel").getValue();
    centralHeadTiltAngle = getProperty<float>("CentralHeadTiltAngle").getValue();

    std::string graphFileName;

    if (ArmarXDataPath::getAbsolutePath("RobotComponents/ViewSelection/graph40k.gra", 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());

        // create visibility mask
        visibilityMaskGraph = new CIntensityGraph(*saliencyEgosphereGraph);
        TNodeList* nodes = visibilityMaskGraph->getNodes();
        const float maxOverallHeadTiltAngle = getProperty<float>("MaxOverallHeadTiltAngle").getValue();
        const float borderAreaAngle = 10.0f;
        const float centralAngle = getProperty<float>("CentralHeadTiltAngle").getValue();

        for (size_t i = 0; i < nodes->size(); i++)
        {
            CIntensityNode* node = (CIntensityNode*) nodes->at(i);

            if (fabs(node->getPosition().fPhi - centralAngle) < maxOverallHeadTiltAngle - borderAreaAngle)
            {
                node->setIntensity(1.0f);
            }
            else if (fabs(node->getPosition().fPhi - centralAngle) < maxOverallHeadTiltAngle)
            {
                node->setIntensity(1.0f - (fabs(node->getPosition().fPhi - centralAngle) - (maxOverallHeadTiltAngle - borderAreaAngle)) / borderAreaAngle);
            }
            else
            {
                node->setIntensity(0.0f);
            }
        }

        randomNoiseGraph = new CIntensityGraph(*saliencyEgosphereGraph);
        randomNoiseGraph->set(0);
    }
    else
    {
        ARMARX_ERROR << "Could not find required graph file";
    }

    halfCameraOpeningAngle = getProperty<float>("HalfCameraOpeningAngle").getValue();
    sleepingTimeBetweenViewDirectionChanges = getProperty<int>("SleepingTimeBetweenViewDirectionChanges").getValue();
    doAutomaticViewSelection = getProperty<bool>("ActiveAtStartup").getValue();
    deviationFromCameraCenterFactor = 0.5f;
    timeOfLastViewChange = IceUtil::Time::now();

    // 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;
}


void ViewSelection::onConnectComponent()
{
    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"));
    headIKUnitProxy = getProxy<HeadIKUnitInterfacePrx>(getProperty<std::string>("HeadIKUnitName").getValue());
    drawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
    headIKUnitProxy->request();

    auto objClassSegment = priorKnowledge->getObjectClassesSegment();
    auto ids = objClassSegment->getAllEntityIds();

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

    processorTask = new PeriodicTask<ViewSelection>(this, &ViewSelection::process, 30);
    processorTask->setDelayWarningTolerance(sleepingTimeBetweenViewDirectionChanges + 100);
    processorTask->start();
}


void ViewSelection::onDisconnectComponent()
{
    try
    {
        if (drawer)
        {
            drawer->removeArrowDebugLayerVisu("HeadRealViewDirection");
        }

        processorTask->stop();
        headIKUnitProxy->release();
    }
    catch (...)
    {
        ARMARX_WARNING_S << "An exception occured during onDisconnectComponent()";
        handleExceptions();
    }
}


void ViewSelection::onExitComponent()
{
    delete visibilityMaskGraph;
    delete graphLookupTable;
    delete saliencyEgosphereGraph;
    delete randomNoiseGraph;
}


void ViewSelection::process()
{
    try
    {

        if (manualViewTargets.size() > 0)
        {
            // look to the manually set target
            SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
            FramedPositionPtr target;
            {
                ScopedLock lock(manualViewTargetsMutex);

                target = new FramedPosition(FramedPositionPtr::dynamicCast(manualViewTargets.at(0))->toEigen(), manualViewTargets.at(0)->frame, robotPrx->getName());

                for (size_t i = 0; i < manualViewTargets.size() - 1; i++)
                {
                    manualViewTargets.at(i) = manualViewTargets.at(i + 1);
                }

                manualViewTargets.pop_back();
            }
            ARMARX_INFO << "Looking to manual head target " << *target;
            headIKUnitProxy->setHeadTarget(headIKKinematicChainName, target);

            boost::this_thread::sleep(boost::posix_time::milliseconds(sleepingTimeBetweenViewDirectionChanges));
        }
        else if (doAutomaticViewSelection)
        {
            // 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();
            SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
            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->getName()) > 0 && !object->getPosition()->getFrame().empty())
                    {
                        if (objectClassesProxy->hasEntityByName(object->getName())) // should be true if and only if object has been requested
                        {
                            FramedPositionPtr position = object->getPosition();
                            position->changeFrame(robotPrx, 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)
            {
                // 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() == objectInstances.at(j)->getName())
                        {
                            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);
                }
            }

            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 saliency = 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;
                        MathTools::convert(position->toEigen() - offsetToHeadCenter, positionInSphereCoordinates);
                        int closestNodeIndex = graphLookupTable->getClosestNode(positionInSphereCoordinates);
                        addSaliencyRecursive(closestNodeIndex, saliency, positionInSphereCoordinates, i, modifiedHalfCameraOpeningAngle);
                    }
                }
            }


            // find the direction with highest saliency
            TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
            TNodeList* saliencyEgosphereGraphNodes = saliencyEgosphereGraph->getNodes();
            TNodeList* randomNoiseGraphNodes = randomNoiseGraph->getNodes();
            int maxIndex = -1;
            float maxSaliency = 0;

            for (size_t i = 0; i < saliencyEgosphereGraphNodes->size(); i++)
            {
                CIntensityNode* visibilityNode = (CIntensityNode*) visibilityMaskGraphNodes->at(i);
                CIntensityNode* saliencyNode = (CIntensityNode*) saliencyEgosphereGraphNodes->at(i);
                CIntensityNode* randomNoiseNode = (CIntensityNode*) randomNoiseGraphNodes->at(i);
                float saliency = visibilityNode->getIntensity() * (saliencyNode->getIntensity() + randomNoiseNode->getIntensity());

                if (saliency > maxSaliency)
                {
                    maxSaliency = saliency;
                    maxIndex = i;
                }
            }

            ARMARX_DEBUG << "Highest saliency: " << maxSaliency;

            // select a new view if saliency is bigger than threshold (which converges towards 0 over time)
            int timeSinceLastViewChange = (IceUtil::Time::now() - timeOfLastViewChange).toMilliSeconds();
            float saliencyThreshold = 0;

            if (timeSinceLastViewChange > 0)
            {
                saliencyThreshold = 2.0f * sleepingTimeBetweenViewDirectionChanges / timeSinceLastViewChange;
            }

            if (maxSaliency > saliencyThreshold)
            {
                Eigen::Vector3f target;
                MathTools::convert(saliencyEgosphereGraphNodes->at(maxIndex)->getPosition(), target);
                const float distanceFactor = 1500;
                target = distanceFactor * target + offsetToHeadCenter;
                FramedPositionPtr viewTargetPositionPtr = new FramedPosition(target, headFrameName, robotPrx->getName());

                if (drawer && robotPrx->hasRobotNode("Cameras") && drawViewDirection)
                {
                    Vector3Ptr startPos = new Vector3(FramedPosePtr::dynamicCast(robotPrx->getRobotNode("Cameras")->getGlobalPose())->toEigen());
                    FramedDirectionPtr currentDirection = new FramedDirection(Eigen::Vector3f(1, 0, 0), "Cameras", robotPrx->getName());
                    drawer->setArrowDebugLayerVisu("CurrentHeadViewDirection", startPos, currentDirection->toGlobal(robotPrx), DrawColor {0, 0.5, 0.5, 0.1}, maxObjectDistance, 5);
                    Eigen::Vector3f plannedDirectionEigen = viewTargetPositionPtr->toGlobalEigen(robotPrx) - startPos->toEigen();
                    Vector3Ptr plannedDirection = new Vector3(plannedDirectionEigen);
                    drawer->setArrowDebugLayerVisu("PlannedHeadViewDirection", startPos, plannedDirection, DrawColor {0.5, 0.5, 0, 0.1}, maxObjectDistance, 5);
                }

                headIKUnitProxy->setHeadTarget(headIKKinematicChainName, viewTargetPositionPtr);

                timeOfLastViewChange = IceUtil::Time::now();
                boost::this_thread::sleep(boost::posix_time::milliseconds(sleepingTimeBetweenViewDirectionChanges));
            }
        }
        else
        {
            boost::this_thread::sleep(boost::posix_time::milliseconds(5));
        }
    }
    catch (...)
    {
        ARMARX_WARNING_S << "An exception occured during process()";
        handleExceptions();
    }
}



void ViewSelection::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 (int i = 0; i < (int)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 ViewSelection::setRandomNoise(const float centralAngleForVerticalDirection, const float directionVariabilityFactor)
{
    if (!robotStateComponent)
    {
        return;
    }

    Eigen::Vector3f currentViewTargetEigen(1000, 0, 0); // virtual central gaze goes into x direction
    FramedPosition currentViewTarget(currentViewTargetEigen, cameraFrameName, robotStateComponent->getSynchronizedRobot()->getName());
    currentViewTarget.changeFrame(robotStateComponent->getSynchronizedRobot(), 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));
    }
}



void ViewSelection::addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c)
{
    ScopedLock lock(manualViewTargetsMutex);

    manualViewTargets.push_back(target);
}


PropertyDefinitionsPtr ViewSelection::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(new ViewSelectionPropertyDefinitions(getConfigIdentifier()));
}