Extract preparation of trackedHumans for association in extra method

source/armarx/navigation/human/HumanTracker.cpp

@@ -12,24 +12,7 @@ namespace armarx::navigation::human
     void
     HumanTracker::update(const CameraMeasurement& measurements)
     {
-        // iterate over all existing tracked humans
-        for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
-        {
-            auto& human = *it;
-            // when the tracked human recieved no new measurement for too long, remove it
-            if ((measurements.detectionTime - human.humanFilter.get().detectionTime) >=
-                parameters.maxTrackingAge)
-            {
-                it = trackedHumans.erase(it);
-            }
-            // otherwise the tracked human is prepared for association at the current point in time
-            else
-            {
-                human.associated = false;
-                human.humanFilter.propagation(measurements.detectionTime);
-                it++;
-            }
-        }
+        prepareTrackedHumans(measurements.detectionTime);
 
         // calculate the poses according to the new received measurements
         std::vector<DetectedHuman> newPoses =
@@ -166,27 +149,19 @@ namespace armarx::navigation::human
 
     std::vector<HumanTracker::PosLaserDistance>
     HumanTracker::getSortedDistances(std::vector<TrackedHuman>& oldHumans,
-                                     std::vector<Cluster>& newClusters)
+                                     std::vector<AdvancedCluster>& newClusters)
     {
         std::vector<PosLaserDistance> posDistances;
 
-        for (auto& newCluster : newClusters)
+        for (auto& oldHuman : oldHumans)
         {
-            //calculate center of cluster
-            Eigen::Vector2f clusterCenter;
-            for (auto& point : newCluster.points)
-            {
-                clusterCenter += point;
-            }
-            clusterCenter /= newCluster.points.size();
-
-            for (auto& oldHuman : oldHumans)
+            for (auto& newCluster : newClusters)
             {
                 // calculate distance between every possible combination of tracked human and cluster
                 posDistances.push_back(
                     {&oldHuman,
                      &newCluster,
-                     (clusterCenter - oldHuman.humanFilter.get().pose.translation()).norm()});
+                     (newCluster.center - oldHuman.humanFilter.get().pose.translation()).norm()});
             }
         }
 
@@ -272,4 +247,27 @@ namespace armarx::navigation::human
         return maxSize;
     }
 
+    void
+    HumanTracker::prepareTrackedHumans(DateTime time)
+    {
+        // iterate over all existing tracked humans
+        for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
+        {
+            auto& human = *it;
+            // when the tracked human recieved no new measurement for too long, remove it
+            if ((time - human.humanFilter.get().detectionTime) >= parameters.maxTrackingAge)
+            {
+                it = trackedHumans.erase(it);
+            }
+            // otherwise the tracked human is prepared for association at the current point in time
+            else
+            {
+                human.associatedClusters = 0;
+                human.associated = false;
+                human.humanFilter.propagation(time);
+                it++;
+            }
+        }
+    }
+
 } // namespace armarx::navigation::human

source/armarx/navigation/human/HumanTracker.h

@@ -76,6 +76,14 @@ namespace armarx::navigation::human
         {
             HumanFilter humanFilter;
             std::optional<std::string> trackingId = std::nullopt;
+            int associatedClusters;
+            bool associated;
+        };
+
+        struct AdvancedCluster
+        {
+            Eigen::Vector2f center;
+            Cluster cluster;
             bool associated;
         };
 
@@ -89,7 +97,7 @@ namespace armarx::navigation::human
         struct PosLaserDistance
         {
             HumanTracker::TrackedHuman* oldHuman;
-            Cluster* newCluster;
+            AdvancedCluster* newCluster;
             float distance;
         };
 
@@ -172,7 +180,7 @@ namespace armarx::navigation::human
          */
         std::vector<PosLaserDistance>
         getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
-                           std::vector<Cluster>& newClusters);
+                           std::vector<AdvancedCluster>& newClusters);
         /**
          * @brief HumanTracker::associateHumans Associates those tracked and detected humans that
          * belong together.
@@ -194,6 +202,10 @@ namespace armarx::navigation::human
          * @return the size
          */
         float getClusterSize(Cluster cluster);
+        /**
+         * @brief prepareTrackedHumans
+         */
+        void prepareTrackedHumans(DateTime time);
 
 
     private: