PersonInstance.cpp

Go to the documentation of this file.

#include "PersonInstance.h"
 
#include <string>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include "RobotAPI/components/ArViz/Client/Elements.h"
#include "RobotAPI/components/ArViz/Client/Layer.h"
#include "RobotAPI/components/ArViz/Client/elements/Color.h"
 
#include "VisionX/libraries/armem_human/types.h"
 
namespace armarx::viz
{
    void
    addPersonInstanceToLayer(const armarx::armem::human::FaceRecognition& faceReco,
                             const std::string& name,
                             armarx::viz::Layer& personInstanceLayer,
                             const std::string& prefix)
    {
        ARMARX_TRACE;
        static const int radius = 100;
        armarx::viz::Color color = armarx::viz::Color::fromRGBA(255, 255, 0, 255);
        Eigen::Matrix pos3D = faceReco.position3DGlobal;
        armarx::viz::Sphere facePos = armarx::viz::Sphere("Person instance of " + name)
                                          .position(pos3D.x(), pos3D.y(), pos3D.z())
                                          .radius(radius)
                                          .color(color);
        Eigen::Matrix4f nameTagRotation;
        nameTagRotation << -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1;
        armarx::viz::Text nameTag =
            armarx::viz::Text("Name of" + name)
                .text(name)
                .scale(15)
                .pose(nameTagRotation)
                .position(pos3D.x(), pos3D.y() + (int)(radius / 2) + 10, pos3D.z());
        personInstanceLayer.add(facePos);
        personInstanceLayer.add(nameTag);
    }
 
    void
    addPersonInstanceToLayer(const armarx::armem::human::PersonInstance& personInstance,
                             const std::string& name,
                             armarx::viz::Layer& personInstanceLayer,
                             const std::string& prefix)
    {
        ARMARX_TRACE;
 
        if (personInstance.globalPose.matrix().isIdentity())
        {
            ARMARX_VERBOSE << "Global pose is identity -> not set. Won't visualize pose.";
            return;
        }
 
        static const int radius = 100;
 
        // Face: position
        {
            armarx::viz::Color color = armarx::viz::Color::fromRGBA(255, 255, 0, 255);
            armarx::viz::Sphere facePos = armarx::viz::Sphere("Position:" + name)
                                              .position(personInstance.globalPose.translation())
                                              .radius(radius)
                                              .color(color);
            personInstanceLayer.add(facePos);
        }
 
        // Face: orientation
        // if (not personInstance.globalPose.linear().isIdentity())
        // {
        //     armarx::viz::Pose facePose = armarx::viz::Pose("Pose:" + name)
        //                                      .position(personInstance.globalPose.translation())
        //                                      .scale(radius / 10);
        //     personInstanceLayer.add(facePose);
        // }
 
        // Name tag
        {
            Eigen::Matrix4f nameTagRotation;
            nameTagRotation << -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1;
            armarx::viz::Text nameTag =
                armarx::viz::Text("Name:" + name)
                    .text(name)
                    .scale(15)
                    .pose(nameTagRotation)
                    .position(personInstance.globalPose.translation() +
                              Eigen::Vector3f::UnitY() * ((radius / 2) + 10));
            personInstanceLayer.add(nameTag);
        }
    }
 
} // namespace armarx::viz