Make getSortedDistances a class method

source/armarx/navigation/human/HumanTracker.cpp

@@ -73,51 +73,6 @@ namespace armarx::navigation::human
     }
 
 
-    struct PosDistance
-    {
-        HumanTracker::TrackedHuman* oldHuman;
-        HumanTracker::DetectedHuman* newHuman;
-        float distance;
-    };
-
-    std::vector<PosDistance>
-    getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
-                       std::vector<HumanTracker::DetectedHuman>& newHumans)
-    {
-        std::vector<PosDistance> posDistances;
-
-        for (auto& oldHuman : oldHumans)
-        {
-            if (oldHuman.associated)
-            {
-                continue;
-            }
-            for (auto& newHuman : newHumans)
-            {
-                if (newHuman.associated)
-                {
-                    continue;
-                }
-                // calculate distance between every possible combination of tracked and detected
-                // humans where none of them was associated
-                posDistances.push_back(
-                    {&oldHuman,
-                     &newHuman,
-                     (newHuman.pose.translation() - oldHuman.humanFilter.get().pose.translation())
-                         .norm()});
-            }
-        }
-
-        // sort the distances ascending by their numeric value
-        std::sort(posDistances.begin(),
-                  posDistances.end(),
-                  [](const PosDistance& a, const PosDistance& b) -> bool
-                  { return a.distance < b.distance; });
-
-        return posDistances;
-    }
-
-
     HumanTracker::DetectedHuman
     HumanTracker::convertHumanPoseToDetectedHuman(const DateTime& time,
                                                   const armem::human::HumanPose& humanPose)
@@ -157,7 +112,7 @@ namespace armarx::navigation::human
             //old version using euler angles:
             //yaw = humanPose //from all human pose keypoints
             //          .keypoints
-            //          .at("HEAD") //find the keypoint representing the head
+            //          .at(parameters.rotationKeypoint) //find the keypoint representing the head
             //          .orientationGlobal //get its global orientation
             //          ->toEigen()
             //          .eulerAngles(2, 1, 0)[0]; //and extract the yaw (rotation around z axis in
@@ -172,6 +127,43 @@ namespace armarx::navigation::human
         return {pose, humanPose.humanTrackingId, time, false};
     }
 
+    std::vector<HumanTracker::PosDistance>
+    HumanTracker::getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+                                     std::vector<HumanTracker::DetectedHuman>& newHumans)
+    {
+        std::vector<PosDistance> posDistances;
+
+        for (auto& oldHuman : oldHumans)
+        {
+            if (oldHuman.associated)
+            {
+                continue;
+            }
+            for (auto& newHuman : newHumans)
+            {
+                if (newHuman.associated)
+                {
+                    continue;
+                }
+                // calculate distance between every possible combination of tracked and detected
+                // humans where none of them was associated
+                posDistances.push_back(
+                    {&oldHuman,
+                     &newHuman,
+                     (newHuman.pose.translation() - oldHuman.humanFilter.get().pose.translation())
+                         .norm()});
+            }
+        }
+
+        // sort the distances ascending by their numeric value
+        std::sort(posDistances.begin(),
+                  posDistances.end(),
+                  [](const PosDistance& a, const PosDistance& b) -> bool
+                  { return a.distance < b.distance; });
+
+        return posDistances;
+    }
+
     void
     HumanTracker::associateHumans(std::vector<DetectedHuman>& detectedHumans)
     {

source/armarx/navigation/human/HumanTracker.h

@@ -71,6 +71,13 @@ namespace armarx::navigation::human
             bool associated;
         };
 
+        struct PosDistance
+        {
+            HumanTracker::TrackedHuman* oldHuman;
+            HumanTracker::DetectedHuman* newHuman;
+            float distance;
+        };
+
         struct Parameters
         {
             // the keypoint which should be used for calculating the rotation of the humans
@@ -115,6 +122,18 @@ namespace armarx::navigation::human
          */
         DetectedHuman convertHumanPoseToDetectedHuman(const DateTime& time,
                                                       const armem::human::HumanPose& humanPose);
+        /**
+         * @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
+         * @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>
+        getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+                           std::vector<HumanTracker::DetectedHuman>& newHumans);
         /**
          * @brief HumanTracker::associateHumans Associates those tracked and detected humans that
          * belong together.