HumanTracker.h

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
 * @author     Corvin Navarro Ecker ( corvin dot ecker at student dot kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <ArmarXCore/core/time.h>
 
#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
 
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/human/HumanFilter.h>
#include <armarx/navigation/human/HumanSystemModel.h>
#include <armarx/navigation/human/types.h>
#include <armarx/navigation/memory/types.h>
 
#include <VisionX/libraries/armem_human/types.h>
 
namespace armarx::navigation::human
{
    using Cluster = memory::LaserScannerFeature;
 
    /**
     * @brief The HumanTracker class can be used to track and filter multiple humans. It hides
     * implementation detail on how new detected humans are associated to the old, already tracked
     * humans. New detected humans can be fed by using the update method. The tracked humans can
     * be obtained using the getTrackedHumans method.
     */
    class HumanTracker
    {
    public:
        HumanTracker() = default;
 
        struct CameraMeasurement
        {
            DateTime detectionTime;
            std::vector<armem::human::HumanPose> humanPoses;
        };
 
        struct LaserMeasurement
        {
            DateTime detectionTime;
            std::vector<Cluster> clusters;
        };
 
        struct DetectedHuman
        {
            core::Pose2D pose;
            std::optional<std::string> trackingId;
            DateTime detectionTime;
            bool associated;
        };
 
        struct AdvancedCluster
        {
            Eigen::Vector2f center;
            Cluster cluster;
            bool associated;
        };
 
        struct TrackedHuman
        {
            HumanFilter humanFilter;
            std::optional<std::string> trackingId = std::nullopt;
            AdvancedCluster* associatedCluster;
            bool associated;
            int confidence;
        };
 
        struct PosHumanDistance
        {
            HumanTracker::TrackedHuman* oldHuman;
            HumanTracker::DetectedHuman* newHuman;
            float distance;
        };
 
        struct PosLaserDistance
        {
            HumanTracker::TrackedHuman* oldHuman;
            AdvancedCluster* newCluster;
            float distance;
        };
 
        struct Parameters
        {
            // the keypoint which should be used for calculating the rotation of the humans
            const std::string rotationKeypoint = "Head";
            // the duration after which tracked humans will be erased if no new measurement for
            // this human is found
            Duration maxTrackingAge = Duration::MilliSeconds(1000);
            // the maximum distance in millimeters of two human measurements where they are still
            // associated with each other
            float maxAssociationDistance = 600;
            // the maximum size (aka length) in millimeters a footprint can have
            float maxFootprintSize = 350;
            // alpha value from interval [0,1] to determine how much the current (and respectively
            // the old) velocity should be weighted when calculating the new velocity
            float velocityAlpha = 0.7;
 
            int confidenceCamera = 100;
            int confidenceLaser = -2;
 
            // whether to use the kalman filter inside the HumanFilter
            bool useKalmanFilter = false;
        };
 
        /**
         * @brief HumanTracker::update Updates the tracked humans with the human measurements from a camera. When a
         * measurement is close enough to an existing tracked human, they are associated, otherwise a
         * new tracked human is created. Tracked humans that were not associated with a new measurement
         * for a specified amount of time are removed. New associated measurements for a tracked human
         * are always filtered to provide a less noisy state.
         * @param measurements the new measurements of the environment
         */
        void update(const CameraMeasurement& measurements);
        /**
         * @brief HumanTracker::update Updates the tracked humans with the measurements from a lasersensor.
         * When a measurement is close enough to an existing tracked human, they are associated,
         * otherwise the measurement is not used for human tracking but returned in the list of unused
         * measurements. Tracked humans that were not associated with a new measurement for a
         * specified amount of time are removed. New associated measurements for a tracked human
         * are always filtered to provide a less noisy state.
         * @param measurements
         * @return
         */
        std::vector<Cluster> update(const LaserMeasurement& measurements);
        /**
         * @brief HumanTracker::getTrackedHumans Returns all humans that are currently tracked.
         * @return  the tracked humans
         */
        std::vector<human::Human> getTrackedHumans() const;
        /**
         * @brief HumanTracker::reset Resets this instance to the same state as if it just would have
         * been created.
         */
        void reset();
 
    private:
        /**
         * @brief convertHumanPoseToDetectedHuman Sets every parameter of a detected human according
         * to the human pose and returns the new created detected human.
         * @param time the time of detection
         * @param humanPose the human pose representing the human
         * @return the new created detected human
         */
        DetectedHuman convertHumanPoseToDetectedHuman(const DateTime& time,
                                                      const armem::human::HumanPose& humanPose);
        /**
         * @brief getSortedDistances Returns a sorted vector of the distances between every possible
         * combination (T, D) where T is an old, tracked human and D is a new, detected human and
         * both of them are not already associated. The smallest distance will be the first entry in
         * the vector.
         * @param oldHumans the old, tracked humans
         * @param newHumans the new, detected humans
         * @return the sorted vector of distances with references to the according humans
         */
        std::vector<PosHumanDistance>
        getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
                           std::vector<HumanTracker::DetectedHuman>& newHumans);
        /**
         * @brief getSortedDistances Returns a sorted vector of the distances between every possible
         * combination (T, C) where T is an old, tracked human and C is a new, detected cluster.
         * The smallest distance will be the first entry in the vector.
         * @param oldHumans
         * @param newClusters
         * @return
         */
        std::vector<PosLaserDistance>
        getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
                           std::vector<AdvancedCluster>& newClusters);
        /**
         * @brief HumanTracker::associateHumans Associates those tracked and detected humans that
         * belong together.
         * @param detectedHumans The detected humans against which the saved list of tracked humans is
         * matched.
         */
        void associateHumans(std::vector<DetectedHuman>& detectedHumans);
        /**
         * @brief HumanTracker::associate Associates the given tracked and detected human. Therefore it
         * updates all necessary variables of the TrackedHuman
         * @param trackedHuman the tracked human
         * @param detectedHuman the detected human
         */
        void associate(TrackedHuman* tracked, DetectedHuman* detected);
        /**
         * @brief getClusterSize Returns the size of the given cluster. That is for now the maximum
         * distance between two points.
         * @param cluster The cluster whose size is measured
         * @return the size
         */
        float getClusterSize(Cluster cluster);
        /**
         * @brief HumanTracker::getClusterCenter Returns the center of the given cluster. That is
         * calculated as the mean of all points.
         * @param cluster The cluster whose center is calculated
         * @return the center
         */
        Eigen::Vector2f getClusterCenter(Cluster cluster);
        /**
         * @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);
 
 
    private:
        std::vector<TrackedHuman> trackedHumans;
        Parameters parameters;
    };
} // namespace armarx::navigation::human