UpdateConsumer.cpp

Go to the documentation of this file.

#include "UpdateConsumer.h"
 
#include <mutex>
#include <optional>
#include <string>
 
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/ScopedStopWatch.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h> // IWYU pragma: keep
#include <RobotAPI/libraries/armem/core/Commit.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include "VisionX/libraries/armem_human/client/PersonInstanceReader.h"
#include <VisionX/components/person_instance_updater/PersonInstanceUpdater.h>
#include <VisionX/libraries/armem_human/aron/FaceRecognition.aron.generated.h>
#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.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/aron_conversions.h>
#include <VisionX/libraries/armem_human/memory_ids.h>
#include <VisionX/libraries/armem_human/types.h>
#include <VisionX/libraries/human/pose/model/k4a_bt_body_32.h>
#include <VisionX/libraries/human/pose/model/mmm_body_68.h>
#include <VisionX/libraries/human/pose/model/openpose_body_25.h>
 
namespace VisionX::components::person_instance_updater
{
 
    UpdateConsumer::UpdateConsumer(armarx::armem::client::MemoryNameSystem& mns,
                                   const Properties& properties) :
        faceRecognitionReader(mns.useReader(armarx::human::FaceRecognitionCoreSegmentID)),
        personInstanceWriter(mns.useWriter(armarx::human::PersonInstanceCoreSegmentID)),
        personInstanceReader(mns.useReader(armarx::human::PersonInstanceCoreSegmentID)),
        poseReader(mns.useReader(armarx::human::PoseCoreSegmentID)),
        properties_(properties)
    {
        personInstanceReaderV2.connect(mns);
 
        ARMARX_IMPORTANT << "At startup:";
        {
            armarx::armem::human::client::PersonInstanceReader::QueryResolved query;
            query.providerName = "";
            query.timestamp = armarx::Clock::Now();
            query.maxAge = armarx::Duration::Seconds(10);
            query.resolveFaceDetection = false;
            query.resolveHumanPose = true;
            query.resolveProfile = true;
 
            const auto result = personInstanceReaderV2.queryResolved(query);
 
            ARMARX_DEBUG << "Humans with tracking ids:";
            for (const auto& personInstance : result.personInstances)
            {
                std::string name = "~unknown~";
                if (personInstance.profile.has_value())
                {
                    name = personInstance.profile->id.firstName + " " +
                           personInstance.profile->id.lastName;
                }
 
                std::string trackingId = "~none~";
                if (personInstance.humanPose.has_value() and
                    personInstance.humanPose->humanTrackingId.has_value())
                {
                    trackingId = personInstance.humanPose->humanTrackingId.value();
                }
 
                ARMARX_DEBUG << trackingId << ": " << name;
            }
        }
    }
 
    //TODO: add some form of history to allow for better understanding of memory changed of the personInstances
    //TODO: add visualization of
 
    /**
     * Process a face recognition update:
     * 1. Find existing PersonInstance by profile ID
     * 2. If found and has valid pose: check if pose is still plausible (nearby)
     *    - If implausible: clear pose link and try to find new matching pose
     * 3. If found but no pose: try to find matching pose by proximity
     * 4. If not found: create new PersonInstance and try to find matching pose
     * 5. Ensure tracking ID uniqueness: remove pose links from other PersonInstances
     *    that were using the same tracking ID
     */
    void
    UpdateConsumer::consumeFaceRecognitionUpdate(
        const armarx::armem::human::FaceRecognition& faceRecognition,
        const armarx::armem::MemoryID& faceRecognitionID)
    {
        std::lock_guard g{consumeMtx}; // Ensure thread-safe processing
 
        const bool verbose = false;
 
        ARMARX_DEBUG << "Consuming face recognition update for "
                     << (faceRecognition.profileID.has_value()
                             ? faceRecognition.profileID->entityName
                             : "unknown");
 
        armarx::core::time::ScopedStopWatch stopwatch(
            [this](const armarx::armem::Duration& duration)
            { logFaceRecognitionUpdateDuration(duration); });
 
        // Query all existing PersonInstances to check if this face already has one
        armarx::armem::client::QueryResult queryResult =
            personInstanceReader.getLatestSnapshotsIn(armarx::human::PersonInstanceCoreSegmentID);
 
        // Face recognition without profile ID cannot be matched to a PersonInstance
        if (not faceRecognition.profileID.has_value())
        {
            ARMARX_INFO << deactivateSpam()
                        << "Face recognition " + faceRecognitionID.entityName +
                               " does not provide valid profileID, finding matching "
                               "personInstance is not possible.";
            return;
        }
 
        const armarx::armem::MemoryID& profileID = faceRecognition.profileID.value();
 
        ARMARX_DEBUG << "Trying to match face recognition with profile ID: "
                     << profileID.entityName;
 
        if (queryResult.success)
        {
            //TODO: add early return in case face recognition id is already used by a personInstance
 
            // ========== STEP 1: Search for existing PersonInstance with matching profile ID ==========
            std::optional<PersonInstanceWithID> personInstanceMatched =
                findMatchingPersonInstance(queryResult, profileID);
 
            // ========== STEP 2: If PersonInstance found, validate and update pose link ==========
            if (personInstanceMatched.has_value())
            {
                ARMARX_DEBUG << "Found existing PersonInstance, updating it.";
                ARMARX_DEBUG << "matched PersonInstance entity name: "
                             << personInstanceMatched->memoryId.entityName;
 
                // First, update the face recognition data (use toAron to convert types)
                toAron(personInstanceMatched->personInstance.faceRecognitionID, faceRecognitionID);
 
                // Extract the current pose ID from the matched PersonInstance
                armarx::armem::MemoryID poseID;
                fromAron(personInstanceMatched->personInstance.poseID, poseID);
 
                ARMARX_DEBUG << "Current linked pose ID: "
                             << (isMemoryIdFullySpecified(poseID) ? poseID.entityName : "~none~");
 
                // Retrieve the actual pose and face data from memory
                const std::optional<::armarx::armem::human::HumanPose> humanPose =
                    humanPoseFromMemId(poseID);
                const std::optional<armarx::armem::human::FaceRecognition> faceRecognition =
                    faceRecognitionFromMemId(faceRecognitionID);
 
                // We just received this face recognition, so it must exist
                ARMARX_CHECK(faceRecognition.has_value());
 
                // Check if the current pose link is still valid (plausible)
                bool poseIdWasCleared = false;
                if (humanPose.has_value())
                {
                    // If pose exists, check if face and head positions are close enough
                    if (not checkHumanPoseForPlausability(humanPose.value(),
                                                          faceRecognition.value()))
                    {
                        ARMARX_INFO << deactivateSpam(10)
                                    << "The face recognition and the human pose are not "
                                       "consistent. Dropping the human pose track.";
                        clearPoseIdFromPersonInstance(personInstanceMatched.value());
                        poseIdWasCleared = true;
                    }
                }
                else if (isMemoryIdFullySpecified(poseID))
                {
                    // Pose ID was set but pose data no longer exists (outdated/deleted)
                    ARMARX_INFO << deactivateSpam(10)
                                << "The human pose is likely outdated and meaningless. We remove "
                                   "the link to it.";
                    clearPoseIdFromPersonInstance(personInstanceMatched.value());
                    poseIdWasCleared = true;
                }
 
                ARMARX_DEBUG << "Human Pose exists and memory ID is fully specified";
 
                // Only refresh globalPose from the face when no valid pose provides a
                // richer (orientation-bearing) pose; otherwise keep the pose-writer's data.
                if (poseIdWasCleared or not humanPose.has_value())
                {
                    Eigen::Isometry3f globalPose = Eigen::Isometry3f::Identity();
                    globalPose.translation() = faceRecognition.value().position3DGlobal;
                    personInstanceMatched->personInstance.pose = globalPose.matrix();
                }
 
                // If we still have a valid pose link, we're done
                if (not poseIdWasCleared and isMemoryIdFullySpecified(poseID))
                {
                    ARMARX_DEBUG << "A valid pose id for the person instance exists";
                    ARMARX_DEBUG << "Commiting PersonInstance for ";
                    ARMARX_DEBUG << VAROUT(profileID);
                    // Ensure no other person has this tracking ID and commit
                    // Commit the new PersonInstance to memory
                    armarx::armem::EntityUpdate update;
                    update.entityID =
                        armarx::human::PersonInstanceCoreSegmentID
                            .withProviderSegmentName(PersonInstanceUpdater::provider_name)
                            .withEntityName(faceRecognition.value().profileID.value().entityName);
                    update.referencedTime = armarx::armem::Time::Now();
                    update.instancesData = {personInstanceMatched.value().personInstance.toAron()};
                    personInstanceWriter.commit(update);
                    return;
                }
 
                // No valid pose link - try to find a matching pose by spatial proximity
                ARMARX_VERBOSE
                    << "The person instance does not have an assigned pose id. Will try to "
                       "match existing ones.";
 
                {
                    ARMARX_CHECK(faceRecognition.has_value());
                    // Find the closest pose to this face position
                    const std::optional<HumanPoseWithID> closestPoseID =
                        getClosestPoseID(faceRecognition->position3DGlobal);
 
                    if (closestPoseID.has_value())
                    {
                        // Found a nearby pose - link it to this PersonInstance
                        ARMARX_INFO << deactivateSpam(10)
                                    << "Found matching human pose with tracking id "
                                    << QUOTED(closestPoseID->humanPose.humanTrackingId.value_or(""))
                                    << " for "
                                    << personInstanceMatched->personInstance.profileID.entityName;
                        setPoseIdForPersonInstance(personInstanceMatched.value(),
                                                   closestPoseID->memoryId);
 
                        armarx::armem::MemoryID profileId;
                        fromAron(personInstanceMatched->personInstance.profileID, profileId);
 
                        // Ensure no other person has this tracking ID
                        ensureTrackingIdUniqueness(closestPoseID->humanPose.humanTrackingId.value(),
                                                   profileId);
                    }
                    else
                    {
                        ARMARX_VERBOSE
                            << "No human pose could be found close to the given face position";
                    }
                }
            }
 
 
            // ========== STEP 3: No existing PersonInstance found - create new one ==========
            else
            {
                armarx::human::arondto::PersonInstance personInstance;
 
                // Set the face recognition and profile IDs
                toAron(personInstance.faceRecognitionID, faceRecognitionID);
                toAron(personInstance.profileID, profileID);
 
                // Try to find a pose nearby that we can link to this new PersonInstance
                const std::optional<HumanPoseWithID> closestPoseID =
                    getClosestPoseID(faceRecognition.position3DGlobal);
                if (closestPoseID.has_value())
                {
                    std::string name = "";
                    if (faceRecognition.profileID.has_value())
                    {
                        name = faceRecognition.profileID->entityName;
                    }
 
                    ARMARX_INFO << deactivateSpam(10) << "Found matching human pose (tracking id "
                                << QUOTED(closestPoseID->humanPose.humanTrackingId.value_or(""))
                                << ") for face recognition of " << QUOTED(name)
                                << " due to spatial proximity.";
                    toAron(personInstance.poseID, closestPoseID->memoryId);
 
                    // Ensure tracking ID uniqueness: no other person should have this tracking ID
                    ensureTrackingIdUniqueness(closestPoseID->humanPose.humanTrackingId.value(),
                                               profileID);
                }
 
                if (faceRecognition.profileID.value().entityName.empty())
                {
                    ARMARX_WARNING << "Face Recognition has no valid profile";
                    return;
                }
 
                // Set the global position based on face position (orientation unknown)
                ARMARX_VERBOSE << "update pose based on latest face recognition result ("
                               << faceRecognition.position3DGlobal.transpose() << ")";
                Eigen::Isometry3f globalPose = Eigen::Isometry3f::Identity();
                // orientation is not available from face recognition
                globalPose.translation() = faceRecognition.position3DGlobal;
                personInstance.pose = globalPose.matrix();
 
                // Commit the new PersonInstance to memory
                armarx::armem::EntityUpdate update;
                update.entityID = armarx::human::PersonInstanceCoreSegmentID
                                      .withProviderSegmentName(PersonInstanceUpdater::provider_name)
                                      .withEntityName(faceRecognition.profileID.value().entityName);
                update.referencedTime = armarx::armem::Time::Now();
                update.instancesData = {personInstance.toAron()};
                personInstanceWriter.commit(update);
            }
        }
    }
 
    /**
     * Process a pose update:
     * 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 ID
     * 4. If not matched: Create new PersonInstance based on pose alone
     *
     * Note: Whenever a pose can be plausibly matched to a recognized face, it will be.
     * Matched instances get their poseId's updated. If no match is available, a new one is created.
     */
    void
    UpdateConsumer::consumePoseUpdate(const armarx::armem::human::HumanPose& humanPose,
                                      const armarx::armem::MemoryID& poseID)
    {
        std::lock_guard g{consumeMtx}; // Ensure thread-safe processing
 
        armarx::core::time::ScopedStopWatch stopwatch(
            [](const armarx::armem::Duration& duration)
            { ARMARX_DEBUG << "consumePoseUpdate took " << duration.toMilliSeconds() << " ms."; });
 
        ARMARX_INFO << deactivateSpam(10) << "Consuming pose update with tracking ID: "
                    << (humanPose.humanTrackingId.has_value() ? humanPose.humanTrackingId.value()
                                                              : "none");
 
        // Query all existing PersonInstances
        armarx::armem::client::QueryResult queryResult =
            personInstanceReader.getLatestSnapshotsIn(armarx::human::PersonInstanceCoreSegmentID);
 
        // ========== Search for matching PersonInstance ==========
        // TODO: what to do if multiple instances match?
 
        // Matching strategies (in priority order):
        // 1. Tracking ID: pose has same humanTrackingId as PersonInstance's current pose
        //    (overrides any spatial match by setting closestDistance to a sentinel)
        // 2. Spatial proximity: pose head position is close to a face recognition
        ARMARX_DEBUG << "Iterating over existing instances";
        if (queryResult.success)
        {
            std::optional<::armarx::armem::human::PersonInstance> matchingInstance = std::nullopt;
            std::optional<::armarx::armem::MemoryID> matchingInstanceId = std::nullopt;
            float closestDistance = std::numeric_limits<float>::max();
 
            queryResult.memory.forEachInstanceWithDataAs(
                [&](const armarx::armem::wm::EntityInstanceBase<
                    armarx::human::arondto::PersonInstance>& instance)
                {
                    ARMARX_DEBUG << "In iteration: " << instance.id().entityName;
 
                    const auto& personInstance = instance.data();
 
                    armarx::armem::MemoryID currentPoseID;
                    fromAron(personInstance.poseID, currentPoseID);
                    auto pose = humanPoseFromMemId(currentPoseID);
 
                    // ===== PRIORITY 2: Match by spatial proximity (face-head distance) =====
                    // Find the CLOSEST PersonInstance, not just the first plausible one
                    armarx::armem::MemoryID currentRecognitionId;
                    fromAron(personInstance.faceRecognitionID, currentRecognitionId);
                    auto recognition = faceRecognitionFromMemId(currentRecognitionId);
                    if (recognition.has_value())
                    {
                        // Check if this pose's head is close to the face recognition position
                        auto headPos = getHeadPos(humanPose);
                        if (headPos.has_value())
                        {
                            float distance =
                                getDistance(recognition->position3DGlobal, headPos.value());
                            ARMARX_VERBOSE << "Distance from pose to " << instance.id().entityName
                                           << ": " << distance << "mm";
 
                            // Keep track of the closest match within threshold
                            if (distance < closestDistance &&
                                distance <= properties_.maxFaceHeadDistance)
                            {
                                armarx::armem::human::PersonInstance bo;
                                fromAron(personInstance, bo);
                                matchingInstance = bo;
                                matchingInstanceId = instance.id();
                                closestDistance = distance;
                                ARMARX_DEBUG << "Instance " << instance.id().entityName
                                             << " is closer (distance: " << distance << "mm)";
                            }
                        }
                    }
 
                    // ===== PRIORITY 1: Match by tracking ID =====
                    // This takes precedence if we find an exact tracking ID match
                    if (pose.has_value())
                    {
                        ARMARX_INFO << deactivateSpam(10) << "Person instance "
                                    << instance.id().entityName << " with tracking id "
                                    << QUOTED(pose->humanTrackingId.value_or(""));
 
                        if (pose->humanTrackingId.has_value() and
                            humanPose.humanTrackingId.has_value() and
                            pose->humanTrackingId.value() == humanPose.humanTrackingId.value())
                        {
                            // Exact tracking ID match - this overrides spatial proximity
                            ARMARX_DEBUG << "Found exact tracking ID match for "
                                         << instance.id().entityName;
                            armarx::armem::human::PersonInstance bo;
                            fromAron(personInstance, bo);
                            matchingInstance = bo;
                            matchingInstanceId = instance.id();
                            closestDistance = -1.0f; // Sentinel value to indicate tracking ID match
                        }
                    }
                });
 
            // ========== Handle the matching result ==========
            // Either a matching instance was found (update it with new pose),
            // OR no match was found (create new PersonInstance)
            if (matchingInstance.has_value())
            {
                ARMARX_DEBUG << "Found matching instance, handling it";
 
                // Ensure tracking ID uniqueness before updating
                if (humanPose.humanTrackingId.has_value())
                {
                    armarx::armem::MemoryID profileId = matchingInstance->profileID;
                    ensureTrackingIdUniqueness(humanPose.humanTrackingId.value(), profileId);
                }
 
                // Update the PersonInstance with the new pose ID and position
                updateInstanceWithNewPoseId(
                    matchingInstanceId.value(), matchingInstance.value(), poseID, humanPose);
            }
            else
            {
                // No matching PersonInstance - create a new one based on this pose
                ARMARX_DEBUG << "No matching instance found, creating new...";
                createNewInstanceBasedOnPose(poseID, humanPose);
            }
        }
        else
        {
            ARMARX_WARNING << deactivateSpam() << "Fetching instance memory failed.";
        }
    }
 
    void
    UpdateConsumer::consumeProfileUpdate(const armarx::human::arondto::Person& /*profile*/,
                                         const armarx::armem::MemoryID& /*profileID*/)
    {
        //TODO: implement profile update logic
        std::lock_guard g{consumeMtx};
    }
 
    void
    UpdateConsumer::removePoseFromPersonInstance(const armarx::armem::MemoryID& personInstanceId,
                                                 armarx::armem::human::PersonInstance data)
    {
        data.poseID = {};
        armarx::human::arondto::PersonInstance dto;
        toAron(dto, data);
        armarx::armem::EntityUpdate update{.entityID = personInstanceId.getEntityID(),
                                           .instancesData = {dto.toAron()},
                                           .referencedTime = armarx::armem::Time::Now()};
        ARMARX_INFO << deactivateSpam()
                    << "Removing pose from PersonInstance: " << personInstanceId.entityName;
        personInstanceWriter.commit(update);
    }
 
    /**
     * Create a new PersonInstance based on a pose.
     * Tries to find a nearby face recognition to link with, otherwise creates
     * a PersonInstance with only pose information (no identity).
     */
    void
    UpdateConsumer::createNewInstanceBasedOnPose(const armarx::armem::MemoryID& poseId,
                                                 const armarx::armem::human::HumanPose& pose)
    {
        ARMARX_INFO << deactivateSpam(10) << "Processing tracking id "
                    << QUOTED(pose.humanTrackingId.value_or("")) << ".";
 
        // Extract head position from the pose
        std::optional<armarx::FramedPosition> headPos = getHeadPos(pose);
        if (headPos.has_value())
        {
            // Try to find a nearby face recognition to link with
            std::optional<armarx::armem::MemoryID> faceRecognitionID =
                getClosestFaceID(headPos.value());
            if (faceRecognitionID.has_value())
            {
                armarx::human::arondto::PersonInstance personInstance;
                toAron(personInstance.faceRecognitionID, faceRecognitionID.value());
 
                // determine profile entity name, to generate personinstance entity name
 
                auto faceRecognition = faceRecognitionFromMemId(faceRecognitionID.value());
                if (not faceRecognition.has_value())
                {
                    ARMARX_WARNING << deactivateSpam() << "Could not get face recognition from id";
                    return;
                }
 
                const auto profileId = faceRecognition->profileID;
                if (not profileId.has_value())
                {
                    ARMARX_INFO << deactivateSpam(10)
                                << "While creating a new instance based on pose: closest face "
                                   "recognition has no attached profile. Giving up.";
                    return;
                }
 
                const std::string& entityName = profileId->entityName;
 
                // set profile id of instance
                toAron(personInstance.profileID, profileId.value());
 
                {
                    const auto headKp =
                        armarx::human::pose::model::k4a_bt_body_32::JointNames.to_name(
                            armarx::human::pose::model::k4a_bt_body_32::Joints::Head);
                    if (pose.keypoints.find(headKp) == pose.keypoints.end())
                    {
                        ARMARX_WARNING << "Pose does not contain head keypoint " << QUOTED(headKp);
                    }
 
                    // update pose based on latest human pose result
                    {
                        ARMARX_VERBOSE << "update pose based on latest human pose result";
                        Eigen::Isometry3f globalPose = Eigen::Isometry3f::Identity();
 
                        if (pose.keypoints.at(headKp).orientationGlobal)
                        {
                            ARMARX_CHECK_EQUAL(
                                pose.keypoints.at(headKp).orientationGlobal->getFrame(),
                                armarx::GlobalFrame);
                            globalPose.linear() =
                                pose.keypoints.at(headKp).orientationGlobal->toEigen();
                        }
 
                        if (pose.keypoints.at(headKp).positionGlobal)
                        {
                            ARMARX_CHECK_EQUAL(pose.keypoints.at(headKp).positionGlobal->getFrame(),
                                               armarx::GlobalFrame);
                            globalPose.translation() =
                                pose.keypoints.at(headKp).positionGlobal->toEigen();
                        }
 
                        personInstance.pose = globalPose.matrix();
                    }
                }
 
                //TODO: get profile id
                //toAron(personInstance.profileID, profileID);
                toAron(personInstance.poseID, poseId);
 
                // Ensure tracking ID uniqueness before creating the new PersonInstance
                if (pose.humanTrackingId.has_value())
                {
                    ensureTrackingIdUniqueness(pose.humanTrackingId.value(), profileId.value());
                }
 
                armarx::armem::EntityUpdate update;
                update.entityID = armarx::human::PersonInstanceCoreSegmentID
                                      .withProviderSegmentName(PersonInstanceUpdater::provider_name)
                                      .withEntityName(entityName);
                update.referencedTime = armarx::armem::Time::Now();
                update.instancesData = {personInstance.toAron()};
                personInstanceWriter.commit(update);
            }
            else
            {
                ARMARX_WARNING << deactivateSpam() << "Could not get closest face";
            }
        }
    }
 
    void
    UpdateConsumer::updateInstanceWithNewPoseId(const armarx::armem::MemoryID& instanceId,
                                                armarx::armem::human::PersonInstance oldInstance,
                                                const armarx::armem::MemoryID& newPoseId,
                                                const ::armarx::armem::human::HumanPose& pose)
    {
 
        // update pose based on latest human pose result
        {
            const auto headKp = armarx::human::pose::model::k4a_bt_body_32::JointNames.to_name(
                armarx::human::pose::model::k4a_bt_body_32::Joints::Head);
 
            oldInstance.globalPose.setIdentity();
            if (pose.keypoints.at(headKp).orientationGlobal)
            {
                ARMARX_CHECK_EQUAL(pose.keypoints.at(headKp).orientationGlobal->getFrame(),
                                   armarx::GlobalFrame);
                oldInstance.globalPose.linear() =
                    pose.keypoints.at(headKp).orientationGlobal->toEigen();
            }
 
            if (pose.keypoints.at(headKp).positionGlobal)
            {
                ARMARX_CHECK_EQUAL(pose.keypoints.at(headKp).positionGlobal->getFrame(),
                                   armarx::GlobalFrame);
                oldInstance.globalPose.translation() =
                    pose.keypoints.at(headKp).positionGlobal->toEigen();
            }
 
            ARMARX_DEBUG << "update pose based on latest human pose result: "
                         << oldInstance.globalPose.translation().transpose();
        }
 
 
        ARMARX_DEBUG << "Start update with new pose id";
        oldInstance.poseID = newPoseId;
        armarx::human::arondto::PersonInstance dto;
        toAron(dto, oldInstance);
        armarx::armem::EntityUpdate update{
            .entityID = instanceId.getEntityID(),
            .instancesData = {dto.toAron()},
            .referencedTime = armarx::armem::Time::Now(),
            .confidence = 1.0F,
        };
 
        ARMARX_DEBUG << "Committing new person instance with updated pose for entity: "
                     << instanceId.entityName;
        personInstanceWriter.commit(update);
    }
 
    bool
    UpdateConsumer::checkHumanPoseForPlausability(
        const ::armarx::armem::human::HumanPose& pose,
        const ::armarx::armem::human::FaceRecognition& face)
    {
        // Here, we check whether the "anonymous" human pose and the detected face are consistent.
        // If a new face detection is not consistent with the human pose, we don't accept it.
 
        ARMARX_DEBUG << "checking for plausability";
        auto facePosFromPose = getHeadPos(pose);
        if (not facePosFromPose.has_value())
        {
            ARMARX_WARNING << "Conversion failed";
            return false;
        }
        auto facePosFromFaceRecognition = face.position3DGlobal;
        float dist = getDistance(facePosFromFaceRecognition, facePosFromPose.value());
        ARMARX_DEBUG << "distance is: " << dist;
        return dist <= properties_.maxFaceHeadDistance;
    }
 
    bool
    UpdateConsumer::checkForPlausability(const ::armarx::armem::human::HumanPose& pose,
                                         const ::armarx::armem::human::FaceRecognition& face)
    {
        ARMARX_DEBUG << "checking for plausability";
        auto facePosFromPose = getHeadPos(pose);
        if (not facePosFromPose.has_value())
        {
            ARMARX_WARNING << "Conversion failed";
            return false;
        }
        auto facePosFromFaceRecognition = face.position3DGlobal;
        float dist = getDistance(facePosFromFaceRecognition, facePosFromPose.value());
        ARMARX_DEBUG << "distance is: " << dist;
        return dist <= properties_.maxFaceHeadDistance;
    }
 
    /**
     * Find the closest pose to a given face position.
     *
     * Iterates through all available poses and finds the one whose head position
     * is closest to the given face position. Only considers poses within the
     * 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<UpdateConsumer::HumanPoseWithID>
    UpdateConsumer::getClosestPoseID(const Eigen::Vector3f& facePos)
    {
        // Query all available poses
        armarx::armem::client::QueryResult queryResult =
            poseReader.getLatestSnapshotsIn(armarx::human::PoseCoreSegmentID);
 
        std::optional<UpdateConsumer::HumanPoseWithID> closestPoseID;
        float closestDistance =
            properties_.maxFaceHeadDistance; // Only consider poses within threshold
 
        if (queryResult.success)
        {
            // Iterate through all poses to find the closest one
            queryResult.memory.forEachInstanceWithDataAs(
                [&facePos, &closestPoseID, &closestDistance](
                    const armarx::armem::wm::EntityInstanceBase<armarx::human::arondto::HumanPose>&
                        instance)
                {
                    armarx::armem::human::HumanPose pose;
                    fromAron(instance.data(), pose);
 
                    // Extract head position from pose keypoints
                    std::optional<armarx::FramedPosition> headPos_opt = getHeadPos(pose);
                    if (headPos_opt.has_value())
                    {
                        const armarx::FramedPosition& headPos = headPos_opt.value();
                        float distance = getDistance(facePos, headPos);
 
                        ARMARX_VERBOSE << pose.humanTrackingId.value_or("") << ": "
                                       << VAROUT(distance);
 
                        // Keep track of the closest pose
                        if (distance < closestDistance)
                        {
                            closestPoseID =
                                HumanPoseWithID{.humanPose = pose, .memoryId = instance.id()};
                            closestDistance = distance;
                        }
                    }
                });
        }
        return closestPoseID;
    }
 
    std::optional<armarx::armem::MemoryID>
    UpdateConsumer::getClosestFaceID(const armarx::FramedPosition& headPos)
    {
        armarx::armem::client::QueryResult queryResult =
            faceRecognitionReader.getLatestSnapshotsIn(armarx::human::FaceRecognitionCoreSegmentID);
 
        std::optional<armarx::armem::MemoryID> closestFaceID = std::nullopt;
        float closestDistance = properties_.maxFaceHeadDistance;
 
        if (queryResult.success)
        {
            queryResult.memory.forEachInstanceWithDataAs(
                [&headPos, &closestFaceID, &closestDistance](
                    const armarx::armem::wm::EntityInstanceBase<
                        armarx::human::arondto::FaceRecognition>& instance)
                {
                    armarx::armem::human::FaceRecognition faceRecognition;
                    fromAron(instance.data(), faceRecognition);
 
                    const float distance = getDistance(faceRecognition.position3DGlobal, headPos);
                    ARMARX_VERBOSE << VAROUT(distance);
 
                    ARMARX_VERBOSE << "Face: " << faceRecognition.position3DGlobal.transpose();
                    ARMARX_VERBOSE << "Head: " << headPos.toEigen().transpose();
 
                    if (distance < closestDistance)
                    {
                        ARMARX_VERBOSE << "Found matching instance: " << instance.id();
                        closestFaceID = instance.id();
                        closestDistance = distance;
                    }
                });
        }
        return closestFaceID;
    }
 
    void
    UpdateConsumer::addPoseToInstance(const armarx::armem::MemoryID& memId,
                                      const armarx::armem::MemoryID& poseId,
                                      armarx::armem::human::PersonInstance currentInstance)
    {
        currentInstance.poseID = poseId;
        armarx::human::arondto::PersonInstance dto;
        toAron(dto, currentInstance);
        armarx::armem::EntityUpdate update{
            .entityID = memId,
            .instancesData = {dto.toAron()},
            .referencedTime = armarx::armem::Time::Now(),
        };
 
        ARMARX_DEBUG << "Adding pose to instance";
        personInstanceWriter.commit(update);
    }
 
    std::optional<armarx::armem::human::FaceRecognition>
    UpdateConsumer::faceRecognitionFromMemId(const armarx::armem::MemoryID& memId)
    {
        ARMARX_DEBUG << "Searching for face recognition";
        armarx::armem::client::QueryBuilder qb;
        qb.coreSegments()
            .withID(memId)
            .providerSegments()
            .withID(memId)
            .entities()
            .all()
            .snapshots()
            .latest();
 
        auto result = faceRecognitionReader.query(qb);
        if (not result.success)
        {
            ARMARX_WARNING << deactivateSpam() << "Could not query recognition.";
            return std::nullopt;
        }
        std::optional<armarx::armem::human::FaceRecognition> resFace = std::nullopt;
        result.memory.forEachInstanceWithDataAs(
            [&resFace](const armarx::armem::wm::EntityInstanceBase<
                       armarx::human::arondto::FaceRecognition>& instance)
            {
                armarx::armem::human::FaceRecognition res;
                armarx::armem::human::fromAron(instance.data(), res);
                resFace = res;
                ARMARX_DEBUG << "Found recognition instance";
            });
        if (not resFace.has_value())
        {
            ARMARX_WARNING << deactivateSpam() << "Recognition result empty";
        }
        return resFace;
    }
 
    std::optional<armarx::armem::human::HumanPose>
    UpdateConsumer::humanPoseFromMemId(const armarx::armem::MemoryID& memId)
    {
        armarx::armem::client::QueryBuilder qb;
        qb.coreSegments()
            .withID(memId)
            .providerSegments()
            .withID(memId)
            .entities()
            .all()
            .snapshots()
            .latest();
 
        auto result = poseReader.query(qb);
        if (not result.success)
        {
            ARMARX_WARNING << deactivateSpam() << "Could not query poses.";
            return std::nullopt;
        }
        std::optional<armarx::armem::human::HumanPose> resPose;
        result.memory.forEachInstanceWithDataAs(
            [&resPose](
                const armarx::armem::wm::EntityInstanceBase<armarx::human::arondto::HumanPose>&
                    instance)
            {
                armarx::armem::human::HumanPose res;
                armarx::armem::human::fromAron(instance.data(), res);
                resPose = res;
                ARMARX_DEBUG << "Found profile instance";
                //armarx::armem::human::fromAron(instance.dataAs<armarx::human::arondto::HumanPose>(), resPose.value());
            });
        if (not resPose.has_value())
        {
            ARMARX_VERBOSE << deactivateSpam() << "Pose result empty";
        }
        return resPose;
    }
 
    std::optional<armarx::FramedPosition>
    UpdateConsumer::getHeadPos(armarx::armem::human::HumanPose pose)
    {
 
        if (pose.poseModelId == armarx::human::pose::model::openpose_body_25::ModelId)
        {
            //TODO: implement pose type
        }
        else if (pose.poseModelId == armarx::human::pose::model::k4a_bt_body_32::ModelId)
        {
            const auto headKp = armarx::human::pose::model::k4a_bt_body_32::JointNames.to_name(
                armarx::human::pose::model::k4a_bt_body_32::Joints::Head);
            if (pose.keypoints.find(headKp) == pose.keypoints.end())
            {
                ARMARX_WARNING << "Pose does not contain head keypoint " << QUOTED(headKp);
                return std::nullopt;
            }
 
            return pose.keypoints.at(headKp).positionGlobal;
        }
        else if (pose.poseModelId == armarx::human::pose::model::mmm_body_68::ModelId)
        {
            //TODO: implement pose type
        }
        else
        {
            ARMARX_WARNING << "Unknown pose model ID '" << pose.poseModelId;
        }
 
        return std::nullopt;
    }
 
    float
    UpdateConsumer::getDistance(const Eigen::Vector3f& facePos,
                                const armarx::FramedPosition& headPos)
    {
        ARMARX_CHECK_EQUAL(headPos.frame, armarx::GlobalFrame);
 
        const Eigen::Vector3f headPosition = headPos.toEigen();
 
        return (headPosition - facePos).norm();
    }
 
    void
    UpdateConsumer::ensureTrackingIdUniqueness(const std::string& trackingId,
                                               const armarx::armem::MemoryID& exceptProfileID)
    {
        // Query all PersonInstances
        armarx::armem::client::QueryResult queryResult =
            personInstanceReader.getLatestSnapshotsIn(armarx::human::PersonInstanceCoreSegmentID);
 
        if (not queryResult.success)
        {
            return;
        }
 
        // Check each PersonInstance
        queryResult.memory.forEachInstanceWithDataAs(
            [this, &trackingId, &exceptProfileID](
                const armarx::armem::wm::EntityInstanceBase<armarx::human::arondto::PersonInstance>&
                    instance)
            {
                armarx::armem::MemoryID thisProfileID;
                fromAron(instance.data().profileID, thisProfileID);
 
                // Skip the PersonInstance that should keep this tracking ID
                if (thisProfileID == exceptProfileID)
                {
                    return;
                }
 
                // Check if this PersonInstance has a pose with the tracking ID
                armarx::human::arondto::PersonInstance personInstance = instance.data();
                armarx::armem::MemoryID humanPoseId;
                fromAron(personInstance.poseID, humanPoseId);
                const auto pose = humanPoseFromMemId(humanPoseId);
 
                if (pose.has_value() && pose->humanTrackingId.has_value() &&
                    pose->humanTrackingId.value() == trackingId)
                {
                    // Clear the pose link from this PersonInstance (only if not already empty)
                    armarx::armem::MemoryID currentPoseID;
                    fromAron(personInstance.poseID, currentPoseID);
 
                    // Only commit if the pose was actually set (avoid redundant commits)
                    if (currentPoseID.hasMemoryName() || currentPoseID.hasCoreSegmentName())
                    {
                        ARMARX_INFO << deactivateSpam(10) << "Clearing tracking ID "
                                    << QUOTED(trackingId) << " from PersonInstance "
                                    << QUOTED(personInstance.profileID.entityName)
                                    << " because it was reassigned.";
 
                        toAron(personInstance.poseID, armarx::armem::MemoryID()); // clear
 
                        armarx::armem::EntityUpdate update;
                        update.entityID = instance.id();
                        update.referencedTime = armarx::armem::Time::Now();
                        update.instancesData = {personInstance.toAron()};
                        personInstanceWriter.commit(update);
                    }
                }
            });
    }
 
    void
    UpdateConsumer::logFaceRecognitionUpdateDuration(const armarx::armem::Duration& duration)
    {
        ARMARX_DEBUG << "consumeFaceRecognitionUpdate took " << duration.toMilliSeconds() << " ms.";
 
        // Resolve human poses and check for human tracking id
        armarx::armem::human::client::PersonInstanceReader::QueryResolved query;
        query.providerName = "";
        query.timestamp = armarx::Clock::Now();
        query.maxAge = armarx::Duration::Seconds(10);
        query.resolveFaceDetection = false;
        query.resolveHumanPose = true;
        query.resolveProfile = true;
 
        ARMARX_VERBOSE << "Query";
        const auto result = personInstanceReaderV2.queryResolved(query);
 
        ARMARX_DEBUG << "Humans with tracking ids:";
        for (const auto& personInstance : result.personInstances)
        {
            std::string name = "~unknown~";
            if (personInstance.profile.has_value())
            {
                name = personInstance.profile->id.firstName + " " +
                       personInstance.profile->id.lastName;
            }
 
            std::string trackingId = "~none~";
            if (personInstance.humanPose.has_value() and
                personInstance.humanPose->humanTrackingId.has_value())
            {
                trackingId = personInstance.humanPose->humanTrackingId.value();
            }
 
            ARMARX_DEBUG << trackingId << ": " << name;
        }
    }
 
    std::optional<UpdateConsumer::PersonInstanceWithID>
    UpdateConsumer::findMatchingPersonInstance(
        const armarx::armem::client::QueryResult& queryResult,
        const armarx::armem::MemoryID& profileID)
    {
        std::optional<PersonInstanceWithID> result;
 
        queryResult.memory.forEachInstanceWithDataAs(
            [this, &profileID, &result](
                const armarx::armem::wm::EntityInstanceBase<armarx::human::arondto::PersonInstance>&
                    instance)
            {
                armarx::human::arondto::PersonInstance personInstance = instance.data();
                // Match by profile ID (the person's identity)
                if (profileID.entityName == personInstance.profileID.entityName)
                {
                    // Found matching PersonInstance - store it for later processing
                    // (we'll update face data and validate/update pose links below)
                    ARMARX_DEBUG << "Found matching personInstance "
                                 << QUOTED(instance.id().entityName) << ".";
 
                    armarx::armem::human::PersonInstance bo;
                    fromAron(personInstance, bo);
                    result = PersonInstanceWithID{.personInstance = personInstance,
                                                  .memoryId = instance.id()};
                }
            });
 
        return result;
    }
 
    void
    UpdateConsumer::clearPoseIdFromPersonInstance(const PersonInstanceWithID& personInstanceMatched)
    {
        ARMARX_INFO << deactivateSpam(10) << "Clearing pose ID of person instance "
                    << personInstanceMatched.personInstance.profileID.entityName;
        auto personInstanceRevised = personInstanceMatched.personInstance;
        toAron(personInstanceRevised.poseID, armarx::armem::MemoryID()); // clear
 
        armarx::armem::EntityUpdate update;
        update.entityID = personInstanceMatched.memoryId;
        update.referencedTime = armarx::armem::Time::Now();
        update.instancesData = {personInstanceRevised.toAron()};
        personInstanceWriter.commit(update);
    }
 
    void
    UpdateConsumer::setPoseIdForPersonInstance(const PersonInstanceWithID& personInstanceMatched,
                                               const armarx::armem::MemoryID& poseId)
    {
        ARMARX_INFO << deactivateSpam(10) << "Setting pose ID of person instance "
                    << QUOTED(personInstanceMatched.personInstance.profileID.entityName) << " to "
                    << QUOTED(poseId);
        auto personInstanceRevised = personInstanceMatched.personInstance;
        toAron(personInstanceRevised.poseID, poseId);
 
        armarx::armem::EntityUpdate update;
        update.entityID = personInstanceMatched.memoryId;
        update.referencedTime = armarx::armem::Time::Now();
        update.instancesData = {personInstanceRevised.toAron()};
        personInstanceWriter.commit(update);
    }
 
    bool
    UpdateConsumer::isMemoryIdFullySpecified(const armarx::armem::MemoryID& memoryId)
    {
        return memoryId.hasMemoryName() and memoryId.hasCoreSegmentName() and
               memoryId.hasProviderSegmentName() and memoryId.hasEntityName() and
               memoryId.hasTimestamp() and memoryId.hasInstanceIndex();
    }
 
    bool
    UpdateConsumer::isBusy()
    {
        if (consumeMtx.try_lock())
        {
            consumeMtx.unlock(); // We only wanted to check
            return false; // Not busy
        }
        return true; // Busy
    }
} // namespace VisionX::components::person_instance_updater