added saliency map visualization + gui plugin for ViewSelection

source/RobotAPI/components/ViewSelection/ViewSelection.cpp

@@ -142,7 +142,6 @@ void ViewSelection::onExitComponent()
 
 ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
 {
-
     boost::mutex::scoped_lock lock(syncMutex);
 
     IceUtil::Time currentTime =  armarx::TimeUtil::GetTime();
@@ -157,7 +156,7 @@ ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
                 activeSaliencyMaps.push_back(p.second->name);
             }
         }
-        else
+        else if ((currentTime - TimestampVariantPtr::dynamicCast(p.second->timeAdded)->toTime()) < IceUtil::Time::seconds(5))
         {
             activeSaliencyMaps.push_back(p.second->name);
         }
@@ -177,6 +176,7 @@ ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
     // find the direction with highest saliency
     int maxIndex = -1;
     float maxSaliency = std::numeric_limits<float>::min();
+
     for (size_t i = 0; i < visibilityMaskGraphNodes->size(); i++)
     {
         float saliency = 0.0f;
@@ -240,6 +240,8 @@ void ViewSelection::process()
         viewTarget = nextAutomaticViewTarget();
     }
 
+
+
     /*
     //discard outdated view targets
     IceUtil::Time currentTime = armarx::TimeUtil::GetTime();
@@ -361,6 +363,7 @@ ViewTargetList ViewSelection::getManualViewTargets(const Ice::Current& c)
 
     while (!temp.empty())
     {
+
         result.push_back(temp.top());
 
         temp.pop();
@@ -377,9 +380,77 @@ void armarx::ViewSelection::updateSaliencyMap(const SaliencyMapBasePtr& map, con
 
     hasNewSaliencyMap = true;
 
+    map->timeAdded = TimestampVariant::nowPtr();
+
     saliencyMaps[map->name] = map;
 
     condition.notify_all();
+
+}
+
+void ViewSelection::drawSaliencyMap(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();
+
+    IceUtil::Time currentTime =  armarx::TimeUtil::GetTime();
+    std::vector<std::string> activeSaliencyMaps;
+
+    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);
+        }
+    }
+
+
+    TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
+
+
+    for (size_t i = 0; i < visibilityMaskGraphNodes->size(); i += 10)
+    {
+        float saliency = 0.0f;
+        for (const std::string & n : activeSaliencyMaps)
+        {
+            saliency += saliencyMaps[n]->map[i];
+        }
+
+        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);
+
+        Eigen::Vector3f dim = Eigen::Vector3f::Ones() * 15;
+        DrawColor color = {r, g, b, 1.0};
+
+        drawer->setBoxVisu("saliencyMap", "node_" + std::to_string(i), new Pose(pose), new Vector3(dim),  color);
+
+    }
+
 }
 
 void ViewSelection::removeSaliencyMap(const string& name, const Ice::Current& c)

source/RobotAPI/components/ViewSelection/ViewSelection.h

@@ -180,6 +180,8 @@ namespace armarx
 
         void removeSaliencyMap(const std::string& name, const Ice::Current& c = ::Ice::Current());
 
+        void drawSaliencyMap(const Ice::Current& c = ::Ice::Current());
+
     private:
 
         void process();

source/RobotAPI/gui-plugins/CMakeLists.txt

@@ -8,4 +8,5 @@ add_subdirectory(RobotViewerPlugin)
 
 
 
-add_subdirectory(DebugDrawerViewer)
\ No newline at end of file
+add_subdirectory(DebugDrawerViewer)
+add_subdirectory(ViewSelection)
\ No newline at end of file

source/RobotAPI/gui-plugins/ViewSelection/CMakeLists.txt

+
+armarx_set_target("ViewSelectionGuiPlugin")
+
+find_package(Qt4 COMPONENTS QtCore QtGui QtDesigner)
+
+armarx_build_if(QT_FOUND "Qt not available")
+# ArmarXGui gets included through depends_on_armarx_package(ArmarXGui "OPTIONAL")
+# in the toplevel CMakeLists.txt
+armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
+
+
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+if(QT_FOUND)
+    include(${QT_USE_FILE})
+endif()
+
+set(SOURCES
+./ViewSelectionGuiPlugin.cpp ./ViewSelectionWidgetController.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@GuiPlugin.cpp @COMPONENT_PATH@/@COMPONENT_NAME@WidgetController.cpp
+)
+
+set(HEADERS
+./ViewSelectionGuiPlugin.h ./ViewSelectionWidgetController.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@GuiPlugin.h @COMPONENT_PATH@/@COMPONENT_NAME@WidgetController.h
+)
+
+set(GUI_MOC_HDRS ${HEADERS})
+
+set(GUI_UIS
+./ViewSelectionWidget.ui
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@Widget.ui
+)
+
+# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
+set(COMPONENT_LIBS RobotAPIInterfaces ${QT_LIBRARIES})
+
+if(ArmarXGui_FOUND)
+	armarx_gui_library(ViewSelectionGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
+endif()

source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.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    RobotAPI::gui-plugins::ViewSelectionGuiPlugin
+ * \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 "ViewSelectionGuiPlugin.h"
+
+#include "ViewSelectionWidgetController.h"
+
+using namespace armarx;
+
+ViewSelectionGuiPlugin::ViewSelectionGuiPlugin()
+{
+    addWidget < ViewSelectionWidgetController > ();
+}
+
+Q_EXPORT_PLUGIN2(armarx_gui_ViewSelectionGuiPlugin, ViewSelectionGuiPlugin)

source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.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    RobotAPI::gui-plugins::ViewSelection
+ * \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
+ */
+
+#ifndef _ARMARX_RobotAPI_ViewSelection_GuiPlugin_H
+#define _ARMARX_RobotAPI_ViewSelection_GuiPlugin_H
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+namespace armarx
+{
+    /**
+     * \class ViewSelectionGuiPlugin
+     * \ingroup ArmarXGuiPlugins
+     * \brief ViewSelectionGuiPlugin brief description
+     *
+     * Detailed description
+     */
+    class ARMARXCOMPONENT_IMPORT_EXPORT ViewSelectionGuiPlugin:
+		public armarx::ArmarXGuiPlugin
+    {
+    public:
+        /**
+         * All widgets exposed by this plugin are added in the constructor
+         * via calls to addWidget()
+         */
+        ViewSelectionGuiPlugin();
+    };
+}
+
+#endif

source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidget.ui

+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>ViewSelectionWidget</class>
+ <widget class="QWidget" name="ViewSelectionWidget">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>400</width>
+    <height>300</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>ViewSelectionWidget</string>
+  </property>
+  <widget class="QWidget" name="verticalLayoutWidget">
+   <property name="geometry">
+    <rect>
+     <x>10</x>
+     <y>10</y>
+     <width>360</width>
+     <height>80</height>
+    </rect>
+   </property>
+   <layout class="QVBoxLayout" name="verticalLayout">
+    <item>
+     <layout class="QHBoxLayout" name="horizontalLayout">
+      <item>
+       <widget class="QCheckBox" name="checkBox">
+        <property name="text">
+         <string>Enable automatic view selection</string>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <widget class="QPushButton" name="pushButton">
+        <property name="text">
+         <string>Draw Saliency Map</string>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </item>
+   </layout>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.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    RobotAPI::gui-plugins::ViewSelectionWidgetController
+ * \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 "ViewSelectionWidgetController.h"
+
+#include <string>
+
+using namespace armarx;
+
+ViewSelectionWidgetController::ViewSelectionWidgetController()
+{
+    widget.setupUi(getWidget());
+
+    connect(widget.pushButton, SIGNAL(clicked()), this, SLOT(triggerSaliencyMapVisualization()));
+}
+
+
+ViewSelectionWidgetController::~ViewSelectionWidgetController()
+{
+
+}
+
+
+void ViewSelectionWidgetController::loadSettings(QSettings* settings)
+{
+
+
+}
+
+void ViewSelectionWidgetController::saveSettings(QSettings* settings)
+{
+
+}
+
+
+void ViewSelectionWidgetController::onInitComponent()
+{
+    usingProxy("ViewSelection");
+    usingTopic("ViewSelectionObserver");
+}
+
+
+void ViewSelectionWidgetController::onConnectComponent()
+{
+    viewSelection = getProxy<ViewSelectionInterfacePrx>("ViewSelection");
+
+    widget.checkBox->setChecked(viewSelection->isEnabledAutomaticViewSelection());
+}
+
+
+void armarx::ViewSelectionWidgetController::onActivateAutomaticViewSelection(const Ice::Current &)
+{
+    widget.checkBox->setChecked(true);
+}
+
+void armarx::ViewSelectionWidgetController::onDeactivateAutomaticViewSelection(const Ice::Current &)
+{
+    widget.checkBox->setChecked(false);
+}
+
+void armarx::ViewSelectionWidgetController::nextViewTarget(Ice::Long, const Ice::Current &)
+{
+}
+
+void ViewSelectionWidgetController::triggerSaliencyMapVisualization()
+{
+    viewSelection->drawSaliencyMap();
+}

source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.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    RobotAPI::gui-plugins::ViewSelectionWidgetController
+ * @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
+ */
+
+#ifndef _ARMARX_RobotAPI_ViewSelection_WidgetController_H
+#define _ARMARX_RobotAPI_ViewSelection_WidgetController_H
+
+#include "ui_ViewSelectionWidget.h"
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <RobotAPI/interface/components/ViewSelectionInterface.h>
+
+namespace armarx
+{
+    /**
+    \page ArmarXGui-GuiPlugins-ViewSelection ViewSelection
+    \brief The ViewSelection allows visualizing ...
+
+    \image html ViewSelection.png
+    The user can
+
+    API Documentation \ref ViewSelectionWidgetController
+
+    \see ViewSelectionGuiPlugin
+    */
+
+    /**
+     * \class ViewSelectionWidgetController
+     * \brief ViewSelectionWidgetController brief one line description
+     *
+     * Detailed description
+     */
+    class ARMARXCOMPONENT_IMPORT_EXPORT
+        ViewSelectionWidgetController:
+    public armarx::ArmarXComponentWidgetController,
+    public armarx::ViewSelectionObserver
+    {
+        Q_OBJECT
+
+    public:
+        /**
+         * Controller Constructor
+         */
+        explicit ViewSelectionWidgetController();
+
+        /**
+         * Controller destructor
+         */
+        virtual ~ViewSelectionWidgetController();
+
+        /**
+         * @see ArmarXWidgetController::loadSettings()
+         */
+        virtual void loadSettings(QSettings* settings);
+
+        /**
+         * @see ArmarXWidgetController::saveSettings()
+         */
+        virtual void saveSettings(QSettings* settings);
+
+
+        void onActivateAutomaticViewSelection(const Ice::Current &c = Ice::Current());
+        void onDeactivateAutomaticViewSelection(const Ice::Current &c = Ice::Current());
+        void nextViewTarget(Ice::Long, const Ice::Current &c = Ice::Current());
+
+        /**
+         * Returns the Widget name displayed in the ArmarXGui to create an
+         * instance of this class.
+         */
+        virtual QString getWidgetName() const
+        {
+            return "RobotControl.ViewSelection";
+        }
+
+        /**
+         * \see armarx::Component::onInitComponent()
+         */
+        virtual void onInitComponent();
+
+        /**
+         * \see armarx::Component::onConnectComponent()
+         */
+        virtual void onConnectComponent();
+
+    public slots:
+
+        void triggerSaliencyMapVisualization();
+        /* QT slot declarations */
+
+    signals:
+        /* QT signal declarations */
+
+    private:
+        /**
+         * Widget Form
+         */
+        Ui::ViewSelectionWidget widget;
+
+        ViewSelectionInterfacePrx viewSelection;
+
+    };
+}
+
+#endif

source/RobotAPI/interface/components/ViewSelectionInterface.ice

@@ -77,6 +77,8 @@ module armarx
 
         void updateSaliencyMap(SaliencyMapBase map);
         void removeSaliencyMap(string name);
+
+        void drawSaliencyMap();
     };