Implement HumanTracker::getMeasurementPositions()

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())
         {

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

@@ -31,7 +31,12 @@ namespace armarx::navigation::components::dynamic_scene_provider
     private:
         std::vector<human::Human> trackedHumans;
 
+        std::map<const armem::human::HumanPose&, armarx::navigation::core::Position>
+        getMeasurementPositions(const Measurements& measurements) const;
+
         std::map<armem::human::HumanPose&, human::Human&>
         getAssociatedHumans(const Measurements& measurements) const;
+
+        std::vector<human::Human> getMostRecentTrackedHumans() const;
     };
 } // namespace armarx::navigation::components::dynamic_scene_provider