human tracker skeleton

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;