HumanPoseK4ABTBody32.cpp

Go to the documentation of this file.

#include "HumanPoseK4ABTBody32.h"
 
#include <string>
 
#include "RobotAPI/components/ArViz/Client/Elements.h"
#include "RobotAPI/components/ArViz/Client/elements/Color.h"
 
#include "VisionX/libraries/ArViz/HumanPose.h"
#include "VisionX/libraries/armem_human/types.h"
#include <VisionX/libraries/human/pose/model/k4a_bt_body_32.h>
 
namespace armarx::viz
{
 
    using namespace armarx::human::pose::model::k4a_bt_body_32;
 
    void
    k4abt_body_32::addPoseToLayer(const armarx::armem::human::HumanPose& pose,
                                  const HumanPoseLayers& layers,
                                  const std::string& prefix,
                                  const DrawingStyle& drawingStyle)
    {
        //const armarx::viz::Color colorCylinder =
        //    armarx::viz::Color::fromRGBA(1, 1, 1, 1); // TODO: Random, seed with person ID.
 
        // 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;
                }
 
                armarx::viz::Color color =
                    drawingStyle.segmentColor.value_or(Colors.at(JointNames.from_name(p2.label)));
 
                armarx::viz::Cylinder line =
                    armarx::viz::Cylinder(prefix + "::" + p1.label + "_" + p2.label)
                        .fromTo(pos1, pos2)
                        .radius(drawingStyle.segmentThickness)
                        .color(color);
                layers.skeleton.add(line);
            }
        }
 
        // Keypoints.
        for (auto const& [id, name] : JointNames.map())
        {
            const auto it = pose.keypoints.find(name);
            if (it != pose.keypoints.end())
            {
                armarx::armem::human::PoseKeypoint kp = pose.keypoints.at(name);
                armarx::viz::Color color =
                    drawingStyle.jointColor.value_or(Colors.at(JointNames.from_name(kp.label)));
 
                armarx::viz::Sphere sphere = armarx::viz::Sphere(prefix + "::" + name)
                                                 .position(kp.positionGlobal->toEigen())
                                                 .radius(drawingStyle.jointThickness)
                                                 .color(color);
 
                if (kp.orientationGlobal.has_value())
                {
                    armarx::viz::Pose pose = armarx::viz::Pose(prefix + "::" + name + "_pose")
                                                 .pose(kp.positionGlobal.value().toEigen(),
                                                       kp.orientationGlobal.value().toEigen());
                    layers.frames.add(pose);
                }
 
                layers.skeleton.add(sphere);
            }
        }
    }
 
 
} // namespace armarx::viz