ViewSelection.h

/*
 * 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
 */

#ifndef _ARMARX_COMPONENT_RobotComponents_ViewSelection_H
#define _ARMARX_COMPONENT_RobotComponents_ViewSelection_H


#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>

#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/components/units/HeadIKUnit.h>

#include <MemoryX/interface/workingmemory/AbstractWorkingMemoryInterface.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>

#include <RobotComponents/interface/components/ViewSelectionInterface.h>

#include <Eigen/Geometry>


// graph stuff
#include "EarlyVisionGraph/IntensityGraph.h"
#include "EarlyVisionGraph/GraphPyramidLookupTable.h"
#include "EarlyVisionGraph/MathTools.h"
//#include "EarlyVisionGraph/GraphTriangulation.h"
//#include "EarlyVisionGraph/GraphGenerator.h"


namespace armarx
{
    /**
     * @class ViewSelectionPropertyDefinitions
     * @brief
     */
    class ViewSelectionPropertyDefinitions:
        public ComponentPropertyDefinitions
    {
    public:
        ViewSelectionPropertyDefinitions(std::string prefix):
            ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
            defineOptionalProperty<std::string>("HeadIKUnitName", "HeadIKUnit", "Name of the head IK unit component that should be used");
            defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of the WorkingMemory component that should be used");
            defineOptionalProperty<std::string>("PriorKnowledgeName", "PriorKnowledge", "Name of the PriorKnowledge component that should be used");
            defineOptionalProperty<std::string>("HeadIKKinematicChainName", "IKVirtualGaze", "Name of the kinematic chain for the head IK");
            defineOptionalProperty<std::string>("HeadFrameName", "Head Base", "Name of the frame of the head base in the robot model");
            defineOptionalProperty<std::string>("CameraFrameName", "VirtualCentralGaze", "Name of the frame of the head base in the robot model");
            defineOptionalProperty<float>("MaxObjectDistance", 1500.f, "Maximum distance of objects the head to be considered for observation");
            defineOptionalProperty<float>("HalfCameraOpeningAngle", 12.0 * M_PI / 180.0, "0.5 * minimal opening angles of the used cameras");
            defineOptionalProperty<int>("SleepingTimeBetweenViewDirectionChanges", 2500, "Time between two view changes, to keep the head looking into one direction for a while (in ms)");
            defineOptionalProperty<bool>("ActiveAtStartup", true, "Decide whether the automatic view selection will be activated (can be changed via the proxy during runtime)");
            defineOptionalProperty<bool>("VisualizeViewDirection", false, "Draw view ray on DebugLayer.");
            defineOptionalProperty<float>("MaxOverallHeadTiltAngle", 55.0f, "Maximal angle the head and eyes can look down (in degrees)");
            defineOptionalProperty<float>("CentralHeadTiltAngle", 110.0f, "Defines the height direction that will be considered 'central' in the reachable area of the head (in degrees). Default is looking 20 degrees downwards");
            defineOptionalProperty<float>("ProbabilityToLookForALostObject", 0.03f, "Probability that one of the objects that have been seen but could later not been localized again will be included in the view selection");
            defineOptionalProperty<float>("RandomNoiseLevel", 1.0f, "Maximum for the random noise that will be added to the localization necessities");

            //defineRequiredProperty<std::string>("KinematicUnitName", "Name of the kinematic unit that should be used");
            //defineRequiredProperty<std::string>("KinematicUnitObserverName", "Name of the kinematic unit observer that should be used");
            //defineOptionalProperty<std::string>("TCPControlUnitName", "TCPControlUnit", "Name of the tcp control unit component that should be used");
            //defineOptionalProperty<std::string>("ObjectMemoryObserverName", "ObjectMemoryObserver", "Name of the ObjectMemoryObserver component that should be used");
            //defineOptionalProperty<std::string>("PriorKnowledgeName", "PriorKnowledge", "Name of the PriorKnowledge component that should be used");
        }
    };

    /**
     * @defgroup Component-ViewSelection ViewSelection
     * @ingroup RobotComponents-Components
     * @brief The ViewSelection component controls the head of the robot with inverse kinematics based on the uncertainty
     * of the current requested object locations.
     * The uncertainty of objects grow based on their motion model and the timed passed since the last localization.
     * It can be activated or deactivated with the Ice interface and given manual target positions to look at.
     */

    /**
     * @ingroup Component-ViewSelection
     * @brief The ViewSelection class
     */
    class ViewSelection :
        virtual public Component,
        virtual public ViewSelectionInterface
    {
    public:
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        virtual std::string getDefaultName() const
        {
            return "ViewSelection";
        }

    protected:
        /**
         * @see armarx::ManagedIceObject::onInitComponent()
         */
        virtual void onInitComponent();

        /**
         * @see armarx::ManagedIceObject::onConnectComponent()
         */
        virtual void onConnectComponent();

        /**
         * @see armarx::ManagedIceObject::onDisconnectComponent()
         */
        virtual void onDisconnectComponent();

        /**
         * @see armarx::ManagedIceObject::onExitComponent()
         */
        virtual void onExitComponent();

        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        virtual PropertyDefinitionsPtr createPropertyDefinitions();

        virtual void addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c = Ice::Current());

        virtual void activateAutomaticViewSelection(const Ice::Current& c = Ice::Current())
        {
            ARMARX_INFO << "activating automatic view selection";
            doAutomaticViewSelection = true;
        }

        virtual void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::Current())
        {
            ARMARX_INFO << "DEactivating automatic view selection";
            doAutomaticViewSelection = false;
        }

    private:

        void process();
        void addSaliencyRecursive(const int currentNodeIndex, const float saliency, const TSphereCoord objectSphereCoord, const int objectIndex, const float maxDistanceOnArc);
        void setRandomNoise(const float centralAngleForVerticalDirection, const float directionVariabilityFactor = 1.0f);

        RobotStateComponentInterfacePrx robotStateComponent;
        memoryx::WorkingMemoryInterfacePrx memoryProxy;
        memoryx::PriorKnowledgeInterfacePrx priorKnowledge;
        memoryx::ObjectInstanceMemorySegmentBasePrx objectInstancesProxy;
        memoryx::ObjectClassMemorySegmentBasePrx objectClassesProxy;
        HeadIKUnitInterfacePrx headIKUnitProxy;
        DebugDrawerInterfacePrx drawer;

        std::string headIKKinematicChainName;
        std::string headFrameName, cameraFrameName;

        float randomNoiseLevel;
        float centralHeadTiltAngle;

        armarx::PeriodicTask<ViewSelection>::pointer_type processorTask;

        CIntensityGraph* saliencyEgosphereGraph;
        CIntensityGraph* visibilityMaskGraph;
        CIntensityGraph* randomNoiseGraph;
        CGraphPyramidLookupTable* graphLookupTable;
        std::vector<int> nodeVisitedForObject;
        float halfCameraOpeningAngle;
        float deviationFromCameraCenterFactor;
        float sleepingTimeBetweenViewDirectionChanges;
        IceUtil::Time timeOfLastViewChange;

        bool drawViewDirection;

        armarx::Mutex manualViewTargetsMutex;
        std::vector<FramedPositionBasePtr> manualViewTargets;
        bool doAutomaticViewSelection;

        std::map<std::string, memoryx::ObjectClassPtr> recognizableObjClasses;
        Eigen::Vector3f offsetToHeadCenter;
    };
}

#endif