From b7dcb8b4c0ea349a5aca664152535ff5bbe9e0ae Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Tue, 16 Aug 2022 01:24:25 +0200
Subject: [PATCH] Fix eigen errors
---
.../dynamic_scene_provider/Component.cpp | 8 +--
.../dynamic_scene_provider/HumanTracker.cpp | 50 +++++++++----------
.../dynamic_scene_provider/HumanTracker.h | 8 +--
3 files changed, 31 insertions(+), 35 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index a4d10991..e8abd88f 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -318,13 +318,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
// here ends: data fetching
-
- humanTracker.update(HumanTracker::Measurements{
- .humanPoses = humanPoseResult.humanPoses
- });
-
-
+ humanTracker.update(HumanTracker::Measurements{.detectionTime = timestamp,
+ .humanPoses = humanPoseResult.humanPoses});
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 61eaaa69..a63f8a30 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -16,7 +16,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
ARMARX_CHECK_NOT_EMPTY(keypoints);
- auto centerPos = core::Position::Zero();
+ Eigen::Vector3f centerPos;
int size = 0;
for (const auto& [_, v] : keypoints)
{
@@ -29,7 +29,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
centerPos /= size;
core::Pose2D pose = core::Pose2D::Identity();
- pose.translation() = centerPos;
+ pose.translation() = centerPos.head(2);
//TODO: angle
pose.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
@@ -69,33 +69,33 @@ namespace armarx::navigation::components::dynamic_scene_provider
struct PosDistance
{
- HumanTracker::TrackedHuman& oldHuman;
- HumanTracker::DetectedHuman& newHuman;
- const float distance;
+ HumanTracker::TrackedHuman* oldHuman;
+ HumanTracker::DetectedHuman* newHuman;
+ float distance;
};
std::vector<PosDistance>
- getSortedDistances(const std::vector<HumanTracker::TrackedHuman>& oldHumans,
- const std::vector<HumanTracker::DetectedHuman>& newHumans)
+ getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+ std::vector<HumanTracker::DetectedHuman>& newHumans)
{
std::vector<PosDistance> posDistances;
- for (const auto& oldHuman : oldHumans)
+ for (auto& oldHuman : oldHumans)
{
if (oldHuman.associated)
{
continue;
}
- for (const auto& newHuman : newHumans)
+ for (auto& newHuman : newHumans)
{
if (newHuman.associated)
{
continue;
}
- posDistances.emplace_back(
- oldHuman,
- newHuman,
- (newHuman.pose.translation() - oldHuman.human.pose.translation()).norm());
+ posDistances.push_back(
+ {&oldHuman,
+ &newHuman,
+ (newHuman.pose.translation() - oldHuman.human.pose.translation()).norm()});
}
}
@@ -125,7 +125,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
if (oldHuman.trackingId.value() == newHuman.trackingId.value())
{
- associate(oldHuman, newHuman);
+ associate(&oldHuman, &newHuman);
}
}
}
@@ -143,7 +143,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
{
break;
}
- if (posDistance.oldHuman.associated || posDistance.newHuman.associated)
+ if (posDistance.oldHuman->associated || posDistance.newHuman->associated)
{
continue;
}
@@ -152,27 +152,27 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
void
- HumanTracker::associate(TrackedHuman& trackedHuman, DetectedHuman& detectedHuman)
+ HumanTracker::associate(TrackedHuman* trackedHuman, DetectedHuman* detectedHuman)
{
- ARMARX_CHECK(!trackedHuman.associated);
- ARMARX_CHECK(!detectedHuman.associated);
+ ARMARX_CHECK(!trackedHuman->associated);
+ ARMARX_CHECK(!detectedHuman->associated);
- trackedHuman.associated = true;
- detectedHuman.associated = true;
+ trackedHuman->associated = true;
+ detectedHuman->associated = true;
// TODO alpha parameter
float a = 0.7;
float dt =
- (detectedHuman.detectionTime - trackedHuman.human.detectionTime).toSecondsDouble();
+ (detectedHuman->detectionTime - trackedHuman->human.detectionTime).toSecondsDouble();
Eigen::Vector2f ds =
- (detectedHuman.pose.translation() - trackedHuman.human.pose.translation());
+ (detectedHuman->pose.translation() - trackedHuman->human.pose.translation());
Eigen::Vector2f linVelocity = ds / dt;
- Eigen::Vector2f velocity = a * linVelocity + (1 - a) * trackedHuman.human.linearVelocity;
+ Eigen::Vector2f velocity = a * linVelocity + (1 - a) * trackedHuman->human.linearVelocity;
- trackedHuman.human = {detectedHuman.pose, velocity, detectedHuman.detectionTime};
- trackedHuman.trackingId = detectedHuman.trackingId;
+ trackedHuman->human = {detectedHuman->pose, velocity, detectedHuman->detectionTime};
+ trackedHuman->trackingId = detectedHuman->trackingId;
}
std::vector<human::Human>
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index f13a6653..e5197d82 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -24,9 +24,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
struct DetectedHuman
{
- const core::Pose2D pose;
- const std::optional<std::string> trackingId;
- const DateTime detectionTime;
+ core::Pose2D pose;
+ std::optional<std::string> trackingId;
+ DateTime detectionTime;
bool associated;
};
@@ -45,7 +45,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
private:
void associateHumans(std::vector<DetectedHuman>& detectedHumans);
- void associate(TrackedHuman& tracked, DetectedHuman& detected);
+ void associate(TrackedHuman* tracked, DetectedHuman* detected);
private:
std::vector<TrackedHuman> trackedHumans;
--
GitLab