Implement basic version of getClusterSize

source/armarx/navigation/human/HumanTracker.cpp

@@ -220,4 +220,22 @@ namespace armarx::navigation::human
         trackedHuman->trackingId = detectedHuman->trackingId;
     }
 
+    float
+    HumanTracker::getClusterSize(armem::vision::LaserScannerFeature cluster)
+    {
+        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;
+                float xdiff = point2.x() - point1.x();
+                float ydiff = point2.y() - point1.y();
+                maxSize = std::max(maxSize, std::sqrt(xdiff * xdiff + ydiff * ydiff));
+            }
+        }
+        return maxSize;
+    }
+
 } // namespace armarx::navigation::human

source/armarx/navigation/human/HumanTracker.h

@@ -167,6 +167,13 @@ namespace armarx::navigation::human
          * @param detectedHuman the detected human
          */
         void associate(TrackedHuman* tracked, DetectedHuman* detected);
+        /**
+         * @brief getClusterSize Returns the size of the given cluster. That is for now the maximum
+         * distance between two points.
+         * @param cluster The cluster whose size is measured
+         * @return the size
+         */
+        float getClusterSize(armem::vision::LaserScannerFeature cluster);
 
 
     private: