HumanPose.cpp

Go to the documentation of this file.

#include "HumanPose.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <VisionX/libraries/ArViz/HumanPoseBody25.h>
#include <VisionX/libraries/ArViz/HumanPoseK4ABTBody32.h>
#include <VisionX/libraries/ArViz/HumanPoseMMMBody68.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 armarx
{
 
    void
    viz::addPoseToLayer(const armarx::armem::human::HumanPose& pose,
                        HumanPoseLayers& layers,
                        const std::string& prefix,
                        const DrawingStyle& drawingStyle)
    {
        namespace mmm_model_body_68 = human::pose::model::mmm_body_68;
        namespace k4a_bt_body_32 = human::pose::model::k4a_bt_body_32;
        namespace openpose_body_25 = human::pose::model::openpose_body_25;
 
        if (pose.poseModelId == openpose_body_25::ModelId)
        {
            body_25::addPoseToLayer(pose, layers, prefix, drawingStyle);
        }
        else if (pose.poseModelId == k4a_bt_body_32::ModelId)
        {
            k4abt_body_32::addPoseToLayer(pose, layers, prefix, drawingStyle);
        }
        else if (pose.poseModelId == mmm_model_body_68::ModelId)
        {
            mmm_body_68::addPoseToLayer(pose, layers, prefix, drawingStyle);
        }
        else
        {
            ARMARX_WARNING << "Unknown pose model ID '" << pose.poseModelId << "'.  No generic "
                           << "visualization is available at this point.";
 
            generic::addPoseToLayer(pose, layers, prefix, drawingStyle);
        }
    }
 
    void
    viz::addPosesToLayer(const std::map<std::string, armarx::armem::human::HumanPose>& poses,
                         HumanPoseLayers& layers,
                         const std::string& prefix,
                         const DrawingStyle& drawingStyle)
    {
        for (const auto& [name, pose] : poses)
        {
            addPoseToLayer(pose, layers, prefix + name, drawingStyle);
        }
    }
 
    void
    viz::generic::addPoseToLayer(const armarx::armem::human::HumanPose& pose,
                                 HumanPoseLayers& layers,
                                 const std::string& prefix,
                                 const DrawingStyle& drawingStyle)
    {
        static const int jointThickness = 30;
        armarx::viz::Color color =
            armarx::viz::Color::fromRGBA(1, 1, 1, 1); // TODO: Random, seed with person ID.
 
        // Cannot visualize segments without pose model information.
 
        // Keypoints.
        for (auto const& [name, kp] : pose.keypoints)
        {
            armarx::viz::Sphere sphere = armarx::viz::Sphere(prefix + "::" + name)
                                             .position(kp.positionGlobal->toEigen())
                                             .radius(jointThickness)
                                             .color(color);
            layers.skeleton.add(sphere);
        }
    }
 
 
} // namespace armarx