Add convex hull to test clusters

source/armarx/navigation/human/test/human_tracker_test.cpp

@@ -103,11 +103,9 @@ namespace armarx::navigation::human
     {
         using Point = Eigen::Vector2f;
 
-        const std::vector<Point> relOffsets = {Point(0.01, 0.003),
-                                               Point(-0.02, 0.007),
-                                               Point(-0.01, -0.01),
-                                               Point(0.015, 0.009),
-                                               Point(0.002, 0.001)};
+        // These are convex
+        const std::vector<Point> relOffsets = {
+            Point(0.01, 0.003), Point(-0.02, 0.007), Point(-0.01, -0.01), Point(0.015, 0.009)};
 
         const Eigen::Vector2f leftFootPos = pos + Eigen::Vector2f(0, 0.1);
         const Eigen::Vector2f rightFootPos = pos + Eigen::Vector2f(0, -0.1);
@@ -121,6 +119,12 @@ namespace armarx::navigation::human
             rightFootPoints.emplace_back(rightFootPos + offset);
             i++;
         }
+        std::vector<Point> leftFootHull = std::vector<Point>(leftFootPoints);
+        std::vector<Point> rightFootHull = std::vector<Point>(rightFootPoints);
+
+        // Add one more non-convex point to the cluster points
+        leftFootPoints.emplace_back(leftFootPos + Point(0.002, 0.001));
+        rightFootPoints.emplace_back(rightFootPos + Point(0.002, 0.003));
 
         using Pose = Eigen::Isometry3f;
         Pose leftFootPose = Pose::Identity();
@@ -138,10 +142,14 @@ namespace armarx::navigation::human
         const Circle leftFootCircle = {.center = leftFootPos, .radius = 0.1};
         const Circle rightFootCircle = {.center = rightFootPos, .radius = 0.1};
 
-        const Cluster leftFootCluster = {
-            .circle = leftFootCircle, .ellipsoid = leftFootEllipse, .points = leftFootPoints};
-        const Cluster rightFootCluster = {
-            .circle = rightFootCircle, .ellipsoid = rightFootEllipse, .points = rightFootPoints};
+        const Cluster leftFootCluster = {.convexHull = leftFootHull,
+                                         .circle = leftFootCircle,
+                                         .ellipsoid = leftFootEllipse,
+                                         .points = leftFootPoints};
+        const Cluster rightFootCluster = {.convexHull = rightFootHull,
+                                          .circle = rightFootCircle,
+                                          .ellipsoid = rightFootEllipse,
+                                          .points = rightFootPoints};
 
         const std::vector<Cluster> clusters =
             std::vector<Cluster>{leftFootCluster, rightFootCluster};