util.cpp

Go to the documentation of this file.

#include "util.h"
 
#include <algorithm>
#include <cmath>
#include <limits>
#include <optional>
#include <set>
#include <string>
#include <vector>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
 
#include <SimoxUtility/algorithm/string.h>
 
#include <ArmarXCore/core/application/properties/PropertyDefinition.h>
#include <ArmarXCore/core/application/properties/PropertyDefinition.hpp> // IWYU pragma: keep
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include "RobotAPI/libraries/core/FramedPose.h"
 
#include "VisionX/libraries/armem_human/types.h"
#include "VisionX/libraries/human/pose/model/k4a_bt_body_32.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;
    }
 
    std::optional<Eigen::Vector3f>
    calculateDirectionOfHuman(const HumanPose& pose)
    {
        // We can either use both shoulders, or both ears, or ...
        // Possible candidates are listed here. We check if they are suitable
        // iteratively
 
        if (pose.poseModelId != armarx::human::pose::model::k4a_bt_body_32::ModelId)
        {
            ARMARX_WARNING << "NYI";
            return std::nullopt;
        }
 
        using Joints = armarx::human::pose::model::k4a_bt_body_32::Joints;
 
        const std::vector<std::vector<Joints>> jointCandidates{
            {Joints::ShoulderLeft, Joints::ShoulderRight},
            {Joints::HipLeft, Joints::HipRight},
            {Joints::EarLeft, Joints::EarRight},
            {Joints::KneeLeft, Joints::KneeRight}};
 
        const auto toString = [](const std::vector<Joints>& joints)
        {
            const auto toString = [](const Joints& joint)
            { return armarx::human::pose::model::k4a_bt_body_32::JointNames.to_name(joint); };
 
            return joints | ranges::views::transform(toString) | ranges::to_vector;
        };
 
        const std::vector<std::vector<std::string>> candidates =
            jointCandidates | ranges::views::transform(toString) | ranges::to_vector;
 
 
        std::vector<std::string> suitableKeypoints;
        for (const auto& candidate : candidates)
        {
            if (checkVisibility(pose, candidate, 0.3))
            {
                ARMARX_INFO << "Chosen candidates for pose direction: " << candidate;
                suitableKeypoints = candidate;
                break;
            }
        }
 
        // did we find anything usable?
        if (suitableKeypoints.empty())
        {
            return std::nullopt;
        }
 
        // idea: take vector between both points, rotate by 90 degrees, and project to
        // x/y direction
 
        ARMARX_CHECK_EQUAL(suitableKeypoints.size(), 2);
        const auto& kpLeft = pose.keypoints.at(suitableKeypoints.at(0));
        const auto& kpRight = pose.keypoints.at(suitableKeypoints.at(1));
 
        ARMARX_CHECK_NOT_NULL(kpLeft.positionGlobal);
        ARMARX_CHECK_NOT_NULL(kpRight.positionGlobal);
 
        ARMARX_CHECK_EQUAL(kpLeft.positionGlobal->frame, armarx::GlobalFrame);
        ARMARX_CHECK_EQUAL(kpRight.positionGlobal->frame, armarx::GlobalFrame);
 
        const Eigen::Vector3f posLeftGlobal = kpLeft.positionGlobal.value().toEigen();
        const Eigen::Vector3f posRightGlobal = kpRight.positionGlobal.value().toEigen();
 
        const Eigen::Vector3f leftToRightGlobal = posRightGlobal - posLeftGlobal;
 
        // from shoulder line to forward direction
        Eigen::Vector3f forwardDirectionOfHumanGlobal =
            Eigen::AngleAxisf(M_PI_2, Eigen::Vector3f::UnitZ()) * leftToRightGlobal;
 
        // project to x/y plane
        forwardDirectionOfHumanGlobal.z() = 0.0f;
 
        // ensure unit length
        forwardDirectionOfHumanGlobal.normalize();
 
        return forwardDirectionOfHumanGlobal;
    }
 
    bool
    checkVisibility(const HumanPose& pose, std::vector<std::string> labels, const float threshold)
    {
        // for every keypoint...
        for (const auto& [u, v] : pose.keypoints)
        {
            //... if it exists ...
            if (v.positionGlobal.has_value())
            {
                // ... find corresponding entry in the vector
                auto elem = std::find(labels.begin(), labels.end(), u);
 
                // check if keypoint is in label vector
                if (elem == labels.end())
                {
                    // this label is not interesting
                    continue;
                }
 
                // is the provided confidence sufficient?
                if (v.confidence < threshold)
                {
                    // we found a keypoint, but the confidence is not sufficient
                    return false;
                }
                else
                {
                    // if all is well, remove entry from vector
                    labels.erase(elem);
                }
            }
        }
 
        // did we miss a keypoint?
        return labels.empty();
    }
 
    std::optional<Eigen::Isometry2f>
    calculatePose(const HumanPose& humanPose)
    {
        const std::optional<Eigen::Vector3f> meanPositionGlobal =
            computeMeanPosition(humanPose, KeyPointCoordinateSystem::GlobalFloorProjection);
        if (not meanPositionGlobal)
        {
            return std::nullopt;
        }
 
        Eigen::Isometry2f global_T_human = Eigen::Isometry2f::Identity();
        global_T_human.translation() = meanPositionGlobal->head<2>();
        const std::optional<Eigen::Vector3f> directionOfHuman =
            calculateDirectionOfHuman(humanPose);
        if (not directionOfHuman)
        {
            return std::nullopt;
        }
 
        const float yaw = std::atan2(directionOfHuman->y(), directionOfHuman->x());
        global_T_human.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
 
        return global_T_human;
    }
 
 
} // namespace armarx::armem::human