ViewSelection.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    RobotAPI::ViewSelection
 * @author     David Schiebener (schiebener at kit dot edu)
 * @author     Markus Grotz ( markus dot grotz 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 <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/math/ColorUtils.h>
 
#include <thread>
 
namespace armarx
{
    void ViewSelection::onInitComponent()
    {
        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
        usingProxy(getProperty<std::string>("HeadIKUnitName").getValue());
 
        offeringTopic("DebugDrawerUpdates");
        offeringTopic(getName() + "Observer");
 
        headIKKinematicChainName = getProperty<std::string>("HeadIKKinematicChainName").getValue();
        headFrameName = getProperty<std::string>("HeadFrameName").getValue();
        cameraFrameName = getProperty<std::string>("CameraFrameName").getValue();
        drawViewDirection = getProperty<bool>("VisualizeViewDirection").getValue();
        visuSaliencyThreshold = getProperty<float>("VisuSaliencyThreshold").getValue();
 
        std::string graphFileName = "RobotAPI/ViewSelection/graph40k.gra";
 
        armarx::CMakePackageFinder finder("RobotAPI");
        ArmarXDataPath::addDataPaths(finder.getDataDir());
 
        if (ArmarXDataPath::getAbsolutePath(graphFileName, graphFileName))
        {
            visibilityMaskGraph = new CIntensityGraph(graphFileName);
            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);
                }
            }
        }
        else
        {
            ARMARX_ERROR << "Could not find required1 graph file";
            handleExceptions();
            return;
        }
 
        sleepingTimeBetweenViewDirectionChanges = getProperty<int>("SleepingTimeBetweenViewDirectionChanges").getValue();
        doAutomaticViewSelection = getProperty<bool>("ActiveAtStartup").getValue();
 
        timeOfLastViewChange =  armarx::TimeUtil::GetTime();
 
        // 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<ViewSelection>(this, &ViewSelection::process, 30);
        processorTask->setDelayWarningTolerance(sleepingTimeBetweenViewDirectionChanges + 100);
    }
 
 
    void ViewSelection::onConnectComponent()
    {
        robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
 
        headIKUnitProxy = getProxy<HeadIKUnitInterfacePrx>(getProperty<std::string>("HeadIKUnitName").getValue());
        headIKUnitProxy->request();
 
        viewSelectionObserver = getTopic<ViewSelectionObserverPrx>(getName() + "Observer");
        drawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
 
        processorTask->start();
    }
 
 
    void ViewSelection::onDisconnectComponent()
    {
        processorTask->stop();
 
        if (drawer)
        {
            drawer->removeArrowDebugLayerVisu("HeadRealViewDirection");
        }
 
        try
        {
            headIKUnitProxy->release();
        }
        catch (...)
        {
            ARMARX_WARNING_S << "An exception occured during onDisconnectComponent()";
            handleExceptions();
        }
    }
 
 
    void ViewSelection::onExitComponent()
    {
        delete visibilityMaskGraph;
    }
 
 
    void ViewSelection::getActiveSaliencyMaps(std::vector<std::string>& activeSaliencyMaps)
    {
        std::unique_lock lock(syncMutex);
 
        IceUtil::Time currentTime =  armarx::TimeUtil::GetTime();
        for (const auto& p : saliencyMaps)
        {
            if (p.second->validUntil)
            {
                TimestampVariantPtr time = TimestampVariantPtr::dynamicCast(p.second->validUntil);
                if (time->toTime() > currentTime)
                {
                    activeSaliencyMaps.push_back(p.second->name);
                }
            }
            else if ((currentTime - TimestampVariantPtr::dynamicCast(p.second->timeAdded)->toTime()) < IceUtil::Time::seconds(5))
            {
                activeSaliencyMaps.push_back(p.second->name);
            }
        }
    }
 
    ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
    {
        std::vector<std::string> activeSaliencyMaps;
        getActiveSaliencyMaps(activeSaliencyMaps);
 
        if (activeSaliencyMaps.empty())
        {
            return nullptr;
        }
 
 
        SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
        TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
 
        // find the direction with highest saliency
        int maxIndex = -1;
        float maxSaliency = std::numeric_limits<float>::min();
 
        std::unique_lock lock(syncMutex);
 
        hasNewSaliencyMap = false;
 
        for (size_t i = 0; i < visibilityMaskGraphNodes->size(); i++)
        {
            float saliency = 0.0f;
            for (const std::string& n : activeSaliencyMaps)
            {
                saliency += saliencyMaps[n]->map[i];
            }
 
            saliency /= activeSaliencyMaps.size();
 
            CIntensityNode* visibilityNode = (CIntensityNode*) visibilityMaskGraphNodes->at(i);
            saliency *= visibilityNode->getIntensity();
 
            if (saliency > maxSaliency)
            {
                maxSaliency = saliency;
                maxIndex = i;
            }
        }
 
 
        lock.unlock();
 
        ARMARX_DEBUG << "Highest saliency: " << maxSaliency;
 
        // select a new view if saliency is bigger than threshold (which converges towards 0 over time)
        int timeSinceLastViewChange = (armarx::TimeUtil::GetTime() - timeOfLastViewChange).toMilliSeconds();
        float saliencyThreshold = 0;
 
        if (timeSinceLastViewChange > 0)
        {
            saliencyThreshold = 2.0f * sleepingTimeBetweenViewDirectionChanges / timeSinceLastViewChange;
        }
 
        if (maxSaliency > saliencyThreshold)
        {
            Eigen::Vector3f target;
            MathTools::convert(visibilityMaskGraphNodes->at(maxIndex)->getPosition(), target);
            const float distanceFactor = 1500;
            target = distanceFactor * target + offsetToHeadCenter;
            FramedPositionPtr viewTargetPositionPtr = new FramedPosition(target, headFrameName, robotPrx->getName());
 
            ViewTargetBasePtr viewTarget = new ViewTargetBase();
            viewTarget->priority = armarx::DEFAULT_VIEWTARGET_PRIORITY; // *  maxSaliency;
            viewTarget->timeAdded = new TimestampVariant(armarx::TimeUtil::GetTime());
            viewTarget->target = viewTargetPositionPtr;
            viewTarget->duration = 0;
 
 
            if (visuSaliencyThreshold > 0.0)
            {
                drawSaliencySphere(activeSaliencyMaps);
            }
 
            return viewTarget;
 
        }
 
        return nullptr;
    }
 
 
 
    void ViewSelection::process()
    {
        /*
        while(getState() < eManagedIceObjectExiting)
        {
        }
        */
 
        ViewTargetBasePtr viewTarget;
 
        if (doAutomaticViewSelection)
        {
            viewTarget = nextAutomaticViewTarget();
        }
 
        /*
        //discard outdated view targets
        IceUtil::Time currentTime = armarx::TimeUtil::GetTime();
        while (!manualViewTargets.empty())
        {
            ViewTargetBasePtr manualViewTarget = manualViewTargets.top();
            TimestampVariantPtr time = TimestampVariantPtr::dynamicCast(manualViewTarget->validUntil);
 
            if (time->toTime() < currentTime)
            {
                ARMARX_INFO << "view target is no longer valid";
                manualViewTargets.pop();
            }
            else
            {
                break;
            }
        }
        */
 
        if (!manualViewTargets.empty())
        {
            std::unique_lock lock(manualViewTargetsMutex);
 
            ViewTargetBasePtr manualViewTarget = manualViewTargets.top();
 
            CompareViewTargets c;
 
            if (!viewTarget)
            {
                viewTarget = manualViewTarget;
                manualViewTargets.pop();
            }
            else if (c(viewTarget, manualViewTarget))
            {
                viewTarget = manualViewTarget;
                manualViewTargets.pop();
            }
        }
 
        if (viewTarget)
        {
            SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
            // FramedPositionPtr viewTargetPositionPtr = FramedPositionPtr::dynamicCast(viewTarget->target);
            FramedPositionPtr viewTargetPositionPtr = new FramedPosition(FramedPositionPtr::dynamicCast(viewTarget->target)->toEigen(), viewTarget->target->frame, robotPrx->getName());
 
            if (drawer && robotPrx->hasRobotNode("Cameras") && drawViewDirection)
            {
                float arrowLength = 1500.0f;
                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}, arrowLength, 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}, arrowLength, 5);
            }
 
            ARMARX_INFO << "Looking at target " << viewTargetPositionPtr->output();
            headIKUnitProxy->setHeadTarget(headIKKinematicChainName, viewTargetPositionPtr);
 
            timeOfLastViewChange = TimeUtil::GetTime();
 
            long duration = viewTarget->duration;
            if (!viewTarget->duration)
            {
                duration = sleepingTimeBetweenViewDirectionChanges;
            }
 
            viewSelectionObserver->nextViewTarget(timeOfLastViewChange.toMilliSeconds() + duration);
 
            std::this_thread::sleep_for(std::chrono::milliseconds(duration));
        }
        else
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(5));
        }
    }
 
 
 
    void ViewSelection::addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c)
    {
        std::unique_lock lock(manualViewTargetsMutex);
 
        //SharedRobotInterfacePrx sharedRobotInterface = robotStateComponent->getRobotSnapshot();
 
        ViewTargetBasePtr viewTarget = new ViewTargetBase();
        viewTarget->priority = armarx::DEFAULT_VIEWTARGET_PRIORITY + 1;
        viewTarget->timeAdded = new TimestampVariant(IceUtil::Time::now());
        viewTarget->target = target;
        viewTarget->duration = 0;
 
        manualViewTargets.push(viewTarget);
 
        std::unique_lock lock2(syncMutex);
 
        condition.notify_all();
    }
 
    void ViewSelection::clearManualViewTargets(const Ice::Current& c)
    {
        std::unique_lock lock(manualViewTargetsMutex);
 
        while (!manualViewTargets.empty())
        {
            manualViewTargets.pop();
        }
 
        // std::priority_queue<ViewTargetBasePtr, vector<ViewTargetBasePtr>, CompareViewTargets> temp;
        // manualViewTargets.swap(temp);
    }
 
    ViewTargetList ViewSelection::getManualViewTargets(const Ice::Current& c)
    {
        std::unique_lock lock(manualViewTargetsMutex);
 
        ViewTargetList result;
 
        std::priority_queue<ViewTargetBasePtr, std::vector<ViewTargetBasePtr>, CompareViewTargets> temp(manualViewTargets);
 
        while (!temp.empty())
        {
            result.push_back(temp.top());
 
            temp.pop();
        }
 
        return result;
    }
 
 
 
    void armarx::ViewSelection::updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current&)
    {
        std::unique_lock lock(syncMutex);
 
        hasNewSaliencyMap = true;
 
        map->timeAdded = TimestampVariant::nowPtr();
 
        saliencyMaps[map->name] = map;
 
        condition.notify_all();
 
    }
 
 
    void ViewSelection::drawSaliencySphere(const ::Ice::StringSeq& names, const Ice::Current& c)
    {
        ARMARX_LOG << "visualizing saliency map";
 
        drawer->clearLayer("saliencyMap");
 
        SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
 
        Eigen::Matrix4f cameraToGlobal = FramedPosePtr::dynamicCast(robotPrx->getRobotNode(headFrameName)->getGlobalPose())->toEigen();
 
        std::vector<std::string> activeSaliencyMaps;
 
        if (names.size())
        {
            for (std::string n : names)
            {
                activeSaliencyMaps.push_back(n);
            }
        }
        else
        {
            getActiveSaliencyMaps(activeSaliencyMaps);
        }
 
        if (!activeSaliencyMaps.size())
        {
            return;
        }
 
 
        std::unique_lock lock(syncMutex);
 
        TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
        DebugDrawer24BitColoredPointCloud cloud;
        cloud.points.reserve(visibilityMaskGraphNodes->size());
        for (size_t i = 0; i < visibilityMaskGraphNodes->size(); i++)
        {
            float saliency = 0.0f;
            for (const std::string& n : activeSaliencyMaps)
            {
                saliency += saliencyMaps[n]->map[i];
            }
 
            saliency /= activeSaliencyMaps.size();
 
            CIntensityNode* visibilityNode = (CIntensityNode*) visibilityMaskGraphNodes->at(i);
            saliency *= visibilityNode->getIntensity();
 
            Eigen::Vector3d out;
            TSphereCoord pos = visibilityNode->getPosition();
            MathTools::convert(pos, out);
 
            Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
            pose.col(3).head<3>() = out.cast<float>() * 1000.0;
            pose = cameraToGlobal * pose;
 
            //        float r = std::min(1.0, (1.0 - saliency) * 0.2 + saliency * 0.8 + 0.2);
            //        float g = std::min(1.0, (1.0 - saliency) * 0.8 + saliency * 0.2 + 0.2);
            //        float b = std::min(1.0, (1.0 - saliency) * 0.2 + saliency * 0.2 + 0.2);
 
            if (saliency < visuSaliencyThreshold)
            {
                continue;
            }
            DebugDrawer24BitColoredPointCloudElement point;
            point.color = colorutils::HeatMapRGBColor(saliency);
            point.x = pose(0, 3);
            point.y = pose(1, 3);
            point.z = pose(2, 3);
            cloud.points.push_back(point);
        }
        cloud.pointSize = 10;
        drawer->set24BitColoredPointCloudVisu("saliencyMap", "SaliencyCloud", cloud);
 
    }
 
    void ViewSelection::clearSaliencySphere(const Ice::Current& c)
    {
        drawer->clearLayer("saliencyMap");
    }
 
    void ViewSelection::removeSaliencyMap(const std::string& name, const Ice::Current& c)
    {
        std::unique_lock lock(syncMutex);
 
        if (saliencyMaps.count(name))
        {
            saliencyMaps.erase(name);
        }
 
        condition.notify_all();
    }
 
    Ice::StringSeq ViewSelection::getSaliencyMapNames(const Ice::Current& c)
    {
        std::vector<std::string> names;
 
        std::unique_lock lock(syncMutex);
 
        for (const auto& p : saliencyMaps)
        {
            names.push_back(p.second->name);
        }
 
        return names;
    }
 
 
    PropertyDefinitionsPtr ViewSelection::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(new ViewSelectionPropertyDefinitions(getConfigIdentifier()));
    }
}