Use new HumanPose definition with TrackingID

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

@@ -11,23 +11,29 @@ namespace armarx::navigation::components::dynamic_scene_provider
 
 
     HumanTracker::DetectedHuman
-    convertHumanPoseToPosition(const armem::human::HumanPose& humanPose)
+    convertHumanPoseToPosition(const DateTime& time, const armem::human::HumanPose& humanPose)
     {
-        const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
+        const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
         ARMARX_CHECK_NOT_EMPTY(keypoints);
 
         auto centerPos = core::Position::Zero();
+        int size = 0;
         for (const auto& [_, v] : keypoints)
         {
-            centerPos += v.positionGlobal;
+            if (v.positionGlobal.has_value())
+            {
+                centerPos += v.positionGlobal.value().toEigen();
+                size++;
+            }
         }
-        centerPos /= static_cast<float>(keypoints.size());
+        centerPos /= size;
 
         core::Pose2D pose = core::Pose2D::Identity();
         pose.translation() = centerPos;
-        pose.linear() = Eigen::Rotation2Df(0 /*angle*/).toRotationMatrix();
+        //TODO: angle
+        pose.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
 
-        return pose;
+        return {pose, humanPose.humanTrackingId, time, false};
     }
 
     void
@@ -51,9 +57,12 @@ namespace armarx::navigation::components::dynamic_scene_provider
             }
         }
 
-        std::vector<DetectedHuman> newPoses = measurements.humanPoses |
-                                              ranges::views::transform(convertHumanPoseToPosition) |
-                                              ranges::to_vector;
+        std::vector<DetectedHuman> newPoses =
+            measurements.humanPoses |
+            ranges::views::transform(
+                [measurements](const armem::human::HumanPose& humanPose) -> DetectedHuman
+                { return convertHumanPoseToPosition(measurements.detectionTime, humanPose); }) |
+            ranges::to_vector;
 
         associateHumans(newPoses);
     }
@@ -110,11 +119,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
             }
             for (auto& newHuman : detectedHumans)
             {
-                if (newHuman.associated)
+                if (newHuman.associated || !newHuman.trackingId)
                 {
                     continue;
                 }
-                if (oldHuman.trackingId.value() == newHuman.trackingId)
+                if (oldHuman.trackingId.value() == newHuman.trackingId.value())
                 {
                     associate(oldHuman, newHuman);
                 }
@@ -125,9 +134,16 @@ namespace armarx::navigation::components::dynamic_scene_provider
         // associate leftover humans by their distances
         const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
 
+        //TODO max distance parameter
+        float maxDistance = 600;
+
         for (auto& posDistance : sortedDistances)
         {
-            if (!posDistance.oldHuman.associated || !posDistance.newHuman.associated)
+            if (posDistance.distance > maxDistance)
+            {
+                break;
+            }
+            if (posDistance.oldHuman.associated || posDistance.newHuman.associated)
             {
                 continue;
             }

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

@@ -24,9 +24,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
 
         struct DetectedHuman
         {
-            core::Pose2D pose;
-            std::string trackingId;
-            DateTime detectionTime;
+            const core::Pose2D pose;
+            const std::optional<std::string> trackingId;
+            const DateTime detectionTime;
             bool associated;
         };