From da681818071ad9467bb3055c44f059e6e7e92806 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 15 Aug 2022 20:13:35 +0200
Subject: [PATCH] Implement HumanTracker::getMeasurementPositions()
---
.../dynamic_scene_provider/HumanTracker.cpp | 40 ++++++++++++++-----
.../dynamic_scene_provider/HumanTracker.h | 5 +++
2 files changed, 34 insertions(+), 11 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index dcd76e7d..6bae4e5d 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/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())
{
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index 209fd345..76c6ce50 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/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
--
GitLab