From 959aa830afb795b5ed383da86048a00b400e1221 Mon Sep 17 00:00:00 2001
From: Markus Grotz <markus.grotz@kit.edu>
Date: Wed, 13 Jul 2016 15:14:41 +0200
Subject: [PATCH] added saliency map visualization + gui plugin for
ViewSelection
---
.../ViewSelection/ViewSelection.cpp | 75 ++++++++++-
.../components/ViewSelection/ViewSelection.h | 2 +
source/RobotAPI/gui-plugins/CMakeLists.txt | 3 +-
.../gui-plugins/ViewSelection/CMakeLists.txt | 40 ++++++
.../ViewSelection/ViewSelectionGuiPlugin.cpp | 34 +++++
.../ViewSelection/ViewSelectionGuiPlugin.h | 51 +++++++
.../ViewSelection/ViewSelectionWidget.ui | 49 +++++++
.../ViewSelectionWidgetController.cpp | 87 ++++++++++++
.../ViewSelectionWidgetController.h | 126 ++++++++++++++++++
.../components/ViewSelectionInterface.ice | 2 +
10 files changed, 466 insertions(+), 3 deletions(-)
create mode 100644 source/RobotAPI/gui-plugins/ViewSelection/CMakeLists.txt
create mode 100644 source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.cpp
create mode 100644 source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.h
create mode 100644 source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidget.ui
create mode 100644 source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.cpp
create mode 100644 source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.h
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
index c1d61f14b..5f16a3173 100644
--- a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
+++ b/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)
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.h b/source/RobotAPI/components/ViewSelection/ViewSelection.h
index 50a114c47..d04451875 100644
--- a/source/RobotAPI/components/ViewSelection/ViewSelection.h
+++ b/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();
diff --git a/source/RobotAPI/gui-plugins/CMakeLists.txt b/source/RobotAPI/gui-plugins/CMakeLists.txt
index 408c06b2d..8ec2cbb05 100644
--- a/source/RobotAPI/gui-plugins/CMakeLists.txt
+++ b/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
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/CMakeLists.txt b/source/RobotAPI/gui-plugins/ViewSelection/CMakeLists.txt
new file mode 100644
index 000000000..1110c902b
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ViewSelection/CMakeLists.txt
@@ -0,0 +1,40 @@
+
+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()
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.cpp b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.cpp
new file mode 100644
index 000000000..037335322
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.cpp
@@ -0,0 +1,34 @@
+/*
+ * 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)
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.h b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.h
new file mode 100644
index 000000000..fc3c6de87
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.h
@@ -0,0 +1,51 @@
+/*
+ * 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
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidget.ui b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidget.ui
new file mode 100644
index 000000000..e5a6f669f
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidget.ui
@@ -0,0 +1,49 @@
+<?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>
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.cpp b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.cpp
new file mode 100644
index 000000000..921c2f136
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.cpp
@@ -0,0 +1,87 @@
+/*
+ * 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();
+}
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.h b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.h
new file mode 100644
index 000000000..9e6ad1ab8
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.h
@@ -0,0 +1,126 @@
+/*
+ * 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
diff --git a/source/RobotAPI/interface/components/ViewSelectionInterface.ice b/source/RobotAPI/interface/components/ViewSelectionInterface.ice
index 8db92bbd9..19fb28c80 100755
--- a/source/RobotAPI/interface/components/ViewSelectionInterface.ice
+++ b/source/RobotAPI/interface/components/ViewSelectionInterface.ice
@@ -77,6 +77,8 @@ module armarx
void updateSaliencyMap(SaliencyMapBase map);
void removeSaliencyMap(string name);
+
+ void drawSaliencyMap();
};
--
GitLab