Implement grouping algorithm

source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h

@@ -7,7 +7,7 @@
 
 namespace armarx::navigation::components::dynamic_scene_provider
 {
-    class CombinedDistance : DistanceFunction
+    class CombinedDistance : public DistanceFunction
     {
     public:
         CombinedDistance(double maxOrientationInfluence);

source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h

@@ -5,7 +5,7 @@
 
 namespace armarx::navigation::components::dynamic_scene_provider
 {
-    class EuclideanDistance : DistanceFunction
+    class EuclideanDistance : public DistanceFunction
     {
     public:
         EuclideanDistance(double min = 0);

source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp

+#include "CombinedDistance.h"
 #include "HumanGrouper.h"
 
 namespace armarx::navigation::components::dynamic_scene_provider
 {
-    HumanGrouper::HumanGrouper()
+    HumanGrouper::HumanGrouper(GroupingSettings settings) : settings(settings)
     {
+        this->distanceFunction = new CombinedDistance(settings.maxOrientationInfluence);
+    }
 
+    HumanGrouper::~HumanGrouper()
+    {
+        delete this->distanceFunction;
     }
 
     void HumanGrouper::updateHumans(std::vector<human::Human> &newHumans)
@@ -14,8 +20,40 @@ namespace armarx::navigation::components::dynamic_scene_provider
 
     std::vector<human::HumanGroup> HumanGrouper::getCurrentGroups()
     {
+        DateTime now = DateTime::Now();
 
-    }
+        std::vector<human::HumanGroup> groups = {};
+
+        // maps index of human in currentHumans to index of group in groups
+        std::unordered_map<int, int> humanGroupMap;
+
+        for (int parentIdx = 0; parentIdx < currentHumans.size(); parentIdx++)
+        {
+            Human parent = currentHumans.at(parentIdx);
 
+            // if no group exists yet for this human, create one just for them
+            if (humanGroupMap.find(parentIdx) == humanGroupMap.end())
+            {
+                human::HumanGroup group = {{{}}, {parent}, now};
+                groups.push_back(group);
+                humanGroupMap[parentIdx] = groups.size() - 1;
+            }
+
+            for (int childIdx = 0; childIdx < currentHumans.size(); childIdx++)
+            {
+                Human child = currentHumans.at(childIdx);
+
+                if (this->distanceFunction->computeDistance(parent, child) < settings.groupingThreshold)
+                {
+                    humanGroupMap[childIdx] = humanGroupMap[parentIdx];
+                    groups.at(humanGroupMap[childIdx]).humans.push_back(currentHumans.at(childIdx));
+                }
+            }
+        }
+
+        // TODO: shape generation
+
+        return groups;
+    }
 
 }

source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h

@@ -21,6 +21,7 @@
 
 #pragma once
 
+#include "DistanceFunction.h"
 #include "armarx/navigation/core/basic_types.h"
 #include "armarx/navigation/human/types.h"
 
@@ -34,11 +35,28 @@ namespace armarx::navigation::components::dynamic_scene_provider
      */
     class HumanGrouper
     {
+
+    using Human = armarx::navigation::human::Human;
+
     public:
+        /**
+         * @brief The GroupingSettings struct contains settings for group generation.
+         * This includes the influence of aligned orientation and how aggressively groups
+         * are generated.
+         */
+        struct GroupingSettings
+        {
+            double groupingThreshold;
+            double maxOrientationInfluence;
+        };
+
         /**
          * @brief Creates a new HumanGrouper
+         * @param settings different parameters that will affect group generation
          */
-        HumanGrouper();
+        HumanGrouper(GroupingSettings settings);
+
+        ~HumanGrouper();
 
         /**
          * @brief Sets the currently detected humans.
@@ -47,7 +65,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
          * when getCurrentGroups() is called the next time.
          * @param newHumans a vector containing the most recently detected humans
          */
-        void updateHumans(std::vector<human::Human>& newHumans);
+        void updateHumans(std::vector<Human> &newHumans);
 
         /**
          * @brief Recognizes groups in the current humans.
@@ -58,12 +76,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
          */
         std::vector<human::HumanGroup> getCurrentGroups();
 
-        struct DistanceFunction
-        {
-
-        };
 
     private:
-        std::vector<human::Human> currentHumans;
+        void generateShapes(std::vector<human::HumanGroup> &groups);
+        std::vector<Human> currentHumans;
+        DistanceFunction *distanceFunction;
+        GroupingSettings settings;
     };
-} // namespace armarx::navigation::components::dynamic_scene_provider
+}

source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h

@@ -5,13 +5,14 @@
 
 namespace armarx::navigation::components::dynamic_scene_provider
 {
-    class OrientationDistance : DistanceFunction
+    class OrientationDistance : public DistanceFunction
     {
     public:
         OrientationDistance(double max = 1, double min = 0);
         virtual double computeDistance(Human &h1, Human &h2);
     private:
         double getOrientationFactor(Human &h1, Human &h2);
+        double getLineOrientation(Human &h2, Human &h1);
         double normOrientation(double orientation);
         double max;
         double min;