human tracker skeleton

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;