From 16ccec9a3bb7ecd47294ac7b77c88c0c7bd594b8 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 17 Aug 2022 09:32:42 +0200
Subject: [PATCH] merge with origin/feature/human-tracker
---
.../dynamic_scene_provider/HumanTracker.cpp | 15 ++++++---------
.../dynamic_scene_provider/HumanTracker.h | 11 +++++++++++
2 files changed, 17 insertions(+), 9 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 7dd61dc6..e7771cb3 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -64,7 +64,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
{
auto& human = *it;
- if ((measurements.detectionTime - human.human.detectionTime) >= maxAge)
+ if ((measurements.detectionTime - human.human.detectionTime) >=
+ parameters.maxTrackingAge)
{
it = trackedHumans.erase(it);
}
@@ -153,12 +154,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
// associate leftover humans by their distances
const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
- //TODO max distance parameter
- float maxDistance = 600;
-
for (auto& posDistance : sortedDistances)
{
- if (posDistance.distance > maxDistance)
+ if (posDistance.distance > parameters.maxAssociationDistance)
{
break;
}
@@ -179,16 +177,15 @@ namespace armarx::navigation::components::dynamic_scene_provider
trackedHuman->associated = true;
detectedHuman->associated = true;
- // TODO alpha parameter
- float a = 0.7;
-
float dt =
(detectedHuman->detectionTime - trackedHuman->human.detectionTime).toSecondsDouble();
Eigen::Vector2f ds =
(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 =
+ parameters.velocityAlpha * linVelocity +
+ (1 - parameters.velocityAlpha) * trackedHuman->human.linearVelocity;
trackedHuman->human = {detectedHuman->pose, velocity, detectedHuman->detectionTime};
trackedHuman->trackingId = detectedHuman->trackingId;
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index e5197d82..9146a798 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -37,6 +37,16 @@ namespace armarx::navigation::components::dynamic_scene_provider
bool associated;
};
+ struct Parameters
+ {
+ // the duration after which tracked humans will be erased if no new measurement for this human is found
+ Duration maxTrackingAge = Duration::MilliSeconds(500);
+ // the maximum distance in millimeters of two human measurements to be associated with each other
+ float maxAssociationDistance = 600;
+ // alpha value from interval [0,1] to determine how much the new (and respectively the old) velocity should be weighted
+ float velocityAlpha = 0.7;
+ };
+
void update(const Measurements& measurements);
std::vector<human::Human> getTrackedHumans() const;
@@ -49,5 +59,6 @@ namespace armarx::navigation::components::dynamic_scene_provider
private:
std::vector<TrackedHuman> trackedHumans;
+ Parameters parameters;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
--
GitLab