HumanPoseMMMBody68.cpp

Go to the documentation of this file.

#include "HumanPoseMMMBody68.h"
 
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <VisionX/libraries/human/pose/model/mmm_body_68.h>
#include <ArmarXCore/core/logging/Logging.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)
    {
        ARMARX_TRACE;
        static const int segmentThickness = 20;
        static const int jointThickness = 30;
        static const float thinFactor = 1. / 3.;
 
        // 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  = Colors.at(index2);
 
                const float scale = (
                    index2 > Joints::WristRight && index2 <= Joints::HandTCPRight
                ) ? thinFactor : 1.;
                armarx::viz::Cylinder line =
                    armarx::viz::Cylinder(prefix + "::" + p1.label + "_" + p2.label)
                        .fromTo(pos1, pos2)
                        .radius(scale * 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 = Colors.at(id);
                const float scale = (id > Joints::WristRight && id <= Joints::HandTCPRight) ?
                    thinFactor : 1.;
                armarx::viz::Sphere sphere = armarx::viz::Sphere(prefix + "::" + name)
                                                 .position(kp.positionGlobal->toEigen())
                                                 .radius(scale * jointThickness)
                                                 .color(color);
                layers.skeleton.add(sphere);
            }
        }
    }
 
} // namespace armarx::viz