human tracker skeleton

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

@@ -11,18 +11,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
     {
         trackedHumans.clear();
 
+        auto positions = getMeasurementPositions(measurements);
+
         for (const armem::human::HumanPose& humanPose : measurements.humanPoses)
         {
-
-            const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
-            ARMARX_CHECK_NOT_EMPTY(keypoints);
-
-            Eigen::Vector3f centerPos = Eigen::Vector3f::Zero();
-            for (const auto& [_, v] : keypoints)
-            {
-                centerPos += v.positionGlobal;
-            }
-            centerPos /= static_cast<float>(keypoints.size());
+            auto centerPos = positions.find(humanPose);
 
             core::Pose2D pose = core::Pose2D::Identity();
             pose.translation() = Eigen::Vector2f{centerPos.x(), centerPos.y()};
@@ -44,13 +37,38 @@ namespace armarx::navigation::components::dynamic_scene_provider
     }
 
 
+    std::map<const armem::human::HumanPose&, armarx::navigation::core::Position>
+    HumanTracker::getMeasurementPositions(const Measurements& measurements) const
+    {
+        std::map<armem::human::HumanPose&, armarx::navigation::core::Position> map;
+
+        for (const armem::human::HumanPose& humanPose : measurements.humanPoses)
+        {
+
+            const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
+            ARMARX_CHECK_NOT_EMPTY(keypoints);
+
+            auto centerPos = armarx::navigation::core::Position::Zero();
+            for (const auto& [_, v] : keypoints)
+            {
+                centerPos += v.positionGlobal;
+            }
+            centerPos /= static_cast<float>(keypoints.size());
+
+            map.insert(
+                std::pair<const armem::human::HumanPose&, armarx::navigation::core::Position>(
+                    humanPose, centerPos));
+        }
+    }
+
+
     std::map<armem::human::HumanPose&, human::Human&>
     HumanTracker::getAssociatedHumans(const Measurements& measurements) const
     {
     }
 
     std::vector<human::Human>
-    HumanTracker::mostRecentTrackedHumans() const
+    HumanTracker::getMostRecentTrackedHumans() const
     {
         if (trackedHumans.empty())
         {