Implement ConvexHullGenerator

source/armarx/navigation/human/ConvexHullGenerator.cpp

 #include "ConvexHullGenerator.h"
 
-ConvexHullGenerator::ConvexHullGenerator()
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/register/point.hpp>
+#include <boost/geometry/multi/geometries/register/multi_point.hpp>
+
+using Point = std::pair<double, double>;
+BOOST_GEOMETRY_REGISTER_POINT_2D(Point, double, boost::geometry::cs::cartesian, first, second)
+
+namespace armarx::navigation::human
 {
 
+    ConvexHullGenerator::ConvexHullGenerator()
+    {
+
+    }
+
+    shapes::Polygon ConvexHullGenerator::createShape(const HumanGroup &group)
+    {
+        using BoostPoly = boost::geometry::model::polygon<Point>;
+        using GroupPoly = shapes::Polygon;
+
+        std::vector<Point> coordinates;
+        for (Human human : group.humans)
+        {
+            coordinates.push_back({human.pose.translation().x(), human.pose.translation().y()});
+        }
+
+        BoostPoly poly, hull;
+        poly.outer().assign(coordinates.begin(), coordinates.end());
+        boost::geometry::convex_hull(poly, hull);
+
+        std::vector<Eigen::Vector2f> vertices;
+
+        for (auto it = boost::begin(boost::geometry::exterior_ring(hull)); it != boost::end(boost::geometry::exterior_ring(hull)); ++it)
+        {
+            double x = boost::geometry::get<0>(*it);
+            double y = boost::geometry::get<1>(*it);
+
+            vertices.push_back(Eigen::Vector2f(x, y));
+        }
+
+        return {.vertices = vertices};
+    }
 }
+

source/armarx/navigation/human/ConvexHullGenerator.h

@@ -21,6 +21,8 @@
 
 #pragma once
 
+#include "GroupShapeGenerator.h"
+
 #include <armarx/navigation/human/types.h>
 
 
@@ -32,9 +34,11 @@ namespace armarx::navigation::human
      * @brief A GroupShapeGenerator that defines the group shape of a set of humans as the
      * convex hull of their positions in 2D space.
      */
-    class ConvexHullGenerator
+    class ConvexHullGenerator : GroupShapeGenerator
     {
     public:
         ConvexHullGenerator();
+
+        virtual shapes::Polygon createShape(const HumanGroup &group);
     };
 }