diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp index 4adc1106292ecb0d7c94f39579caf15abec8e132..a86c684cc194acf6ce1f6e223ed630989d593f8d 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 e5197d8222457b91992bc2c74b89a65c4a704fb0..b29011b47dd63fc2d4364833114191284e669654 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