merge with origin/feature/human-tracker

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;

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