FaceRecognition.cpp

Go to the documentation of this file.

#include "FaceRecognition.h"
 
#include <cmath>
#include <map>
#include <string>
 
#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
{
    void
    viz::addFaceRecognitionsToLayer(
        const std::map<std::string, armarx::armem::human::FaceRecognition>& faceRecognitions,
        armarx::viz::Layer& faceRecognitionLayer,
        const std::string& prefix)
    {
        for (const auto& [name, faceRecognition] : faceRecognitions)
        {
            ARMARX_TRACE;
            static const int radius = 100;
            armarx::viz::Color color = faceRecognition.profileID.has_value()
                                           ? armarx::viz::Color::green()
                                           : armarx::viz::Color::red();
            Eigen::Matrix pos3D = faceRecognition.position3DGlobal;
            armarx::viz::Sphere facePos = armarx::viz::Sphere("Face of " + name)
                                              .position(pos3D.x(), pos3D.y(), pos3D.z())
                                              .radius(radius)
                                              .color(color);
            Eigen::Matrix3f nameTagRotation =
                Eigen::AngleAxisf{M_PI_2, Eigen::Vector3f::UnitX()}.toRotationMatrix();
 
            // 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)
                    .orientation(nameTagRotation)
                    .position(pos3D.x(), pos3D.y() + (int)(radius / 2) + 10, pos3D.z());
            faceRecognitionLayer.add(facePos);
            faceRecognitionLayer.add(nameTag);
        }
    }
 
} // namespace armarx