diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp index 94ee8e376990f015d24e275ff136a59e46b0db25..41649ff44412dea5aa6773bdfe96af68abfbcfa5 100644 --- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp +++ b/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;