HumanPoseMMMBody68.cpp

Go to the documentation of this file.

#include "HumanPoseMMMBody68.h"
 
#include <string>
 
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include "RobotAPI/components/ArViz/Client/elements/Color.h"
#include <RobotAPI/components/ArViz/Client/Elements.h>
 
#include "VisionX/libraries/ArViz/HumanPose.h"
#include "VisionX/libraries/armem_human/types.h"
#include <VisionX/libraries/human/pose/model/mmm_body_68.h>
 
namespace armarx::viz
{
 
    using namespace armarx::human::pose::model::mmm_body_68;
 
    void
    mmm_body_68::addPoseToLayer(const armarx::armem::human::HumanPose& pose,
                                HumanPoseLayers& layers,
                                const std::string& prefix,
                                const DrawingStyle& drawingStyle)
    {
        ARMARX_TRACE;
 
        // Segments.
        for (const auto& [index1, index2] : Segments)
        {
            const std::string& name1 = JointNames.map().left.at(index1);
            const std::string& name2 = JointNames.map().left.at(index2);
 
            // Only draw segment if start end end are in keypoint map.
            if (pose.keypoints.find(name1) == pose.keypoints.end() or
                pose.keypoints.find(name2) == pose.keypoints.end())
            {
                continue;
            }
 
            const armarx::armem::human::PoseKeypoint p1 = pose.keypoints.at(name1);
            const armarx::armem::human::PoseKeypoint p2 = pose.keypoints.at(name2);
 
            const Eigen::Vector3f pos1 = p1.positionGlobal->toEigen();
            const Eigen::Vector3f pos2 = p2.positionGlobal->toEigen();
 
            if (pos1.allFinite() and pos2.allFinite())
            {
                if (pos1 == Eigen::Vector3f(0, 0, 0) or pos2 == Eigen::Vector3f(0, 0, 0))
                {
                    continue;
                }
 
                const armarx::viz::Color color =
                    drawingStyle.segmentColor.value_or(Colors.at(index2));
 
                const float scale = (index2 > Joints::WristRight && index2 <= Joints::HandTCPRight)
                                        ? drawingStyle.thinFactor
                                        : 1.;
                armarx::viz::Cylinder line =
                    armarx::viz::Cylinder(prefix + "::" + p1.label + "_" + p2.label)
                        .fromTo(pos1, pos2)
                        .radius(scale * drawingStyle.segmentThickness)
                        .color(color);
                layers.skeleton.add(line);
            }
        }
 
        // Keypoints.
        for (auto const& [id, name] : JointNames.map())
        {
            if (pose.keypoints.find(name) != pose.keypoints.end())
            {
                armarx::armem::human::PoseKeypoint kp = pose.keypoints.at(name);
 
                const armarx::viz::Color color = drawingStyle.jointColor.value_or(Colors.at(id));
                const float scale = (id > Joints::WristRight && id <= Joints::HandTCPRight)
                                        ? drawingStyle.thinFactor
                                        : 1.;
                armarx::viz::Sphere sphere = armarx::viz::Sphere(prefix + "::" + name)
                                                 .position(kp.positionGlobal->toEigen())
                                                 .radius(scale * drawingStyle.jointThickness)
                                                 .color(color);
                layers.skeleton.add(sphere);
            }
        }
    }
 
} // namespace armarx::viz