UpdateConsumer.cpp

Go to the documentation of this file.

#include "UpdateConsumer.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/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.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::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 << "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());
                }
                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 + ".";
            }
        }
    }
 
    void
    UpdateConsumer::consumePoseUpdate(armarx::armem::human::HumanPose humanPose,
                                      armarx::armem::MemoryID poseID,
                                      armarx::armem::client::MemoryNameSystem mns)
    {
        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);
        if (queryResult.success)
        {
            bool poseKnown = false;
            queryResult.memory.forEachInstance(
                [&poseKnown, &poseID](const armarx::armem::wm::EntityInstance& instance)
                {
                    armarx::human::arondto::PersonInstance personInstance =
                        instance.dataAs<armarx::human::arondto::PersonInstance>();
                    armarx::armem::MemoryID currentPoseID;
                    fromAron(personInstance.poseID, currentPoseID);
                    poseKnown = poseKnown or (currentPoseID == poseID);
                });
 
            if (poseKnown)
            {
                //TODO: check for plausibility of personInstance, remove in case plausibility is no longer ensured, restart consume process afterwards to generate new personInstance
                return;
            }
            std::optional<armarx::FramedPosition> headPos = getHeadPos(humanPose);
            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());
                    //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(faceRecognition.profileID.value().entityName);
                    update.referencedTime = armarx::armem::Time::Now();
                    update.instancesData = {personInstance.toAron()};
                    personInstanceWriter.commit(update);
                    ARMARX_INFO << "Created new personInstance for human pose " +
                                       poseID.entityName + ".";
                }
            }
        }
    }
 
    void
    UpdateConsumer::consumeProfileUpdate(armarx::human::arondto::Profile profile,
                                         armarx::armem::MemoryID profileID,
                                         armarx::armem::client::MemoryNameSystem mns)
    {
        //TODO: implement profile update logic
    }
 
    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);
 
        armarx::armem::MemoryID closestFaceID;
        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;
    }
 
    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