Implement update method for lasersensor measurement

source/armarx/navigation/human/HumanTracker.cpp

@@ -35,11 +35,134 @@ namespace armarx::navigation::human
                 trackedHumans.push_back(TrackedHuman{
                     .humanFilter = HumanFilter{detectedHuman.pose, detectedHuman.detectionTime},
                     .trackingId = detectedHuman.trackingId,
+                    .associatedCluster = nullptr,
                     .associated = true});
             }
         }
     }
 
+    std::vector<Cluster>
+    HumanTracker::update(const LaserMeasurement& measurements)
+    {
+        // TODO do we receive cluster with correct threshold / expected size / as accurate as that
+        // they distinguish multiple footprints?
+
+        prepareTrackedHumans(measurements.detectionTime);
+
+        // only look at the clusters that have a size reasonable enough to contain at most two
+        // footprints
+        std::vector<AdvancedCluster> potentialFootprints;
+        std::vector<Cluster> unusedClusters;
+        for (auto it = measurements.clusters.begin(); it != measurements.clusters.end();)
+        {
+            auto& cluster = *it;
+
+            if (getClusterSize(cluster) <= 2 * parameters.maxFootprintSize)
+            {
+                //calculate center of cluster
+                Eigen::Vector2f clusterCenter;
+                for (auto& point : cluster.points)
+                {
+                    clusterCenter += point;
+                }
+                clusterCenter /= cluster.points.size();
+
+                potentialFootprints.push_back(AdvancedCluster{
+                    .center = clusterCenter, .cluster = cluster, .associated = false});
+            }
+            else
+            {
+                unusedClusters.push_back(cluster);
+            }
+        }
+
+
+        // associate humans and clusters by their distances
+        const auto sortedDistances = getSortedDistances(trackedHumans, potentialFootprints);
+
+        for (auto& posDistance : sortedDistances)
+        {
+            if (posDistance.distance > parameters.maxAssociationDistance)
+            {
+                break;
+            }
+            if (posDistance.oldHuman->associated || posDistance.newCluster->associated)
+            {
+                continue;
+            }
+            // no footprint was found before, so associate first footprint
+            if (!posDistance.oldHuman->associatedCluster)
+            {
+                posDistance.oldHuman->associatedCluster = posDistance.newCluster;
+                posDistance.newCluster->associated = true;
+            }
+            // found both footprints, so associate both footprints and update human with new pose
+            else
+            {
+                //calculate center of the points of both footprints combined
+                auto& footprint1 = posDistance.oldHuman->associatedCluster->cluster.points;
+                auto& footprint2 = posDistance.newCluster->cluster.points;
+
+                Eigen::Vector2f centerPos;
+                for (auto& point : footprint1)
+                {
+                    centerPos += point;
+                }
+                for (auto& point : footprint2)
+                {
+                    centerPos += point;
+                }
+                centerPos /= (footprint1.size() + footprint2.size());
+
+                //create pose with orientation from old human
+                core::Pose2D pose = core::Pose2D::Identity();
+                pose.translation() = conv::to2D(centerPos);
+                pose.linear() = posDistance.oldHuman->humanFilter.get().pose.linear();
+
+
+                posDistance.oldHuman->humanFilter.update(pose, measurements.detectionTime);
+
+                posDistance.oldHuman->associatedCluster = nullptr;
+                posDistance.oldHuman->associated = true;
+                posDistance.newCluster->associated = true;
+            }
+        }
+
+        //associate humans where only one footprint was found
+        for (auto& human : trackedHumans)
+        {
+            if (human.associated)
+            {
+                continue;
+            }
+
+            if (human.associatedCluster)
+            {
+                auto& footprint = human.associatedCluster;
+                //create pose with orientation from old human
+                core::Pose2D pose = core::Pose2D::Identity();
+                pose.translation() = conv::to2D(footprint->center);
+                pose.linear() = human.humanFilter.get().pose.linear();
+
+                human.humanFilter.update(pose, measurements.detectionTime);
+
+                human.associatedCluster = nullptr;
+                human.associated = true;
+            }
+        }
+
+        // add unused clusters to return list
+        for (auto& cluster : potentialFootprints)
+        {
+            if (!cluster.associated)
+            {
+                unusedClusters.push_back(cluster.cluster);
+            }
+        }
+
+        return unusedClusters;
+    }
+
     std::vector<human::Human>
     HumanTracker::getTrackedHumans() const
     {
@@ -262,7 +385,7 @@ namespace armarx::navigation::human
             // otherwise the tracked human is prepared for association at the current point in time
             else
             {
-                human.associatedClusters = 0;
+                human.associatedCluster = nullptr;
                 human.associated = false;
                 human.humanFilter.propagation(time);
                 it++;

source/armarx/navigation/human/HumanTracker.h

@@ -72,18 +72,18 @@ namespace armarx::navigation::human
             bool associated;
         };
 
-        struct TrackedHuman
+        struct AdvancedCluster
         {
-            HumanFilter humanFilter;
-            std::optional<std::string> trackingId = std::nullopt;
-            int associatedClusters;
+            Eigen::Vector2f center;
+            Cluster cluster;
             bool associated;
         };
 
-        struct AdvancedCluster
+        struct TrackedHuman
         {
-            Eigen::Vector2f center;
-            Cluster cluster;
+            HumanFilter humanFilter;
+            std::optional<std::string> trackingId = std::nullopt;
+            AdvancedCluster* associatedCluster;
             bool associated;
         };
 
@@ -203,7 +203,9 @@ namespace armarx::navigation::human
          */
         float getClusterSize(Cluster cluster);
         /**
-         * @brief prepareTrackedHumans
+         * @brief prepareTrackedHumans Deletes all tracked humans that received no update for too
+         * long. All remaining tracked humans are prepared for association with the given point in
+         * time.
          */
         void prepareTrackedHumans(DateTime time);