util.cpp

Go to the documentation of this file.

#include "util.h"
 
#include <limits>
 
#include <SimoxUtility/algorithm/string.h>
 
#include <ArmarXCore/core/application/properties/PropertyDefinition.h>
#include <ArmarXCore/core/application/properties/PropertyDefinition.hpp>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
 
namespace armarx::armem::human
{
 
    std::optional<Eigen::Vector3f>
    computeMeanPosition(const HumanPose& humanPose, KeyPointCoordinateSystem coordSystem)
    {
        Eigen::Vector3f sumPosition = Eigen::Vector3f::Zero();
        int sumWeights = 0;
        for (const auto& [id, keypoint] : humanPose.keypoints)
        {
            switch (coordSystem)
            {
                case Camera:
                {
                    sumPosition += keypoint.positionCamera.toEigen();
                    sumWeights += 1;
                    break;
                }
                case Global:
                    [[fallthrough]];
                case GlobalFloorProjection:
                {
                    if (keypoint.positionGlobal)
                    {
                        sumPosition += keypoint.positionGlobal.value().toEigen();
                        sumWeights += 1;
                    }
                    break;
                }
                case Robot:
                {
                    if (keypoint.positionRobot)
                    {
                        sumPosition += keypoint.positionRobot.value().toEigen();
                        sumWeights += 1;
                    }
                    break;
                }
                default:
                    break;
            }
        }
        if (sumWeights == 0)
            return std::nullopt;
        if (coordSystem == KeyPointCoordinateSystem::GlobalFloorProjection)
            sumPosition.z() = 0.0;
        return sumPosition / sumWeights;
    }
 
    std::optional<HumanPose>
    getNearestHuman(const std::vector<HumanPose>& humanPoses,
                    const NearestHumanParameters& parameters)
    {
        std::optional<HumanPose> nearestHumanPose = std::nullopt;
        double minDistance = std::numeric_limits<double>::infinity();
        for (const HumanPose& humanPose : humanPoses)
        {
            if (parameters.modelIDs.empty() or parameters.modelIDs.count(humanPose.poseModelId))
            {
                const std::optional<Eigen::Vector3f> meanPosition =
                    computeMeanPosition(humanPose, parameters.coordSystem);
                if (not meanPosition)
                    continue;
                const double distance = (meanPosition.value() - parameters.positionMM).norm();
                if (distance < minDistance)
                {
                    minDistance = distance;
                    nearestHumanPose = humanPose;
                }
            }
        }
        if (parameters.distanceThresholdMM and minDistance > parameters.distanceThresholdMM)
        {
            return std::nullopt;
        }
        return nearestHumanPose;
    }
 
    void
    NearestHumanParameters::Properties::addTo(armarx::PropertyDefinitionsPtr& def,
                                              const std::string& prefix)
    {
        const std::string prefix_ = prefix + "nearestHuman.";
        def->optional(
            positionMM.x(), prefix_ + "position.x", "Anchor position (x-coordinate) in mm.");
        def->optional(
            positionMM.y(), prefix_ + "position.y", "Anchor position (y-coordinate) in mm.");
        def->optional(
            positionMM.z(), prefix_ + "position.z", "Anchor position (z-coordinate) in mm.");
        def->optional(
               coordSystem, prefix_ + "coordSystem", "Coordinate system of the anchor position.")
            .map("Camera", KeyPointCoordinateSystem::Camera)
            .map("Global", KeyPointCoordinateSystem::Global)
            .map("GlobalFloorProjection", KeyPointCoordinateSystem::GlobalFloorProjection)
            .map("Robot", KeyPointCoordinateSystem::Robot)
            .setCaseInsensitive(true);
        def->optional(distanceThresholdMM,
                      prefix_ + "distanceThreshold",
                      "Optional minimum distance threshold in mm.");
        def->optional(
            modelIDs, prefix_ + "allowedModelIDs", "Allowed pose model ids, seperated by commas.");
    }
 
    void
    NearestHumanParameters::updateFromProperties()
    {
        positionMM = properties.positionMM;
        coordSystem = properties.coordSystem;
        if (properties.distanceThresholdMM > 0.0)
        {
            distanceThresholdMM = properties.distanceThresholdMM;
        }
        const std::vector<std::string> ids = simox::alg::split(properties.modelIDs, ",");
        modelIDs = std::set<std::string>(ids.begin(), ids.end());
 
        ARMARX_INFO << "Setting nearest human position to " << positionMM.transpose()
                    << " in frame " << coordSystem;
    }
 
} // namespace armarx::armem::human