From f73113a36c1d33eb98f8b4a31bc1f2dca29f2c62 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Tue, 16 Aug 2022 23:43:35 +0000
Subject: [PATCH] Create parameter struct in header file for human tracking
parameters
Not quite sure if it builds though as I edited this in the online editor, should be checked at some time
---
.../dynamic_scene_provider/HumanTracker.cpp | 15 +++------------
.../dynamic_scene_provider/HumanTracker.h | 11 +++++++++++
2 files changed, 14 insertions(+), 12 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 4adc1106..a86c684c 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -40,13 +40,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
void
HumanTracker::update(const Measurements& measurements)
{
- //TODO: proper time to live
- Duration maxAge = Duration::MilliSeconds(500);
-
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);
}
@@ -135,12 +132,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;
}
@@ -161,16 +155,13 @@ 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..b29011b4 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;
+ Parameter parameters;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
--
GitLab