extended viewselection visu

source/RobotAPI/components/ViewSelection/ViewSelection.cpp

@@ -29,7 +29,7 @@
 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
+#include <RobotAPI/libraries/core/math/ColorUtils.h>
 
 using namespace armarx;
 
@@ -310,18 +310,10 @@ void ViewSelection::process()
         }
 
         viewSelectionObserver->nextViewTarget(timeOfLastViewChange.toMilliSeconds() + duration);
-
-
-
-        /*
-         *
-        while (duration > 0 && !hasNewSaliencyMap)
+        if (getProperty<bool>("ShowVisuAlways").getValue())
         {
-            //condition.wait_for(syncMutex , boost::chrono::milliseconds(100));
-            duration -= 100;
+            drawSaliencySphere(getSaliencyMapNames());
         }
-         */
-
         boost::this_thread::sleep(boost::posix_time::milliseconds(duration));
     }
     else
@@ -404,7 +396,7 @@ void ViewSelection::drawSaliencySphere(const ::Ice::StringSeq& names, const Ice:
 {
     ARMARX_LOG << "visualizing saliency map";
 
-    drawer->clearLayer("saliencyMap");
+    //    drawer->clearLayer("saliencyMap");
 
     SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
 
@@ -439,10 +431,11 @@ void ViewSelection::drawSaliencySphere(const ::Ice::StringSeq& names, const Ice:
         }
     }
 
+    float threshold = getProperty<float>("VisuSaliencyThreshold");
 
     TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
-
-
+    DebugDrawer24BitColoredPointCloud cloud;
+    cloud.points.reserve(visibilityMaskGraphNodes->size() / 10);
     for (size_t i = 0; i < visibilityMaskGraphNodes->size(); i += 10)
     {
         float saliency = 0.0f;
@@ -462,18 +455,23 @@ void ViewSelection::drawSaliencySphere(const ::Ice::StringSeq& names, const Ice:
         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->setLineSetVisu();
-
-        drawer->setBoxVisu("saliencyMap", "node_" + std::to_string(i), new Pose(pose), new Vector3(dim),  color);
+        //        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 < threshold)
+        {
+            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);
 
 }
 

source/RobotAPI/components/ViewSelection/ViewSelection.h

@@ -88,6 +88,8 @@ namespace armarx
             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<bool>("ShowVisuAlways", false, "If true the saliency map is drawn into the debug drawer on each iteration.");
+            defineOptionalProperty<float>("VisuSaliencyThreshold", 0.7f, "Minimum saliency threshold for a point to be shown in debug visu");
         }
     };