Add laser scanner features to teb planning

This contains the move of the LaserScannerFeatureExtraction into navigation (!72 (merged)).

Implement merging of the features from individual sensors into one list while removing duplicates. Add the features to the dynamic scene and use them as teb obstacles.

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);
             }