UpdateConsumer.h

Go to the documentation of this file.

#pragma once
 
#include <mutex>
#include <optional>
 
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include "VisionX/libraries/armem_human/client/PersonInstanceReader.h"
#include <VisionX/libraries/armem_human/aron/Person.aron.generated.h>
#include <VisionX/libraries/armem_human/aron/PersonInstance.aron.generated.h>
#include <VisionX/libraries/armem_human/types.h>
 
namespace VisionX::components::person_instance_updater
{
    /**
     * @brief Core logic for fusing face recognition and pose tracking into PersonInstance entities.
     *
     * This class handles the complex task of matching and linking face recognition data with
     * body pose tracking data to maintain a unified representation of tracked people.
     *
     * Matching Strategy:
     * - Face -> PersonInstance: Match by profile ID, then try to link with a nearby pose
     * - Pose -> PersonInstance: Match by tracking ID or spatial proximity to a recognized face
     * - Ensures tracking ID uniqueness: only one PersonInstance per tracking ID
     *
     * Key Operations:
     * - consumeFaceRecognitionUpdate(): Process new face recognition data
     * - consumePoseUpdate(): Process new pose tracking data
     * - Uses spatial proximity (maxFaceHeadDistance) to match faces with pose head positions
     */
    class UpdateConsumer
    {
    public:
        struct Properties
        {
            float maxFaceHeadDistance = 1000.0F; // in [mm]
        };
 
        UpdateConsumer(armarx::armem::client::MemoryNameSystem& mns, const Properties& properties);
 
        /**
         * @brief Process a new face recognition result.
         *
         * Workflow:
         * 1. Find existing PersonInstance by profile ID
         * 2. If found: Update face data and validate pose link is still plausible
         * 3. If not found: Create new PersonInstance
         * 4. Try to match with a nearby pose (within maxFaceHeadDistance)
         * 5. Ensure tracking ID uniqueness across all PersonInstances
         */
        void
        consumeFaceRecognitionUpdate(const armarx::armem::human::FaceRecognition& faceRecognition,
                                     const armarx::armem::MemoryID& faceRecognitionID);
 
        /**
         * @brief Process a new human pose update.
         *
         * Workflow:
         * 1. Search for PersonInstance with matching tracking ID
         * 2. If not found, try to match by spatial proximity to a recognized face
         * 3. If matched: Update PersonInstance with new pose
         * 4. If not matched: Create new PersonInstance based on pose alone
         */
        void consumePoseUpdate(const armarx::armem::human::HumanPose& humanPose,
                               const armarx::armem::MemoryID& poseID);
 
        /**
         * @brief Process a profile update (not yet implemented).
         */
        void consumeProfileUpdate(const armarx::human::arondto::Person& profile,
                                  const armarx::armem::MemoryID& profileID);
 
        /**
         * @brief Check if the consumer is currently processing an update.
         * @return true if busy (mutex locked), false otherwise.
         */
        bool isBusy();
 
    private:
        /**
         * @brief Checks whether a pose and face recognition instance can be plausibly matched.
         *
         * Compares the head position from the pose with the face position from face recognition.
         * They must be within maxFaceHeadDistance to be considered plausible.
         *
         * @return true when plausible (distance within threshold).
         * @return false when not plausible (too far apart or missing data).
         */
        bool checkForPlausability(const ::armarx::armem::human::HumanPose& pose,
                                  const ::armarx::armem::human::FaceRecognition& face);
 
        /**
         * @brief Same as checkForPlausability (appears to be a duplicate).
         */
        bool checkHumanPoseForPlausability(const ::armarx::armem::human::HumanPose& pose,
                                           const ::armarx::armem::human::FaceRecognition& face);
 
        /**
         * @brief Clears the pose ID from a PersonInstance.
         *
         * Used when a pose link is no longer valid (e.g., too far from face, outdated data).
         */
        void removePoseFromPersonInstance(const armarx::armem::MemoryID& personInstanceId,
                                          armarx::armem::human::PersonInstance oldData);
 
        /// Helper struct to bundle a pose with its memory ID
        struct HumanPoseWithID
        {
            armarx::armem::human::HumanPose humanPose;
            armarx::armem::MemoryID memoryId;
        };
 
        /**
         * @brief Find the closest pose to a given face position.
         *
         * Searches all available poses and finds the one whose head position is closest
         * to the given face position (within maxFaceHeadDistance threshold).
         *
         * @param facePos The 3D position of the detected face
         * @return The closest pose with its ID, or std::nullopt if none within threshold
         */
        std::optional<HumanPoseWithID> getClosestPoseID(const Eigen::Vector3f& facePos);
 
        /**
         * @brief Find the closest face recognition to a given head position.
         *
         * Searches all available face recognitions and finds the one closest to the
         * given head position (within maxFaceHeadDistance threshold).
         *
         * @param headPos The 3D position of the head from pose tracking
         * @return The closest face recognition ID, or std::nullopt if none within threshold
         */
        std::optional<armarx::armem::MemoryID>
        getClosestFaceID(const armarx::FramedPosition& headPos);
 
        /// Query and retrieve a HumanPose from memory by its ID
        std::optional<armarx::armem::human::HumanPose>
        humanPoseFromMemId(const armarx::armem::MemoryID& memId);
 
        /// Query and retrieve a FaceRecognition from memory by its ID
        std::optional<armarx::armem::human::FaceRecognition>
        faceRecognitionFromMemId(const armarx::armem::MemoryID& memId);
 
        /**
         * @brief Updates a PersonInstance with a new pose ID and position.
         *
         * Updates both the pose reference and the global position of the PersonInstance
         * based on the head position from the new pose.
         */
        void updateInstanceWithNewPoseId(const armarx::armem::MemoryID& instanceId,
                                         armarx::armem::human::PersonInstance oldInstance,
                                         const armarx::armem::MemoryID& newPoseId,
                                         const ::armarx::armem::human::HumanPose& pose);
 
        /**
         * @brief Creates a new PersonInstance based on a pose without face recognition.
         *
         * Tries to find a nearby face recognition to link with, otherwise creates
         * an instance with only pose information.
         */
        void createNewInstanceBasedOnPose(const armarx::armem::MemoryID& poseId,
                                          const armarx::armem::human::HumanPose& pose);
 
        /// Adds a pose ID to an existing PersonInstance
        void addPoseToInstance(const armarx::armem::MemoryID& memId,
                               const armarx::armem::MemoryID& poseId,
                               armarx::armem::human::PersonInstance currentInstance);
 
        /**
         * @brief Extracts the head position from a HumanPose.
         *
         * Supports different pose models (k4a, openpose, mmm) and extracts the
         * head keypoint position in global coordinates.
         *
         * @return Head position if available, std::nullopt otherwise
         */
        static std::optional<armarx::FramedPosition>
        getHeadPos(armarx::armem::human::HumanPose pose);
 
        /// Calculate Euclidean distance between face and head positions
        static float getDistance(const Eigen::Vector3f& facePos,
                                 const armarx::FramedPosition& headPos);
 
        /**
         * @brief Ensure tracking ID uniqueness: clear a tracking ID from all other PersonInstances.
         *
         * When assigning a tracking ID to a PersonInstance, this ensures no other PersonInstance
         * has the same tracking ID by clearing the pose link from any others using it.
         *
         * @param trackingId The tracking ID to make unique
         * @param exceptProfileID The profile ID of the PersonInstance that should keep this tracking ID
         */
        void ensureTrackingIdUniqueness(const std::string& trackingId,
                                        const armarx::armem::MemoryID& exceptProfileID);
 
        /**
         * @brief Logs the tracking state after processing a face recognition update.
         */
        void logFaceRecognitionUpdateDuration(const armarx::armem::Duration& duration);
 
        /// Helper struct to bundle a PersonInstance with its memory ID
        struct PersonInstanceWithID
        {
            armarx::human::arondto::PersonInstance personInstance;
            armarx::armem::MemoryID memoryId;
        };
 
        /**
         * @brief Finds a PersonInstance matching the given profile ID.
         *
         * @param queryResult The query result containing PersonInstances to search
         * @param profileID The profile ID to match
         * @return The matching PersonInstance with its ID, or std::nullopt if not found
         */
        std::optional<PersonInstanceWithID>
        findMatchingPersonInstance(const armarx::armem::client::QueryResult& queryResult,
                                   const armarx::armem::MemoryID& profileID);
 
        /**
         * @brief Clears the pose ID from a PersonInstance and commits the change.
         *
         * @param personInstanceMatched The PersonInstance with its ID
         */
        void clearPoseIdFromPersonInstance(const PersonInstanceWithID& personInstanceMatched);
 
        /**
         * @brief Sets a new pose ID for a PersonInstance and commits the change.
         *
         * @param personInstanceMatched The PersonInstance with its ID
         * @param poseId The new pose ID to set
         */
        void setPoseIdForPersonInstance(const PersonInstanceWithID& personInstanceMatched,
                                        const armarx::armem::MemoryID& poseId);
 
        /**
         * @brief Checks if a memory ID has all components specified.
         *
         * @param memoryId The memory ID to check
         * @return true if all components are present, false otherwise
         */
        static bool isMemoryIdFullySpecified(const armarx::armem::MemoryID& memoryId);
 
        // Memory readers and writers for accessing different data sources
        armarx::armem::client::Reader faceRecognitionReader;   ///< Read face recognition data
        armarx::armem::client::Writer personInstanceWriter;    ///< Write PersonInstance updates
        armarx::armem::client::Reader personInstanceReader;    ///< Read PersonInstance data
        armarx::armem::client::Reader poseReader;              ///< Read pose tracking data
 
        /// Higher-level reader that can resolve linked data (faces, poses, profiles)
        armarx::armem::human::client::PersonInstanceReader personInstanceReaderV2;
 
        const Properties properties_;  ///< Configuration (e.g., maxFaceHeadDistance threshold)
 
        std::mutex consumeMtx;  ///< Mutex to prevent concurrent processing of updates
    };
} // namespace VisionX::components::person_instance_updater