From c9bb2065192b0609ae2a28ca8df5d772443fafac Mon Sep 17 00:00:00 2001
From: Marius Baden <marius.baden@student.kit.edu>
Date: Wed, 3 Aug 2022 18:32:26 +0200
Subject: [PATCH] Create basic tracked humans from OpenPose
---
.../dynamic_scene_provider/HumanTracker.cpp | 25 +++++++++++++++++++
1 file changed, 25 insertions(+)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 94ee8e37..41649ff4 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;
--
GitLab