Implement new cluster center function as ellipsoids are 3d objects

source/armarx/navigation/human/HumanTracker.cpp

@@ -339,6 +339,18 @@ namespace armarx::navigation::human
         return std::max(cluster.ellipsoid.radii.x(), cluster.ellipsoid.radii.y());
     }
 
+    Eigen::Vector2f
+    HumanTracker::getClusterCenter(Cluster cluster)
+    {
+        Eigen::Vector2f center;
+        for (auto& point : cluster.points)
+        {
+            center += point;
+        }
+        center /= cluster.points.size();
+        return center;
+    }
+
     void
     HumanTracker::prepareTrackedHumans(DateTime time)
     {

source/armarx/navigation/human/HumanTracker.h

@@ -200,6 +200,13 @@ namespace armarx::navigation::human
          * @return the size
          */
         float getClusterSize(Cluster cluster);
+        /**
+         * @brief HumanTracker::getClusterCenter Returns the center of the given cluster. That is
+         * calculated as the mean of all points.
+         * @param cluster The cluster whose center is calculated
+         * @return the center
+         */
+        Eigen::Vector2f HumanTracker::getClusterCenter(Cluster cluster);
         /**
          * @brief prepareTrackedHumans Deletes all tracked humans that received no update for too
          * long. All remaining tracked humans are prepared for association with the given point in