From fc47ac5cfba574a8662823bcd7fe1d296396ff92 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 15 Aug 2022 17:22:33 +0200
Subject: [PATCH] Implement basic version of human tracker.
Uses average position of KeypointMap.
Missing angle (currently set to 0).
Missing velocity (currently set to 0).
---
.../dynamic_scene_provider/HumanTracker.cpp | 77 +++++++++----------
.../dynamic_scene_provider/HumanTracker.h | 3 +-
2 files changed, 37 insertions(+), 43 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 41649ff4..1cd4c42f 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -1,61 +1,54 @@
+
#include "HumanTracker.h"
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
namespace armarx::navigation::components::dynamic_scene_provider
{
-HumanTracker::HumanTracker()
-{
+ void
+ HumanTracker::update(const Measurements& measurements)
+ {
+ trackedHumans.clear();
-}
+ for (const armem::human::HumanPose& humanPose : measurements.humanPoses)
+ {
+ const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
+ ARMARX_CHECK_NOT_EMPTY(keypoints);
-void HumanTracker::update(const Measurements &measurements)
-{
- trackedHumans.clear();
+ Eigen::Vector3f centerPos = Eigen::Vector3f::Zero();
+ for (const auto& [_, v] : keypoints)
+ {
+ centerPos += v.positionGlobal;
+ }
+ centerPos /= static_cast<float>(keypoints.size());
- foreach (humanPose, measurements.humanPoses) {
+ core::Pose2D pose = core::Pose2D::Identity();
+ pose.translation() = Eigen::Vector2f{centerPos.x(), centerPos.y()};
+ pose.linear() = Eigen::Rotation2Df(0 /*angle*/).toRotationMatrix();
- const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
- ARMARX_CHECK_NOT_EMPTY(keypoints);
+ const HumanTracker::TrackedHuman newHuman = {
+ .global_T_human = pose,
+ .linearVelocity = Eigen::Vector2f::Zero() //TODO more sophisticated guess
+ };
- 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);
}
- trackedHumans.push_back(newHuman);
}
-
- this->trackedHumans = HumanTracker::TrackedHuman{
- .global_T_human = core::Pose2D{
- .x = 0;
- .y = 0;
- };
- .linearVelocity = 0;
+ const std::vector<HumanTracker::TrackedHuman>&
+ HumanTracker::getTrackedHumans() const
+ {
+ return trackedHumans;
}
-}
-
-
-std::vector<HumanTracker::TrackedHuman> HumanTracker::getTrackedHumans() const
-{
-
-}
-void HumanTracker::reset()
-{
+ void
+ HumanTracker::reset()
+ {
+ trackedHumans.clear();
+ }
-}
-
-}
+} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index 770901f1..556d516b 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -2,6 +2,7 @@
#include "VisionX/libraries/armem_human/types.h"
+
#include "armarx/navigation/core/basic_types.h"
namespace armarx::navigation::components::dynamic_scene_provider
@@ -27,7 +28,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
Eigen::Vector2f linearVelocity;
};
- std::vector<TrackedHuman> getTrackedHumans() const;
+ const std::vector<TrackedHuman>& getTrackedHumans() const;
void reset();
--
GitLab