HumanTracker.cpp

Go to the documentation of this file.

#include "HumanTracker.h"
 
#include <algorithm>
#include <cmath>
#include <map>
#include <string>
#include <vector>
 
#include <boost/geometry.hpp> // IWYU pragma: keep
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/centroid.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/DateTime.h>
 
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/human/HumanFilter.h>
#include <armarx/navigation/human/types.h>
 
#include <VisionX/libraries/armem_human/types.h>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
 
namespace armarx::navigation::human
{
 
    void
    HumanTracker::update(const CameraMeasurement& measurements)
    {
        prepareTrackedHumans(measurements.detectionTime);
 
        // calculate the poses according to the new received measurements
        std::vector<DetectedHuman> newPoses =
            measurements.humanPoses |
            ranges::views::transform(
                [measurements, this](const armem::human::HumanPose& humanPose) -> DetectedHuman
                {
                    return convertHumanPoseToDetectedHuman(measurements.detectionTime, humanPose);
                }) |
            ranges::to_vector;
 
        // associate the new poses from the measurement with the tracked humans
        associateHumans(newPoses);
 
        // add all not associated poses as new tracked humans
        for (const auto& detectedHuman : newPoses)
        {
            if (not detectedHuman.associated)
            {
                //add new tracked human to list of tracked humans
                trackedHumans.push_back(
                    TrackedHuman{.humanFilter = HumanFilter{detectedHuman.pose,
                                                            detectedHuman.detectionTime,
                                                            parameters.useKalmanFilter},
                                 .trackingId = detectedHuman.trackingId,
                                 .associatedCluster = nullptr,
                                 .associated = true,
                                 .confidence = parameters.confidenceCamera});
            }
        }
    }
 
    std::vector<Cluster>
    HumanTracker::update(const LaserMeasurement& measurements)
    {
 
        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 (const auto& cluster : measurements.clusters)
        {
            if (getClusterSize(cluster) <= 2 * parameters.maxFootprintSize)
            {
                potentialFootprints.push_back(AdvancedCluster{
                    .center = getClusterCenter(cluster), .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 between both footprints
                auto& footprint1 = posDistance.oldHuman->associatedCluster->cluster;
                auto& footprint2 = posDistance.newCluster->cluster;
 
                //create pose with orientation from old human
                core::Pose2D pose = core::Pose2D::Identity();
                pose.translation() =
                    (getClusterCenter(footprint1) + getClusterCenter(footprint2)) / 2;
                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;
                posDistance.oldHuman->confidence += parameters.confidenceLaser;
            }
        }
 
        //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() = footprint->center;
                pose.linear() = human.humanFilter.get().pose.linear();
 
                human.humanFilter.update(pose, measurements.detectionTime);
 
                human.associatedCluster = nullptr;
                human.associated = true;
                human.confidence += parameters.confidenceLaser;
            }
        }
 
        // 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
    {
        return trackedHumans |
               ranges::views::transform([](const TrackedHuman& h) -> human::Human
                                        { return h.humanFilter.get(); }) |
               ranges::to_vector;
    }
 
    void
    HumanTracker::reset()
    {
        trackedHumans.clear();
    }
 
    HumanTracker::DetectedHuman
    HumanTracker::convertHumanPoseToDetectedHuman(const DateTime& time,
                                                  const armem::human::HumanPose& humanPose)
    {
        const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
        ARMARX_CHECK_NOT_EMPTY(keypoints);
 
        // calculate the arithmetic mean of all keypoint positions
        Eigen::Vector3f centerPos = Eigen::Vector3f::Zero();
        int size = 0;
        for (const auto& [_, v] : keypoints)
        {
            if (v.positionGlobal.has_value())
            {
                centerPos += v.positionGlobal.value().toEigen();
                size++;
            }
        }
        centerPos /= size;
 
        // calculate the yaw of the specified keypoint representing the orientation if it exists
        double yaw = 0;
        auto head = humanPose.keypoints.find(parameters.rotationKeypoint);
        if (head != humanPose.keypoints.end())
        {
            const armem::human::PoseKeypoint& refKeypoint = head->second;
            ARMARX_CHECK(refKeypoint.orientationGlobal.has_value());
            Eigen::Quaternionf qhead = refKeypoint.orientationGlobal->toEigenQuaternion();
            Eigen::Vector3f vec(1, 0, 0);
            Eigen::Vector3f rot3 = qhead._transformVector(vec);
            Eigen::Vector2f rot2(rot3.x(), rot3.y());
            if (rot2.norm() != 0)
            {
                // calculate angle of rot2
                yaw = atan2(rot2.y(), rot2.x());
            }
        }
 
        // create the new pose with the calculated position and yaw
        core::Pose2D pose = core::Pose2D::Identity();
        pose.translation() = conv::to2D(centerPos);
        pose.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
 
        return {pose, humanPose.humanTrackingId, time, false};
    }
 
    std::vector<HumanTracker::PosHumanDistance>
    HumanTracker::getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
                                     std::vector<HumanTracker::DetectedHuman>& newHumans)
    {
        std::vector<PosHumanDistance> posDistances;
 
        for (auto& oldHuman : oldHumans)
        {
            if (oldHuman.associated)
            {
                continue;
            }
            for (auto& newHuman : newHumans)
            {
                if (newHuman.associated)
                {
                    continue;
                }
                // calculate distance between every possible combination of tracked and detected
                // humans where none of them was associated
                posDistances.push_back(
                    {&oldHuman,
                     &newHuman,
                     (newHuman.pose.translation() - oldHuman.humanFilter.get().pose.translation())
                         .norm()});
            }
        }
 
        // sort the distances ascending by their numeric value
        std::sort(posDistances.begin(),
                  posDistances.end(),
                  [](const PosHumanDistance& a, const PosHumanDistance& b) -> bool
                  { return a.distance < b.distance; });
 
        return posDistances;
    }
 
    std::vector<HumanTracker::PosLaserDistance>
    HumanTracker::getSortedDistances(std::vector<TrackedHuman>& oldHumans,
                                     std::vector<AdvancedCluster>& newClusters)
    {
        std::vector<PosLaserDistance> posDistances;
 
        for (auto& oldHuman : oldHumans)
        {
            for (auto& newCluster : newClusters)
            {
                // calculate distance between every possible combination of tracked human and cluster
                posDistances.push_back(
                    {&oldHuman,
                     &newCluster,
                     (newCluster.center - oldHuman.humanFilter.get().pose.translation()).norm()});
            }
        }
 
        // sort the distances ascending by their numeric value
        std::sort(posDistances.begin(),
                  posDistances.end(),
                  [](const PosLaserDistance& a, const PosLaserDistance& b) -> bool
                  { return a.distance < b.distance; });
 
        return posDistances;
    }
 
    void
    HumanTracker::associateHumans(std::vector<DetectedHuman>& detectedHumans)
    {
        // first, associate humans by their tracking id
        for (auto& oldHuman : trackedHumans)
        {
            if (oldHuman.associated || not oldHuman.trackingId)
            {
                continue;
            }
            for (auto& newHuman : detectedHumans)
            {
                if (newHuman.associated || not newHuman.trackingId)
                {
                    continue;
                }
                // check for every possible combination of old and new humans (that were not already
                // associated and have a tracking id) if their tracking id is the same
                if (oldHuman.trackingId.value() == newHuman.trackingId.value())
                {
                    associate(&oldHuman, &newHuman);
                }
            }
        }
 
        // second, 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 the pair with the currently smallest distance between non-associated humans
            associate(posDistance.oldHuman, posDistance.newHuman);
        }
    }
 
    void
    HumanTracker::associate(TrackedHuman* trackedHuman, DetectedHuman* detectedHuman)
    {
        ARMARX_CHECK(not trackedHuman->associated);
        ARMARX_CHECK(not detectedHuman->associated);
 
 
        trackedHuman->associated = true;
        detectedHuman->associated = true;
 
        trackedHuman->humanFilter.update(detectedHuman->pose, detectedHuman->detectionTime);
        trackedHuman->trackingId = detectedHuman->trackingId;
 
        trackedHuman->confidence = parameters.confidenceCamera;
    }
 
    float
    HumanTracker::getClusterSize(Cluster cluster)
    {
        return 2 * cluster.circle.radius;
    }
 
    Eigen::Vector2f
    HumanTracker::getClusterCenter(Cluster cluster)
    {
        typedef boost::geometry::model::d2::point_xy<double> boost_point;
        typedef boost::geometry::model::polygon<boost_point> boost_polygon;
 
        ARMARX_CHECK(!cluster.convexHull.empty());
        std::vector<boost_point> points;
        for (auto& point : cluster.convexHull)
        {
            points.push_back(boost_point(point.x(), point.y()));
        }
        //make polygon enclosed
        points.push_back(
            boost_point(cluster.convexHull.front().x(), cluster.convexHull.front().y()));
        boost_polygon poly;
        boost::geometry::assign_points(poly, points);
 
        boost_point p = {};
        boost::geometry::centroid(poly, p);
 
        return Eigen::Vector2f{p.x(), p.y()};
    }
 
    void
    HumanTracker::prepareTrackedHumans(DateTime time)
    {
        // iterate over all existing tracked humans
        for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
        {
            auto& human = *it;
            // 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);
            }
            // otherwise the tracked human is prepared for association at the current point in time
            else
            {
                human.associatedCluster = nullptr;
                human.associated = false;
                human.humanFilter.propagation(time);
                it++;
            }
        }
    }
 
} // namespace armarx::navigation::human