Use new implemented features, remove todos

source/armarx/navigation/human/HumanTracker.cpp

@@ -45,8 +45,6 @@ namespace armarx::navigation::human
     std::vector<Cluster>
     HumanTracker::update(const LaserMeasurement& measurements)
     {
-        // TODO do we receive cluster with correct threshold / expected size / as accurate as that
-        // they distinguish multiple footprints?
 
         prepareTrackedHumans(measurements.detectionTime);
 
@@ -56,14 +54,10 @@ namespace armarx::navigation::human
         std::vector<Cluster> unusedClusters;
         for (const auto& cluster : measurements.clusters)
         {
-
             if (getClusterSize(cluster) <= 2 * parameters.maxFootprintSize)
             {
-                // TODO: Why is the ellipse translation() here 3D? Can we just use the first 2 entries?
-                Eigen::Vector2f clusterCenter = cluster.ellipsoid.pose.translation().segment(0, 2);
-
                 potentialFootprints.push_back(AdvancedCluster{
-                    .center = clusterCenter, .cluster = cluster, .associated = false});
+                    .center = getClusterCenter(cluster), .cluster = cluster, .associated = false});
             }
             else
             {
@@ -97,15 +91,10 @@ namespace armarx::navigation::human
                 auto& footprint1 = posDistance.oldHuman->associatedCluster->cluster;
                 auto& footprint2 = posDistance.newCluster->cluster;
 
-                // TODO: Why is the ellipse translation() here 3D? Can we just use the first 2 entries?
-                Eigen::Vector2f centerPos =
-                    (footprint1.ellipsoid.pose.translation().segment(0, 2) +
-                     footprint2.ellipsoid.pose.translation().segment(0, 2)) /
-                    2;
-
                 //create pose with orientation from old human
                 core::Pose2D pose = core::Pose2D::Identity();
-                pose.translation() = centerPos;
+                pose.translation() =
+                    (getClusterCenter(footprint1) + getClusterCenter(footprint2)) / 2;
                 pose.linear() = posDistance.oldHuman->humanFilter.get().pose.linear();
 
 
@@ -336,14 +325,14 @@ namespace armarx::navigation::human
     float
     HumanTracker::getClusterSize(Cluster cluster)
     {
-        return std::max(cluster.ellipsoid.radii.x(), cluster.ellipsoid.radii.y());
+        return 2 * cluster.circle.radius;
     }
 
     Eigen::Vector2f
     HumanTracker::getClusterCenter(Cluster cluster)
     {
         Eigen::Vector2f center;
-        for (auto& point : cluster.points)
+        for (auto& point : cluster.convexHull)
         {
             center += point;
         }