Implement getSortedDistances for distance between humans and clusters

source/armarx/navigation/human/HumanTracker.cpp

@@ -127,11 +127,11 @@ namespace armarx::navigation::human
         return {pose, humanPose.humanTrackingId, time, false};
     }
 
-    std::vector<HumanTracker::PosDistance>
+    std::vector<HumanTracker::PosHumanDistance>
     HumanTracker::getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
                                      std::vector<HumanTracker::DetectedHuman>& newHumans)
     {
-        std::vector<PosDistance> posDistances;
+        std::vector<PosHumanDistance> posDistances;
 
         for (auto& oldHuman : oldHumans)
         {
@@ -158,7 +158,42 @@ namespace armarx::navigation::human
         // sort the distances ascending by their numeric value
         std::sort(posDistances.begin(),
                   posDistances.end(),
-                  [](const PosDistance& a, const PosDistance& b) -> bool
+                  [](const PosHumanDistance& a, const PosHumanDistance& b) -> bool
+                  { return a.distance < b.distance; });
+
+        return posDistances;
+    }
+
+    std::vector<HumanTracker::PosLaserDistance>
+    HumanTracker::getSortedDistances(std::vector<TrackedHuman>& oldHumans,
+                                     std::vector<Cluster>& newClusters)
+    {
+        std::vector<PosLaserDistance> posDistances;
+
+        for (auto& newCluster : newClusters)
+        {
+            //calculate center of cluster
+            Eigen::Vector2f clusterCenter;
+            for (auto& point : newCluster.points)
+            {
+                clusterCenter += point;
+            }
+            clusterCenter /= newCluster.points.size();
+
+            for (auto& oldHuman : oldHumans)
+            {
+                // calculate distance between every possible combination of tracked human and cluster
+                posDistances.push_back(
+                    {&oldHuman,
+                     &newCluster,
+                     (clusterCenter - oldHuman.humanFilter.get().pose.translation()).norm()});
+            }
+        }
+
+        // sort the distances ascending by their numeric value
+        std::sort(posDistances.begin(),
+                  posDistances.end(),
+                  [](const PosHumanDistance& a, const PosHumanDistance& b) -> bool
                   { return a.distance < b.distance; });
 
         return posDistances;
@@ -221,7 +256,7 @@ namespace armarx::navigation::human
     }
 
     float
-    HumanTracker::getClusterSize(armem::vision::LaserScannerFeature cluster)
+    HumanTracker::getClusterSize(Cluster cluster)
     {
         float maxSize = 0;
         for (auto it1 = cluster.points.begin(); it1 != cluster.points.end();)
@@ -230,9 +265,8 @@ namespace armarx::navigation::human
             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));
+                //TODO does subtraction work for vectors like this?
+                maxSize = std::max(maxSize, (point2 - point1).norm());
             }
         }
         return maxSize;

source/armarx/navigation/human/HumanTracker.h

@@ -39,6 +39,7 @@ namespace armarx::navigation::human
     using T = double;
     using Vector = Eigen::Matrix<T, 2, 1>;
     using SystemModelT = kalman_filter::SystemModelSO2xR2<T>;
+    using Cluster = armem::vision::LaserScannerFeature;
 
     /**
      * @brief The HumanTracker class can be used to track and filter multiple humans. It hides
@@ -60,7 +61,7 @@ namespace armarx::navigation::human
         struct LaserMeasurement
         {
             DateTime detectionTime;
-            std::vector<armem::vision::LaserScannerFeature> clusters;
+            std::vector<Cluster> clusters;
         };
 
         struct DetectedHuman
@@ -78,13 +79,20 @@ namespace armarx::navigation::human
             bool associated;
         };
 
-        struct PosDistance
+        struct PosHumanDistance
         {
             HumanTracker::TrackedHuman* oldHuman;
             HumanTracker::DetectedHuman* newHuman;
             float distance;
         };
 
+        struct PosLaserDistance
+        {
+            HumanTracker::TrackedHuman* oldHuman;
+            Cluster* newCluster;
+            float distance;
+        };
+
         struct Parameters
         {
             // the keypoint which should be used for calculating the rotation of the humans
@@ -95,6 +103,8 @@ namespace armarx::navigation::human
             // the maximum distance in millimeters of two human measurements where they are still
             // associated with each other
             float maxAssociationDistance = 600;
+            // the maximum size (aka length) in millimeters a footprint can have
+            float maxFootprintSize = 350;
             // alpha value from interval [0,1] to determine how much the current (and respectively
             // the old) velocity should be weighted when calculating the new velocity
             float velocityAlpha = 0.7;
@@ -118,8 +128,7 @@ namespace armarx::navigation::human
          * @param measurements
          * @return
          */
-        std::vector<armem::vision::LaserScannerFeature>
-        update(const LaserMeasurement& measurements);
+        std::vector<Cluster> update(const LaserMeasurement& measurements);
         /**
          * @brief HumanTracker::getTrackedHumans Returns all humans that are currently tracked.
          * @return  the tracked humans
@@ -145,14 +154,25 @@ namespace armarx::navigation::human
          * @brief getSortedDistances Returns a sorted vector of the distances between every possible
          * combination (T, D) where T is an old, tracked human and D is a new, detected human and
          * both of them are not already associated. The smallest distance will be the first entry in
-         * the vector
+         * the vector.
          * @param oldHumans the old, tracked humans
          * @param newHumans the new, detected humans
          * @return the sorted vector of distances with references to the according humans
          */
-        std::vector<PosDistance>
+        std::vector<PosHumanDistance>
         getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
                            std::vector<HumanTracker::DetectedHuman>& newHumans);
+        /**
+         * @brief getSortedDistances Returns a sorted vector of the distances between every possible
+         * combination (T, C) where T is an old, tracked human and C is a new, detected cluster.
+         * The smallest distance will be the first entry in the vector.
+         * @param oldHumans
+         * @param newClusters
+         * @return
+         */
+        std::vector<PosLaserDistance>
+        getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+                           std::vector<Cluster>& newClusters);
         /**
          * @brief HumanTracker::associateHumans Associates those tracked and detected humans that
          * belong together.
@@ -173,7 +193,7 @@ namespace armarx::navigation::human
          * @param cluster The cluster whose size is measured
          * @return the size
          */
-        float getClusterSize(armem::vision::LaserScannerFeature cluster);
+        float getClusterSize(Cluster cluster);
 
 
     private: