human tracker skeleton

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

+
+#include "HumanTracker.h"
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+#include "armarx/navigation/human/types.h"
+#include <armarx/navigation/conversions/eigen.h>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+
+
+    HumanTracker::DetectedHuman
+    convertHumanPoseToPosition(const DateTime& time, const armem::human::HumanPose& humanPose)
+    {
+        const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
+        ARMARX_CHECK_NOT_EMPTY(keypoints);
+
+        Eigen::Vector3f centerPos;
+        int size = 0;
+        for (const auto& [_, v] : keypoints)
+        {
+            if (v.positionGlobal.has_value())
+            {
+                centerPos += v.positionGlobal.value().toEigen();
+                size++;
+            }
+        }
+        centerPos /= size;
+
+        core::Pose2D pose = core::Pose2D::Identity();
+        pose.translation() = conv::to2D(centerPos);
+        //TODO: angle
+        pose.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+
+        return {pose, humanPose.humanTrackingId, time, false};
+    }
+
+    void
+    HumanTracker::update(const Measurements& measurements)
+    {
+        trackedHumans.clear();
+
+        for (const armem::human::HumanPose& measurement : measurements.humanPoses)
+        {
+            human::Human human{
+                .pose = conv::to2D(
+                    core::Pose(Eigen::Translation3f(measurement.keypoints.begin()->second.positionGlobal->toEigen()))),
+                .linearVelocity = Eigen::Vector2f::Zero(),
+                .detectionTime = 0};
+
+            trackedHumans.push_back(TrackedHuman{
+                .human = human, .trackingId = std::to_string(trackedHumans.size()), .associated = true});
+        }
+
+        return; // FIXME remove section above
+
+        for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
+        {
+            auto& human = *it;
+            if ((measurements.detectionTime - human.human.detectionTime) >=
+                parameters.maxTrackingAge)
+            {
+                it = trackedHumans.erase(it);
+            }
+            else
+            {
+                human.associated = false;
+                human.human.pose = human.human.estimateAt(measurements.detectionTime);
+                it++;
+            }
+        }
+
+        std::vector<DetectedHuman> newPoses =
+            measurements.humanPoses |
+            ranges::views::transform(
+                [measurements](const armem::human::HumanPose& humanPose) -> DetectedHuman
+                { return convertHumanPoseToPosition(measurements.detectionTime, humanPose); }) |
+            ranges::to_vector;
+
+        associateHumans(newPoses);
+    }
+
+    struct PosDistance
+    {
+        HumanTracker::TrackedHuman* oldHuman;
+        HumanTracker::DetectedHuman* newHuman;
+        float distance;
+    };
+
+    std::vector<PosDistance>
+    getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+                       std::vector<HumanTracker::DetectedHuman>& newHumans)
+    {
+        std::vector<PosDistance> posDistances;
+
+        for (auto& oldHuman : oldHumans)
+        {
+            if (oldHuman.associated)
+            {
+                continue;
+            }
+            for (auto& newHuman : newHumans)
+            {
+                if (newHuman.associated)
+                {
+                    continue;
+                }
+                posDistances.push_back(
+                    {&oldHuman,
+                     &newHuman,
+                     (newHuman.pose.translation() - oldHuman.human.pose.translation()).norm()});
+            }
+        }
+
+        std::sort(posDistances.begin(),
+                  posDistances.end(),
+                  [](const PosDistance& a, const PosDistance& b) -> bool
+                  { return a.distance < b.distance; });
+
+        return posDistances;
+    }
+
+    void
+    HumanTracker::associateHumans(std::vector<DetectedHuman>& detectedHumans)
+    {
+        // associate humans by their tracking id
+        for (auto& oldHuman : trackedHumans)
+        {
+            if (oldHuman.associated || !oldHuman.trackingId)
+            {
+                continue;
+            }
+            for (auto& newHuman : detectedHumans)
+            {
+                if (newHuman.associated || !newHuman.trackingId)
+                {
+                    continue;
+                }
+                if (oldHuman.trackingId.value() == newHuman.trackingId.value())
+                {
+                    associate(&oldHuman, &newHuman);
+                }
+            }
+        }
+
+
+        // associate leftover humans by their distances
+        const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
+
+        for (auto& posDistance : sortedDistances)
+        {
+            if (posDistance.distance > parameters.maxAssociationDistance)
+            {
+                break;
+            }
+            if (posDistance.oldHuman->associated || posDistance.newHuman->associated)
+            {
+                continue;
+            }
+            associate(posDistance.oldHuman, posDistance.newHuman);
+        }
+    }
+
+    void
+    HumanTracker::associate(TrackedHuman* trackedHuman, DetectedHuman* detectedHuman)
+    {
+        ARMARX_CHECK(!trackedHuman->associated);
+        ARMARX_CHECK(!detectedHuman->associated);
+
+        trackedHuman->associated = true;
+        detectedHuman->associated = true;
+
+        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 =
+            parameters.velocityAlpha * linVelocity +
+            (1 - parameters.velocityAlpha) * 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()
+    {
+        trackedHumans.clear();
+    }
+
+} // namespace armarx::navigation::components::dynamic_scene_provider