Implement human tracking.

TODO: Integrate new definitions of keypoints

source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt

@@ -35,6 +35,8 @@ armarx_add_component(dynamic_scene_provider
         ## RobotAPICore
         ## RobotAPIInterfaces
         ## RobotAPIComponentPlugins  # For ArViz and other plugins.
+    DEPENDENCIES_PRIVATE
+        range-v3::range-v3
     # DEPENDENCIES_LEGACY
         ## Add libraries that do not provide any targets but ${FOO_*} variables.
         # FOO

source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp

@@ -3,97 +3,172 @@
 
 #include "ArmarXCore/core/exceptions/local/ExpressionException.h"
 
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
+
 namespace armarx::navigation::components::dynamic_scene_provider
 {
 
-    void
-    HumanTracker::update(const Measurements& measurements)
-    {
-        trackedHumans.clear();
 
-        auto positions = getMeasurementPositions(measurements);
+    HumanTracker::DetectedHuman
+    convertHumanPoseToPosition(const armem::human::HumanPose& humanPose)
+    {
+        const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
+        ARMARX_CHECK_NOT_EMPTY(keypoints);
 
-        for (const armem::human::HumanPose& humanPose : measurements.humanPoses)
+        auto centerPos = core::Position::Zero();
+        for (const auto& [_, v] : keypoints)
         {
-            auto centerPos = positions.find(humanPose);
-
-            core::Pose2D pose = core::Pose2D::Identity();
-            pose.translation() = Eigen::Vector2f{centerPos.x(), centerPos.y()};
-            pose.linear() = Eigen::Rotation2Df(0 /*angle*/).toRotationMatrix();
+            centerPos += v.positionGlobal;
+        }
+        centerPos /= static_cast<float>(keypoints.size());
 
-            const human::Human newHuman = {
-                .pose = pose,
-                .linearVelocity = Eigen::Vector2f::Zero(), //TODO more sophisticated guess
-                .detectionTime = measurements.detectionTime};
+        core::Pose2D pose = core::Pose2D::Identity();
+        pose.translation() = centerPos;
+        pose.linear() = Eigen::Rotation2Df(0 /*angle*/).toRotationMatrix();
 
-            trackedHumans.push_back(newHuman);
-        }
+        return pose;
     }
 
-    const std::vector<human::Human>&
-    HumanTracker::getTrackedHumans() const
+    void
+    HumanTracker::update(const Measurements& measurements)
     {
-        return trackedHumans;
-    }
+        //TODO: proper time to live
+        Duration maxAge = Duration::MilliSeconds(500);
 
+        for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
+        {
+            auto& human = *it;
+            if ((measurements.detectionTime - human.human.detectionTime) >= maxAge)
+            {
+                it = trackedHumans.erase(it);
+            }
+            else
+            {
+                human.associated = false;
+                human.human.pose = human.human.estimateAt(measurements.detectionTime);
+                it++;
+            }
+        }
 
-    std::map<const armem::human::HumanPose&, armarx::navigation::core::Position>
-    HumanTracker::getMeasurementPositions(const Measurements& measurements) const
-    {
-        std::map<armem::human::HumanPose&, armarx::navigation::core::Position> map;
+        std::vector<DetectedHuman> newPoses = measurements.humanPoses |
+                                              ranges::views::transform(convertHumanPoseToPosition) |
+                                              ranges::to_vector;
 
-        for (const armem::human::HumanPose& humanPose : measurements.humanPoses)
-        {
+        associateHumans(newPoses);
+    }
 
-            const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
-            ARMARX_CHECK_NOT_EMPTY(keypoints);
+    struct PosDistance
+    {
+        HumanTracker::TrackedHuman& oldHuman;
+        HumanTracker::DetectedHuman& newHuman;
+        const float distance;
+    };
+
+    std::vector<PosDistance>
+    getSortedDistances(const std::vector<HumanTracker::TrackedHuman>& oldHumans,
+                       const std::vector<HumanTracker::DetectedHuman>& newHumans)
+    {
+        std::vector<PosDistance> posDistances;
 
-            auto centerPos = armarx::navigation::core::Position::Zero();
-            for (const auto& [_, v] : keypoints)
+        for (const auto& oldHuman : oldHumans)
+        {
+            if (oldHuman.associated)
             {
-                centerPos += v.positionGlobal;
+                continue;
+            }
+            for (const auto& newHuman : newHumans)
+            {
+                if (newHuman.associated)
+                {
+                    continue;
+                }
+                posDistances.emplace_back(
+                    oldHuman,
+                    newHuman,
+                    (newHuman.pose.translation() - oldHuman.human.pose.translation()).norm());
             }
-            centerPos /= static_cast<float>(keypoints.size());
-
-            map.insert(
-                std::pair<const armem::human::HumanPose&, armarx::navigation::core::Position>(
-                    humanPose, centerPos));
         }
-    }
 
+        std::sort(posDistances.begin(),
+                  posDistances.end(),
+                  [](const PosDistance& a, const PosDistance& b) -> bool
+                  { return a.distance < b.distance; });
 
-    std::map<armem::human::HumanPose&, human::Human&>
-    HumanTracker::getAssociatedHumans(const Measurements& measurements) const
-    {
+        return posDistances;
     }
 
-    std::vector<human::Human>
-    HumanTracker::getMostRecentTrackedHumans() const
+    void
+    HumanTracker::associateHumans(std::vector<DetectedHuman>& detectedHumans)
     {
-        if (trackedHumans.empty())
+        // associate humans by their tracking id
+        for (auto& oldHuman : trackedHumans)
         {
-            return {};
+            if (oldHuman.associated || !oldHuman.trackingId)
+            {
+                continue;
+            }
+            for (auto& newHuman : detectedHumans)
+            {
+                if (newHuman.associated)
+                {
+                    continue;
+                }
+                if (oldHuman.trackingId.value() == newHuman.trackingId)
+                {
+                    associate(oldHuman, newHuman);
+                }
+            }
         }
 
-        std::sort(trackedHumans.begin(),
-                  trackedHumans.end(),
-                  [](human::Human a, human::Human b) -> bool
-                  { return a.detectionTime.operator<=(b.detectionTime); });
-        DateTime mostRecentTime = trackedHumans.back().detectionTime;
 
-        std::vector<human::Human> mostRecentHumans;
+        // associate leftover humans by their distances
+        const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
 
-        for (human::Human trackedHuman : trackedHumans)
+        for (auto& posDistance : sortedDistances)
         {
-            if (trackedHuman.detectionTime == mostRecentTime)
+            if (!posDistance.oldHuman.associated || !posDistance.newHuman.associated)
             {
-                mostRecentHumans.push_back(trackedHuman);
+                continue;
             }
+            associate(posDistance.oldHuman, posDistance.newHuman);
         }
+    }
 
-        return mostRecentHumans;
+    void
+    HumanTracker::associate(TrackedHuman& trackedHuman, DetectedHuman& detectedHuman)
+    {
+        ARMARX_CHECK(!trackedHuman.associated);
+        ARMARX_CHECK(!detectedHuman.associated);
+
+        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;
+
+        trackedHuman.human = {detectedHuman.pose, velocity, detectedHuman.detectionTime};
+        trackedHuman.trackingId = detectedHuman.trackingId;
     }
 
+    std::vector<human::Human>
+    HumanTracker::getTrackedHumans() const
+    {
+        return trackedHumans |
+               ranges::views::transform([](const TrackedHuman& h) -> human::Human
+                                        { return h.human; }) |
+               ranges::to_vector;
+    }
+
+
     void
     HumanTracker::reset()
     {

source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h

@@ -22,21 +22,32 @@ namespace armarx::navigation::components::dynamic_scene_provider
             std::vector<armem::human::HumanPose> humanPoses;
         };
 
+        struct DetectedHuman
+        {
+            core::Pose2D pose;
+            std::string trackingId;
+            DateTime detectionTime;
+            bool associated;
+        };
+
+        struct TrackedHuman
+        {
+            human::Human human;
+            std::optional<std::string> trackingId = std::nullopt;
+            bool associated;
+        };
+
         void update(const Measurements& measurements);
 
-        const std::vector<human::Human>& getTrackedHumans() const;
+        std::vector<human::Human> getTrackedHumans() const;
 
         void reset();
 
     private:
-        std::vector<human::Human> trackedHumans;
-
-        std::map<const armem::human::HumanPose&, armarx::navigation::core::Position>
-        getMeasurementPositions(const Measurements& measurements) const;
+        void associateHumans(std::vector<DetectedHuman>& detectedHumans);
+        void associate(TrackedHuman& tracked, DetectedHuman& detected);
 
-        std::map<armem::human::HumanPose&, human::Human&>
-        getAssociatedHumans(const Measurements& measurements) const;
-
-        std::vector<human::Human> getMostRecentTrackedHumans() const;
+    private:
+        std::vector<TrackedHuman> trackedHumans;
     };
 } // namespace armarx::navigation::components::dynamic_scene_provider