Use ellipsoids saved in clusters instead of iterating over points

source/armarx/navigation/human/HumanTracker.cpp

@@ -59,13 +59,7 @@ namespace armarx::navigation::human
 
             if (getClusterSize(cluster) <= 2 * parameters.maxFootprintSize)
             {
-                //calculate center of cluster
-                Eigen::Vector2f clusterCenter;
-                for (auto& point : cluster.points)
-                {
-                    clusterCenter += point;
-                }
-                clusterCenter /= cluster.points.size();
+                Eigen::Vector2f clusterCenter = cluster.ellipsoid.pose.translation();
 
                 potentialFootprints.push_back(AdvancedCluster{
                     .center = clusterCenter, .cluster = cluster, .associated = false});
@@ -99,21 +93,13 @@ namespace armarx::navigation::human
             // found both footprints, so associate both footprints and update human with new pose
             else
             {
-                //calculate center of the points of both footprints combined
-                auto& footprint1 = posDistance.oldHuman->associatedCluster->cluster.points;
-                auto& footprint2 = posDistance.newCluster->cluster.points;
+                //calculate center between both footprints
+                auto& footprint1 = posDistance.oldHuman->associatedCluster->cluster;
+                auto& footprint2 = posDistance.newCluster->cluster;
 
-                //TODO more efficient by just calculating mean between two circle centers for example
-                Eigen::Vector2f centerPos;
-                for (auto& point : footprint1)
-                {
-                    centerPos += point;
-                }
-                for (auto& point : footprint2)
-                {
-                    centerPos += point;
-                }
-                centerPos /= (footprint1.size() + footprint2.size());
+                Eigen::Vector2f centerPos = (footprint1.ellipsoid.pose.translation() +
+                                             footprint2.ellipsoid.pose.translation()) /
+                                            2;
 
                 //create pose with orientation from old human
                 core::Pose2D pose = core::Pose2D::Identity();
@@ -357,18 +343,7 @@ namespace armarx::navigation::human
     float
     HumanTracker::getClusterSize(Cluster cluster)
     {
-        //TODO more effictient by using radius of circle saved in cluster for example
-        float maxSize = 0;
-        for (auto it1 = cluster.points.begin(); it1 != cluster.points.end();)
-        {
-            auto& point1 = *it1;
-            for (auto it2 = it1 + 1; it2 != cluster.points.end();)
-            {
-                auto& point2 = *it2;
-                maxSize = std::max(maxSize, (point2 - point1).norm());
-            }
-        }
-        return maxSize;
+        return std::max(cluster.ellipsoid.radii.x(), cluster.ellipsoid.radii.y());
     }
 
     void