Add confidence to tracker

source/armarx/navigation/human/HumanTracker.cpp

@@ -6,12 +6,12 @@
 
 #include "ArmarXCore/core/exceptions/local/ExpressionException.h"
 
-#include <armarx/navigation/human/types.h>
 #include <armarx/navigation/conversions/eigen.h>
+#include <armarx/navigation/human/types.h>
+
 #include <range/v3/range/conversion.hpp>
 #include <range/v3/view/transform.hpp>
 
-
 namespace armarx::navigation::human
 {
 
@@ -42,7 +42,8 @@ namespace armarx::navigation::human
                     .humanFilter = HumanFilter{detectedHuman.pose, detectedHuman.detectionTime},
                     .trackingId = detectedHuman.trackingId,
                     .associatedCluster = nullptr,
-                    .associated = true});
+                    .associated = true,
+                    .confidence = parameters.confidenceCamera});
             }
         }
     }
@@ -108,6 +109,7 @@ namespace armarx::navigation::human
                 posDistance.oldHuman->associatedCluster = nullptr;
                 posDistance.oldHuman->associated = true;
                 posDistance.newCluster->associated = true;
+                posDistance.oldHuman->confidence += parameters.confidenceLaser;
             }
         }
 
@@ -131,6 +133,7 @@ namespace armarx::navigation::human
 
                 human.associatedCluster = nullptr;
                 human.associated = true;
+                human.confidence += parameters.confidenceLaser;
             }
         }
 
@@ -160,7 +163,6 @@ namespace armarx::navigation::human
         trackedHumans.clear();
     }
 
-
     HumanTracker::DetectedHuman
     HumanTracker::convertHumanPoseToDetectedHuman(const DateTime& time,
                                                   const armem::human::HumanPose& humanPose)
@@ -326,6 +328,8 @@ namespace armarx::navigation::human
 
         trackedHuman->humanFilter.update(detectedHuman->pose, detectedHuman->detectionTime);
         trackedHuman->trackingId = detectedHuman->trackingId;
+
+        trackedHuman->confidence = parameters.confidenceCamera;
     }
 
     float
@@ -365,8 +369,10 @@ namespace armarx::navigation::human
         for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
         {
             auto& human = *it;
-            // when the tracked human recieved no new measurement for too long, remove it
-            if (human.humanFilter.detectionAge(time) >= parameters.maxTrackingAge)
+            // when the tracked human recieved no new measurement for too long or
+            // wasn't detected by the camera for too long, remove it
+            if (human.humanFilter.detectionAge(time) >= parameters.maxTrackingAge ||
+                human.confidence <= 0)
             {
                 it = trackedHumans.erase(it);
             }

source/armarx/navigation/human/HumanTracker.h

@@ -26,14 +26,14 @@
 
 #include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
 
-#include <VisionX/libraries/armem_human/types.h>
-
 #include <armarx/navigation/core/basic_types.h>
 #include <armarx/navigation/human/HumanFilter.h>
 #include <armarx/navigation/human/HumanSystemModel.h>
 #include <armarx/navigation/human/types.h>
 #include <armarx/navigation/memory/types.h>
 
+#include <VisionX/libraries/armem_human/types.h>
+
 namespace armarx::navigation::human
 {
     using Cluster = memory::LaserScannerFeature;
@@ -82,6 +82,7 @@ namespace armarx::navigation::human
             std::optional<std::string> trackingId = std::nullopt;
             AdvancedCluster* associatedCluster;
             bool associated;
+            int confidence;
         };
 
         struct PosHumanDistance
@@ -113,6 +114,9 @@ namespace armarx::navigation::human
             // alpha value from interval [0,1] to determine how much the current (and respectively
             // the old) velocity should be weighted when calculating the new velocity
             float velocityAlpha = 0.7;
+
+            int confidenceCamera = 100;
+            int confidenceLaser = -2;
         };
 
         /**