familiar object visu: label visu

source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp

@@ -42,6 +42,7 @@ namespace armarx::armem::server::obj::familiar_object_instance
         defs->optional(visualizePointCloud, prefix + "visualizePointCloud", "");
         defs->optional(visualizePose, prefix + "visualizePose", "");
         defs->optional(visualizeBoundingBox, prefix + "visualizeBoundingBox", "");
+        defs->optional(visualizeLabels, prefix + "visualizeLabels", "");
 
         defs->optional(objectFrames, prefix + "objectFrames", "Enable showing object frames.");
         defs->optional(
@@ -73,6 +74,7 @@ namespace armarx::armem::server::obj::familiar_object_instance
         {
             auto layerPose = arviz.layer("familiar_objects/pose/" + providerName);
             auto layerPointCloud = arviz.layer("familiar_objects/points/" + providerName);
+            auto layerLabels = arviz.layer("familiar_objects/labels/" + providerName);
             auto layerBox = arviz.layer("familiar_objects/box/" + providerName);
 
             auto confidenceCmap = simox::color::cmaps::viridis();
@@ -108,6 +110,19 @@ namespace armarx::armem::server::obj::familiar_object_instance
                         layerPointCloud.add(points);
                     }
 
+                    if (visualizeLabels)
+                    {
+                        Eigen::Isometry3f pose{familiarObject.poseGlobal->pose};
+
+                        // gently above object
+                        // FIXME derive from bounding box
+                        pose.translation().z() += 200;
+
+                        const std::string name = familiarObject.objectID.className + "/" + familiarObject.objectID.instanceName;
+
+                        layerLabels.add(viz::Text(name).pose(pose).scale(20));
+                    }
+
                     if (visualizeBoundingBox)
                     {
 
@@ -138,6 +153,7 @@ namespace armarx::armem::server::obj::familiar_object_instance
 
             layers.push_back(layerPose);
             layers.push_back(layerPointCloud);
+            layers.push_back(layerLabels);
             layers.push_back(layerBox);
         }
 

source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h

@@ -53,6 +53,7 @@ namespace armarx::armem::server::obj::familiar_object_instance
         bool visualizePointCloud = true;
         bool visualizeBoundingBox = true;
         bool visualizePose = true;
+        bool visualizeLabels = true;
 
         bool objectFrames = false;
         float objectFramesScale = 1.0;