util.cpp

Go to the documentation of this file.

#include "util.h"
 
#include <limits>
 
#include <ArmarXCore/core/application/properties/PropertyDefinition.h>
#include <ArmarXCore/core/application/properties/PropertyDefinition.hpp>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <SimoxUtility/algorithm/string.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