PersonSimulator.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    VisionX::COMPONENT_NAME_SNAKE::core
 * @author     Peter Albrecht ( usnlf at student dot kit dot edu)
 * @date       2024
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PersonSimulator.h"
 
#include <math.h>
 
#include <optional>
#include <sstream>
#include <utility>
 
#include <Eigen/Geometry>
#include <Eigen/src/Core/Matrix.h>
 
#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <SimoxUtility/math/convert/rad_to_deg.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include "RobotAPI/libraries/armem/client/Reader.h"
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include "RobotAPI/libraries/core/FramedPose.h"
 
#include "VisionX/libraries/human/pose/model/k4a_bt_body_32.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/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/armem_human/types.h>
 
namespace VisionX::person_simulator::core
{
    PersonSimulator::PersonSimulator(const Remote& r, const Properties& p) :
        remote(r), properties(p)
    {
        // these are rough approximations
        generatedPoseParams = {
            .hipHeight = 1000.F,
            .pelvisHeight = 1000.F,
            .shoulderHeight = 1600.F,
            .headHeight = 1700.F,
            .earHeight = 1800.F,
        };
    }
 
    void
    PersonSimulator::createHumanProfile(const std::string& name)
    {
        armarx::armem::client::Writer poseWriter =
            remote.mns.getWriter(armarx::human::ProfileCoreSegmentID);
 
        // we only set the firstName. FaceImages are not required,
        // as the user manually sets the profile in the face recognition.
        // therefore, this will not work with a real implementation of face recognition
        // TODO: implement option to add face images
        armarx::human::arondto::Profile dto;
        dto.firstName = name;
 
        armarx::armem::MemoryID entityId =
            armarx::human::ProfileCoreSegmentID
                .withProviderSegmentName(properties.profileProviderName)
                .withEntityName(name);
 
        armarx::armem::EntityUpdate update = {
            .entityID = entityId,
            .instancesData = {dto.toAron()},
            .referencedTime = armarx::armem::Time::Now(),
            .confidence = 1.0F,
            .sentTime = armarx::armem::Time::Now(),
            .arrivedTime = armarx::armem::Time::Now(),
        };
 
        ARMARX_INFO << "Committing profile with name: " << name;
        poseWriter.commit(update);
    }
 
    std::optional<armarx::armem::MemoryID>
    PersonSimulator::findProfileId(std::string& entityId)
    {
        armarx::armem::client::Reader profileReader =
            remote.mns.getReader(armarx::human::ProfileCoreSegmentID);
 
        armarx::armem::client::QueryResult queryResult =
            profileReader.getLatestSnapshotsIn(armarx::human::ProfileCoreSegmentID);
        std::optional<armarx::armem::MemoryID> foundProfileId = std::nullopt;
 
        // memory fetch failed
        if (not queryResult.success)
        {
            return std::nullopt;
        }
 
        // find the first matching profile
        queryResult.memory.forEachInstance(
            [&entityId, &foundProfileId](armarx::armem::wm::EntityInstance& instance)
            {
                if (foundProfileId.has_value())
                {
                    // we want the first match, and ignore all further matches here.
                    return;
                }
 
                if (instance.id().entityName == entityId)
                {
                    // match found
                    foundProfileId = instance.id();
                }
            });
 
        return foundProfileId;
    }
 
    std::string
    PersonSimulator::increaseAndConvert(unsigned int& counter)
    {
        std::stringstream ss;
        ss << ++counter;
        return ss.str();
    }
 
    void
    PersonSimulator::createFaceRecognition(const Eigen::Vector3f& facePosition,
                                           std::optional<std::string> entityName)
    {
        ARMARX_INFO << "Creating face recognition at cartesian position: " << facePosition.x()
                    << ", " << facePosition.y() << ", " << facePosition.z();
        armarx::armem::client::Writer poseWriter =
            remote.mns.getWriter(armarx::human::FaceRecognitionCoreSegmentID);
 
 
        std::optional<armarx::armem::MemoryID> profileId = std::nullopt;
 
        if (entityName.has_value())
        {
            profileId = findProfileId(entityName.value());
        }
 
        armarx::armem::human::FaceRecognition bo;
        bo.framedPosition3D = armarx::FramedPosition(facePosition, "global", properties.robotName);
        // doesnt make sense, but this isn't set in prod anyways
        bo.position2D = Eigen::Vector2i::Identity();
        bo.position3DGlobal = facePosition;
        // profile id may be nullopt, but this case is communicated to the user
        bo.profileID = profileId;
 
        armarx::human::arondto::FaceRecognition dto;
        armarx::armem::human::toAron(dto, bo);
 
        armarx::armem::MemoryID entityId =
            armarx::human::PoseCoreSegmentID.withProviderSegmentName(properties.poseProviderName)
                .withEntityName(entityName.value_or(increaseAndConvert(faceRecognitionIdCounter)));
 
        armarx::armem::EntityUpdate update = {
            .entityID = entityId,
            .instancesData = {dto.toAron()},
            .referencedTime = armarx::armem::Time::Now(),
            .confidence = 1.0F,
            .sentTime = armarx::armem::Time::Now(),
            .arrivedTime = armarx::armem::Time::Now(),
        };
 
        // hack
        ARMARX_INFO
            << "Committing face recognition for profile: "
            << profileId.value_or(armarx::armem::MemoryID().withEntityName("INVALID")).entityName;
        poseWriter.commit(update);
    }
 
    armarx::armem::human::HumanPose
    PersonSimulator::createHumanPose(const std::string& humanId,
                                     Eigen::Vector2f meanFloorPosition,
                                     float clockwiseOrientationDegrees)
    {
        ARMARX_INFO << "Creating human pose at cartesian position: " << meanFloorPosition.x()
                    << ", " << meanFloorPosition.y();
        armarx::armem::client::Writer poseWriter =
            remote.mns.getWriter(armarx::human::PoseCoreSegmentID);
 
        armarx::armem::human::HumanPose pose;
        pose.humanTrackingId = humanId;
        pose.poseModelId = armarx::human::pose::model::k4a_bt_body_32::ModelId;
        pose.cameraFrameName = "global";
        pose.timestamp = armarx::DateTime::Now();
        armarx::FramedOrientation defaultOrientation(
            Eigen::Quaternionf::UnitRandom(), "global", "Armar7");
 
        using namespace armarx::human::pose::model::k4a_bt_body_32;
 
        pose.keypoints =
            constructDefaultKeypoints(std::move(meanFloorPosition), clockwiseOrientationDegrees);
 
        armarx::armem::MemoryID entityId =
            armarx::human::PoseCoreSegmentID.withProviderSegmentName(properties.poseProviderName)
                .withEntityName(humanId);
 
        armarx::human::arondto::HumanPose dto;
        armarx::armem::human::toAron(dto, pose);
 
        armarx::armem::EntityUpdate update = {
            .entityID = entityId,
            .instancesData = {dto.toAron()},
            .referencedTime = armarx::armem::Time::Now(),
            .confidence = 1.0F,
            .sentTime = armarx::armem::Time::Now(),
            .arrivedTime = armarx::armem::Time::Now(),
        };
 
        ARMARX_INFO << "Committing pose with HumanID '" << humanId << "'.";
        poseWriter.commit(update);
        return pose;
    }
 
    armarx::armem::human::HumanPose
    PersonSimulator::createHumanPoseFacingRobot(const std::string& humanId,
                                                Eigen::Vector2f meanFloorPosition)
    {
        // calculate the needed orientation, then call `createHumanPose`
 
        // get robot position, calculate difference
        const auto robotPose =
            remote.robotReader.queryGlobalPose(properties.robotName, armarx::armem::Time::Now());
        const Eigen::Vector3f robotPosition = robotPose->translation();
        ARMARX_DEBUG << "Robot Position: " << robotPosition;
        Eigen::Vector2f robot2d{robotPosition.x(), robotPosition.y()};
        Eigen::Vector2f diff = robot2d - meanFloorPosition;
        diff.normalize();
        Eigen::Vector2f forwards = Eigen::Vector2f::UnitY();
 
        // see: Definition 18.23 in the lecture notes of "Lineare Algebra" by Enrico Leuzinger
        const float dot = diff.dot(forwards);
        const float cos_angle = dot;
        float angle = acosf(cos_angle);
        if (angle <= (M_PI / 2.F))
        {
            angle = -1.F * angle;
        }
        angle = simox::math::rad_to_deg(angle);
 
        ARMARX_DEBUG << "Calculated angle: " << angle;
 
        auto pose = createHumanPose(humanId, meanFloorPosition, angle);
        return pose;
    }
 
    std::map<std::string, armarx::armem::human::PoseKeypoint>
    PersonSimulator::constructDefaultKeypoints(Eigen::Vector2f meanFloorPosGlobal,
                                               float clockwiseOrientationDegrees)
    {
        using namespace armarx::human::pose::model::k4a_bt_body_32;
        // General idea: we only explicitly set some keypoints to "plausible" values, as to enable orientation detection
        // -> set all keypoints to the mean floor projection, then manually set keypoints to rotated values
        // reference: y is forwards, x is left, z is up
 
        /// helper to rotate a vector by the vertical axis
        auto rotateGlobalVector =
            [&clockwiseOrientationDegrees, &meanFloorPosGlobal](Eigen::Vector3f globalPosition)
        {
            // helper
            const Eigen::Vector3f projectedMean(
                meanFloorPosGlobal.x(), meanFloorPosGlobal.y(), 0.F);
            // create angleaxis from floor projection
            Eigen::Vector3f verticalAxis = Eigen::Vector3f::UnitZ();
            Eigen::AngleAxisf verticalAxisRotation = Eigen::AngleAxisf(
                -1.F * simox::math::deg_to_rad(clockwiseOrientationDegrees), verticalAxis);
 
            // make position local, rotate and translate back
            const Eigen::Vector3f localPosition = globalPosition - projectedMean;
 
            const Eigen::Vector3f rotatedLocalPos =
                verticalAxisRotation.toRotationMatrix() * localPosition;
 
            const Eigen::Vector3f rotatedGlobalPos = rotatedLocalPos + projectedMean;
            return rotatedGlobalPos;
        };
 
        armarx::armem::human::HumanPose::KeyPointMap keypoints = {};
 
        /// note: label denotes the real label without the "Left" or "Right" suffix of the k4a model
        auto setSymmetricJointsAtHeight = [&](std::string label, float height)
        {
            // i shall pray for this to work
            const std::string left_sf = "Left";
            const std::string right_sf = "Right";
 
            Eigen::Vector3f leftPos =
                Eigen::Vector3f(meanFloorPosGlobal.x() - properties.jointDisplacement,
                                meanFloorPosGlobal.y(),
                                height);
            Eigen::Vector3f rightPos =
                Eigen::Vector3f(meanFloorPosGlobal.x() + properties.jointDisplacement,
                                meanFloorPosGlobal.y(),
                                height);
            leftPos = rotateGlobalVector(leftPos);
            rightPos = rotateGlobalVector(rightPos);
            keypoints[label + left_sf] = constructKeyPoint(leftPos, label + left_sf);
            keypoints[label + right_sf] = constructKeyPoint(rightPos, label + right_sf);
        };
 
        // assumption: position does not have to be rotated
        auto setSingleJointAtHeight = [&](std::string label, float height)
        {
            Eigen::Vector3f pos =
                Eigen::Vector3f(meanFloorPosGlobal.x(), meanFloorPosGlobal.y(), height);
            keypoints[label] = constructKeyPoint(pos, label);
        };
 
        // create keypointmap, initialize all to default
        for (const auto& joint : JointNames.names())
        {
            keypoints[joint] = constructKeyPoint(
                Eigen::Vector3f(meanFloorPosGlobal.x(), meanFloorPosGlobal.y(), 0.F), joint);
        }
 
        // note: these heights are somewhat arbitrary
 
        // set shoulders
        setSymmetricJointsAtHeight("Shoulder", generatedPoseParams.shoulderHeight);
 
        // set hips
        setSymmetricJointsAtHeight("Hip", generatedPoseParams.hipHeight);
 
        // set ears
        setSymmetricJointsAtHeight("Ear", generatedPoseParams.earHeight);
 
        // pelvis and head (needed for positioning/face recognition matching)
        setSingleJointAtHeight("Pelvis", generatedPoseParams.pelvisHeight);
        setSingleJointAtHeight("Head", generatedPoseParams.headHeight);
 
        // to visualize orientation
        keypoints["WristRight"] = constructKeyPoint(
            rotateGlobalVector({meanFloorPosGlobal.x() + properties.jointDisplacement,
                                meanFloorPosGlobal.y() + 100,
                                generatedPoseParams.hipHeight}),
            "WristRight");
 
 
        return keypoints;
    }
 
    armarx::armem::human::PoseKeypoint
    PersonSimulator::constructKeyPoint(Eigen::Vector3f globalPos, const std::string& label)
    {
        // helper lambdas to avoid typing
        auto makeDefaultOrientation = [&](std::string frameName)
        {
            return armarx::FramedOrientation(
                Eigen::Quaternionf::Identity(), frameName, properties.robotName);
        };
        auto makeDefaultPosition = [&](std::string frameName)
        {
            Eigen::Vector3f ident = Eigen::Vector3f::Identity();
            return armarx::FramedPosition(ident, frameName, properties.robotName);
        };
 
 
        armarx::armem::human::PoseKeypoint key = {
            .label = label,
            .confidence = 1.0F,
            .positionCamera = makeDefaultPosition("camera"),
            .orientationCamera = makeDefaultOrientation("camera"),
            .positionRobot = makeDefaultPosition("robot"),
            .orientationRobot = makeDefaultOrientation("robot"),
            .positionGlobal = armarx::FramedPosition(globalPos, "global", properties.robotName),
            .orientationGlobal = makeDefaultOrientation("global"),
 
        };
        return key;
    }
 
} // namespace VisionX::person_simulator::core