UpdateConsumer.h

Go to the documentation of this file.

#pragma once
 
#include <ArmarXCore/core/logging/Logging.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/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/core/FramedPose.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/Profile.aron.generated.h>
#include <VisionX/libraries/armem_human/types.h>
 
namespace VisionX::components::person_instance_updater
{
    class UpdateConsumer
    {
    public:
        UpdateConsumer();
        UpdateConsumer(int maxFaceHeadDistance);
        void consumeFaceRecognitionUpdate(armarx::armem::human::FaceRecognition faceRecognition,
                                          armarx::armem::MemoryID faceRecognitionID,
                                          armarx::armem::client::MemoryNameSystem mns);
        void consumePoseUpdate(armarx::armem::human::HumanPose humanPose,
                               armarx::armem::MemoryID poseID,
                               armarx::armem::client::MemoryNameSystem mns);
        void consumeProfileUpdate(armarx::human::arondto::Profile profile,
                                  armarx::armem::MemoryID profileID,
                                  armarx::armem::client::MemoryNameSystem mns);
 
    private:
        /**
         * @brief Checks, whether a pose and face recognition instance can be plausibly matched.
         * 
         * @return true when plausible.
         * @return false when not plausible.
         */
        bool checkForPlausability(::armarx::armem::human::HumanPose& pose,
                                  ::armarx::armem::human::FaceRecognition& face);
 
        static void
        removePoseFromPersonInstance(const armarx::armem::MemoryID& personInstanceId,
                                     armarx::armem::human::PersonInstance oldData,
                                     armarx::armem::client::Writer& personInstanceWriter);
 
        std::optional<armarx::armem::MemoryID>
        getClosestPoseID(Eigen::Matrix<float, 3, 1> facePos,
                         armarx::armem::client::MemoryNameSystem mns);
 
        std::optional<armarx::armem::MemoryID>
        getClosestFaceID(armarx::FramedPosition headPos,
                         armarx::armem::client::MemoryNameSystem mns);
 
        static std::optional<armarx::armem::human::HumanPose>
        humanPoseFromMemId(armarx::armem::MemoryID& memId,
                           armarx::armem::client::Reader& poseReader);
 
        static std::optional<armarx::armem::human::FaceRecognition>
        faceRecognitionFromMemId(armarx::armem::MemoryID& memId,
                                 armarx::armem::client::Reader& recognitionReader);
 
        static void
        updateInstanceWithNewPoseId(const armarx::armem::MemoryID& instanceId,
                                    armarx::armem::human::PersonInstance oldInstance,
                                    const armarx::armem::MemoryID& newPoseId,
                                    const armarx::armem::client::Writer& instanceWriter);
 
        void createNewInstanceBasedOnPose(const armarx::armem::MemoryID& poseId,
                                          const armarx::armem::human::HumanPose& pose,
                                          armarx::armem::client::MemoryNameSystem& mns);
 
        static void addPoseToInstance(const armarx::armem::MemoryID& memId,
                                      const armarx::armem::MemoryID& poseId,
                                      const armarx::armem::client::Writer& personInstanceWriter,
                                      armarx::armem::human::PersonInstance currentInstance);
 
        static std::optional<armarx::FramedPosition>
        getHeadPos(armarx::armem::human::HumanPose pose);
 
        static float getDistance(Eigen::Matrix<float, 3, 1> facePos,
                                 armarx::FramedPosition headPos);
        float maxFaceHeadDistance;
    };
} // namespace VisionX::components::person_instance_updater