UpdateConsumer.cpp

Go to the documentation of this file.

#include "UpdateConsumer.h"
 
#include <optional>
#include <sstream>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include "RobotAPI/libraries/armem/client/Query.h"
#include "RobotAPI/libraries/armem/core/Commit.h"
#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#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/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
 
#include "VisionX/libraries/armem_human/types.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/PersonInstance.aron.generated.h>
#include <VisionX/libraries/armem_human/aron/Profile.aron.generated.h>
#include <VisionX/libraries/armem_human/aron_conversions.h>
#include <VisionX/libraries/armem_human/memory_ids.h>
#include <VisionX/libraries/armem_human/server/PoseSegment.h>
#include <VisionX/libraries/armem_human/server/face_recognition/Segment.h>
#include <VisionX/libraries/armem_human/server/profile/Segment.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() : maxFaceHeadDistance(1000.0)
    {
    }
 
    UpdateConsumer::UpdateConsumer(int maxFaceHeadDistance) :
        maxFaceHeadDistance(maxFaceHeadDistance)
    {
    }
 
    //TODO: add some form of history to allow for better understanding of memory changed of the personInstances
    //TODO: add visualization of
    void
    UpdateConsumer::consumeFaceRecognitionUpdate(
        armarx::armem::human::FaceRecognition faceRecognition,
        armarx::armem::MemoryID faceRecognitionID,
        armarx::armem::client::MemoryNameSystem mns)
    {
        ARMARX_DEBUG << "Consuming face recognition update...";
        armarx::armem::client::Writer personInstanceWriter =
            mns.getWriter(armarx::human::PersonInstanceCoreSegmentID);
        armarx::armem::client::Reader personInstanceReader =
            mns.getReader(armarx::human::PersonInstanceCoreSegmentID);
        armarx::armem::client::QueryResult queryResult =
            personInstanceReader.getLatestSnapshotsIn(armarx::human::PersonInstanceCoreSegmentID);
        armarx::armem::MemoryID profileID;
 
        if (faceRecognition.profileID.has_value())
        {
            profileID = faceRecognition.profileID.value();
        }
        else
        {
            ARMARX_INFO << deactivateSpam()
                        << "Face recognition " + faceRecognitionID.entityName +
                               " does not provide valid profileID, finding matching "
                               "personInstance is not possible.";
            return;
        }
 
        if (queryResult.success)
        {
            //TODO: add early return in case face recognition id is already used by a personInstance
            bool foundMatchingEntity = false;
 
            queryResult.memory.forEachInstance(
                [&profileID, &personInstanceWriter, &faceRecognitionID, &foundMatchingEntity](
                    const armarx::armem::wm::EntityInstance& instance)
                {
                    armarx::human::arondto::PersonInstance personInstance =
                        instance.dataAs<armarx::human::arondto::PersonInstance>();
                    if (profileID.entityName == personInstance.profileID.entityName)
                    {
                        //update matching personInstance
                        toAron(personInstance.faceRecognitionID, faceRecognitionID);
                        armarx::armem::EntityUpdate update;
                        update.entityID = instance.id();
                        update.referencedTime = armarx::armem::Time::Now();
                        update.instancesData = {personInstance.toAron()};
                        personInstanceWriter.commit(update);
                        ARMARX_INFO << "Updated face recognition on personInstance '" +
                                           instance.id().entityName + "'.";
                        //TODO: check for plausibility of personInstance, remove in case plausibility is no longer ensured, restart consume process afterwards to generate new personInstance
                        foundMatchingEntity = true;
                        return;
                    }
                });
            if (foundMatchingEntity)
            {
                return;
            }
            //create new personInstance in case no matching personInstance could be found
            {
                armarx::human::arondto::PersonInstance personInstance;
                toAron(personInstance.faceRecognitionID, faceRecognitionID);
                toAron(personInstance.profileID, profileID);
                std::optional<armarx::armem::MemoryID> closestPoseID =
                    getClosestPoseID(faceRecognition.position3DGlobal, mns);
                if (closestPoseID.has_value())
                {
                    toAron(personInstance.poseID, closestPoseID.value());
                }
 
                if (faceRecognition.profileID.value().entityName == "")
                {
                    ARMARX_WARNING << "Face Recognition has no valid profile";
                    return;
                }
 
                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);
                ARMARX_INFO << "Created new personInstance for face recognition " +
                                   faceRecognitionID.entityName + ".";
            }
        }
    }
 
    /** 
      * Checks all person instances, whether the pose can be matched
      * Note: in all cases, if the pose-face can be plausably matched to the 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(armarx::armem::human::HumanPose humanPose,
                                      armarx::armem::MemoryID poseID,
                                      armarx::armem::client::MemoryNameSystem mns)
    {
        ARMARX_DEBUG << "Consuming pose update...";
        armarx::armem::client::Writer personInstanceWriter =
            mns.getWriter(armarx::human::PersonInstanceCoreSegmentID);
        armarx::armem::client::Reader personInstanceReader =
            mns.getReader(armarx::human::PersonInstanceCoreSegmentID);
        armarx::armem::client::Reader poseReader = mns.getReader(armarx::human::PoseCoreSegmentID);
        armarx::armem::client::Reader faceRecognitionReader =
            mns.getReader(armarx::human::FaceRecognitionCoreSegmentID);
        armarx::armem::client::QueryResult queryResult =
            personInstanceReader.getLatestSnapshotsIn(armarx::human::PersonInstanceCoreSegmentID);
 
        // iterate over all instances, and find the first one that matches
        // TODO: what to do, if multiple instances match?
 
        // Two cases: either the instance contains a pose with the same humanId,
        // or the face recognition position matches the face recognition position
        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;
            queryResult.memory.forEachInstance(
                [&](const armarx::armem::wm::EntityInstance& instance)
                {
                    ARMARX_DEBUG << "In iteration: " << instance.id().entityName;
                    // we want the first matching instance,
                    // therefore skip all iterations once a match is found
                    if (matchingInstance.has_value())
                    {
                        ARMARX_DEBUG << "Already got matching instance, returning";
                        return;
                    }
 
                    armarx::human::arondto::PersonInstance personInstance =
                        instance.dataAs<armarx::human::arondto::PersonInstance>();
                    armarx::armem::MemoryID currentPoseID;
                    fromAron(personInstance.poseID, currentPoseID);
                    auto pose = humanPoseFromMemId(currentPoseID, poseReader);
 
 
                    // prioritize matching face recognition, in case of the pose entity changing
                    armarx::armem::MemoryID currentRecognitionId;
                    fromAron(personInstance.faceRecognitionID, currentRecognitionId);
                    // else: check, if instance is plausible (by checking face-head distance)
                    auto recognition =
                        faceRecognitionFromMemId(currentRecognitionId, faceRecognitionReader);
                    if (recognition.has_value())
                    {
                        ARMARX_DEBUG << "checking for plausability";
                        if (checkForPlausability(humanPose, recognition.value()))
                        {
                            armarx::armem::human::PersonInstance bo;
                            fromAron(personInstance, bo);
                            matchingInstance = bo;
                            matchingInstanceId = instance.id();
                            ARMARX_DEBUG << "Instance is plausible";
                            return;
                        }
                    }
 
                    if (pose.has_value())
                    {
                        if (pose->humanTrackingId.has_value() or
                            humanPose.humanTrackingId.has_value())
                        {
                            ARMARX_WARNING
                                << "A pose does not define a humanId. Unable to match...";
                            return;
                        }
 
                        if (pose->humanTrackingId == humanPose.humanTrackingId)
                        {
                            ARMARX_DEBUG << "Found matching poses based on humanId";
                            armarx::armem::human::PersonInstance bo;
                            fromAron(personInstance, bo);
                            matchingInstance = bo;
                            matchingInstanceId = instance.id();
                        }
                        return;
                    }
                });
 
            // either a matching instance was found, in which case we update it,
            // OR: no match was found, and we create a new one
            if (matchingInstance.has_value())
            {
                ARMARX_DEBUG << "Found matching instance, handling it";
 
                // in any case, we simply add the current pose id to the person instance
                updateInstanceWithNewPoseId(matchingInstanceId.value(),
                                            matchingInstance.value(),
                                            poseID,
                                            personInstanceWriter);
            }
            else
            {
                ARMARX_DEBUG << "No matching instance found, creating new...";
                createNewInstanceBasedOnPose(poseID, humanPose, mns);
            }
        }
        else
        {
            ARMARX_WARNING << deactivateSpam() << "Fetching instance memory failed.";
        }
    }
 
    void
    UpdateConsumer::consumeProfileUpdate(armarx::human::arondto::Profile profile,
                                         armarx::armem::MemoryID profileID,
                                         armarx::armem::client::MemoryNameSystem mns)
    {
        //TODO: implement profile update logic
    }
 
    void
    UpdateConsumer::removePoseFromPersonInstance(
        const armarx::armem::MemoryID& personInstanceId,
        armarx::armem::human::PersonInstance data,
        armarx::armem::client::Writer& personInstanceWriter)
    {
        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);
    }
 
    void
    UpdateConsumer::createNewInstanceBasedOnPose(const armarx::armem::MemoryID& poseId,
                                                 const armarx::armem::human::HumanPose& pose,
                                                 armarx::armem::client::MemoryNameSystem& mns)
    {
        armarx::armem::client::Reader faceRecognitionReader =
            mns.getReader(armarx::human::FaceRecognitionCoreSegmentID);
        armarx::armem::client::Writer personInstanceWriter =
            mns.getWriter(armarx::human::PersonInstanceCoreSegmentID);
        std::optional<armarx::FramedPosition> headPos = getHeadPos(pose);
        if (headPos.has_value())
        {
            std::optional<armarx::armem::MemoryID> faceRecognitionID =
                getClosestFaceID(headPos.value(), mns);
            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(), faceRecognitionReader);
                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 << "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());
 
                //TODO: get profile id
                //toAron(personInstance.profileID, profileID);
                toAron(personInstance.poseID, poseId);
                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);
                ARMARX_INFO << "Created new personInstance for human pose " + poseId.entityName +
                                   ".";
            }
            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::client::Writer& instanceWriter)
    {
        ARMARX_DEBUG << "Start update with new pose id";
        if (oldInstance.poseID.getEntityID() == newPoseId.getEntityID())
        {
            // committing this "new" data will not change anything
            return;
        }
        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;
        instanceWriter.commit(update);
    }
 
    bool
    UpdateConsumer::checkForPlausability(::armarx::armem::human::HumanPose& pose,
                                         ::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 <= maxFaceHeadDistance;
    }
 
    std::optional<armarx::armem::MemoryID>
    UpdateConsumer::getClosestPoseID(Eigen::Matrix<float, 3, 1> facePos,
                                     armarx::armem::client::MemoryNameSystem mns)
    {
        armarx::armem::client::Reader poseReader = mns.getReader(armarx::human::PoseCoreSegmentID);
        armarx::armem::client::QueryResult queryResult =
            poseReader.getLatestSnapshotsIn(armarx::human::PoseCoreSegmentID);
 
        armarx::armem::MemoryID closestPoseID;
        float closestDistance = maxFaceHeadDistance;
 
        if (queryResult.success)
        {
            queryResult.memory.forEachInstance(
                [&facePos, &closestPoseID, &closestDistance](
                    const armarx::armem::wm::EntityInstance& instance)
                {
                    armarx::human::arondto::HumanPose dto =
                        instance.dataAs<armarx::human::arondto::HumanPose>();
                    armarx::armem::human::HumanPose pose;
                    fromAron(dto, pose);
                    std::optional<armarx::FramedPosition> headPos_opt = getHeadPos(pose);
                    if (headPos_opt.has_value())
                    {
                        armarx::FramedPosition headPos = headPos_opt.value();
                        float distance = getDistance(facePos, headPos);
                        if (distance < closestDistance)
                        {
                            closestPoseID = instance.id();
                            closestDistance = distance;
                        }
                    }
                });
        }
        return closestPoseID;
    }
 
    std::optional<armarx::armem::MemoryID>
    UpdateConsumer::getClosestFaceID(armarx::FramedPosition headPos,
                                     armarx::armem::client::MemoryNameSystem mns)
    {
        armarx::armem::client::Reader faceRecognitionReader =
            mns.getReader(armarx::human::FaceRecognitionCoreSegmentID);
        armarx::armem::client::QueryResult queryResult =
            faceRecognitionReader.getLatestSnapshotsIn(armarx::human::FaceRecognitionCoreSegmentID);
 
        std::optional<armarx::armem::MemoryID> closestFaceID = std::nullopt;
        float closestDistance = maxFaceHeadDistance;
 
        if (queryResult.success)
        {
            queryResult.memory.forEachInstance(
                [&headPos, &closestFaceID, &closestDistance](
                    const armarx::armem::wm::EntityInstance& instance)
                {
                    armarx::human::arondto::FaceRecognition dto =
                        instance.dataAs<armarx::human::arondto::FaceRecognition>();
                    armarx::armem::human::FaceRecognition faceRecognition;
                    fromAron(dto, faceRecognition);
 
                    float distance = getDistance(faceRecognition.position3DGlobal, headPos);
                    if (distance < closestDistance)
                    {
                        closestFaceID = instance.id();
                        closestDistance = distance;
                    }
                });
        }
        return closestFaceID;
    }
 
    void
    UpdateConsumer::addPoseToInstance(const armarx::armem::MemoryID& memId,
                                      const armarx::armem::MemoryID& poseId,
                                      const armarx::armem::client::Writer& personInstanceWriter,
                                      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(armarx::armem::MemoryID& memId,
                                             armarx::armem::client::Reader& recognitionReader)
    {
        ARMARX_DEBUG << "Searching for face recognition";
        armarx::armem::client::QueryBuilder qb;
        qb.coreSegments()
            .withID(memId)
            .providerSegments()
            .withID(memId)
            .entities()
            .all()
            .snapshots()
            .latest();
        auto result = recognitionReader.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.forEachInstance(
            [&resFace](const armarx::armem::wm::EntityInstance& instance)
            {
                auto recognition = instance.dataAs<armarx::human::arondto::FaceRecognition>();
                armarx::armem::human::FaceRecognition res;
                armarx::armem::human::fromAron(recognition, 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(armarx::armem::MemoryID& memId,
                                       armarx::armem::client::Reader& poseReader)
    {
        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.forEachInstance(
            [&resPose](const armarx::armem::wm::EntityInstance& instance)
            {
                auto pose = instance.dataAs<armarx::human::arondto::HumanPose>();
                armarx::armem::human::HumanPose res;
                armarx::armem::human::fromAron(pose, 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_WARNING << deactivateSpam() << "Pose result empty";
        }
        return resPose;
    }
 
    std::optional<armarx::FramedPosition>
    UpdateConsumer::getHeadPos(armarx::armem::human::HumanPose pose)
    {
        std::optional<armarx::FramedPosition> headPos;
        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)
        {
            headPos = pose.keypoints[armarx::human::pose::model::k4a_bt_body_32::JointNames.to_name(
                                         armarx::human::pose::model::k4a_bt_body_32::Joints::Head)]
                          .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 headPos;
    }
 
    float
    UpdateConsumer::getDistance(Eigen::Matrix<float, 3, 1> facePos, armarx::FramedPosition headPos)
    {
        return sqrt(facePos.x() * headPos.x + facePos.y() * headPos.y + facePos.z() * headPos.z);
    }
} // namespace VisionX::components::person_instance_updater