human tracker skeleton

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

@@ -11,6 +11,31 @@ HumanTracker::HumanTracker()
 
 void HumanTracker::update(const Measurements &measurements)
 {
+    trackedHumans.clear();
+
+    foreach (humanPose, measurements.humanPoses) {
+
+        const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
+        ARMARX_CHECK_NOT_EMPTY(keypoints);
+
+        const Eigen::Vector3f centerPos = Eigen::Vector3f::Zero();
+        for (const auto& [_, v] : keypoints)
+        {
+            centerPos += v.positionGlobal;
+        }
+        centerPos /= static_cast<float>(keypoints.size());
+
+        const HumanTracker::TrackedHuman newHuman = HumanTracker::TrackedHuman{
+            .global_T_human = core::Pose2D{
+                .x = centerPos.x;
+                .y = centerPos.y;
+            };;
+            .linearVelocity = 0; //TODO more sophisticated guess
+        }
+        trackedHumans.push_back(newHuman);
+    }
+
+
     this->trackedHumans = HumanTracker::TrackedHuman{
             .global_T_human = core::Pose2D{
                 .x = 0;