Implement polygon drawing

source/armarx/navigation/components/dynamic_scene_provider/Component.cpp

@@ -366,6 +366,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
         using HumanGroup = armarx::navigation::human::HumanGroup;
         std::vector<HumanGroup> groups = humanGrouper.getCurrentGroups();
 
+        ARMARX_INFO << "Detected " << groups.size() << " human groups";
+
         humanWriterPlugin->get().store(groups, getName(), timestamp);
     }
 

source/armarx/navigation/components/navigation_memory/Visu.cpp

@@ -215,24 +215,17 @@ namespace armarx::navigation::memory
 
             for (const auto& humanGroup : humanGroups)
             {
-                viz::Polygon polygon();
-                polygon.addPoints(humanGroup.shape.vertices);
-                // viz::Cylinder cylinder(std::to_string(layer.size()));
-                // cylinder.fromTo(conv::to3D(human.pose.translation()),
-                //                 conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
-
-
-                // cylinder.color(simox::Color::orange());
-                // cylinder.radius(300);
-                // layer.add(cylinder);
-
+                viz::Polygon polygon(std::to_string(layer.size()));
 
-                viz::Robot mmm(std::to_string(layer.size()));
-                mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
-                mmm.pose(conv::to3D(human.pose) * human_T_mmm);
-                mmm.scale(1.7); // 1.7m
-                mmm.overrideColor(viz::Color::orange(255, 100));
-                layer.add(mmm);
+                const std::vector<Eigen::Vector2f> verts2D = humanGroup.shape.vertices;
+                std::vector<Eigen::Vector3f> verts3D;
+                for (const auto& v2d : verts2D)
+                {
+                    verts3D.emplace_back(Eigen::Vector3f(v2d.x(), v2d.y(), 0));
+                }
+                polygon.points(verts3D);
+                polygon.color(simox::Color::yellow());
+                layer.add(polygon);
             }
         }
 
@@ -357,7 +350,7 @@ namespace armarx::navigation::memory
 
         for (const auto& [providerName, humanGroups] : namedProviderHumanGroups)
         {
-            viz::Layer& layer = layers.emplace_back(arviz.layer("humans_" + providerName));
+            viz::Layer& layer = layers.emplace_back(arviz.layer("humanGroups_" + providerName));
             visualize(humanGroups, layer);
         }
     }