LaserBasedProximity.cpp

Go to the documentation of this file.

#include "LaserBasedProximity.h"
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdlib>
#include <functional>
#include <limits>
#include <optional>
#include <string>
#include <utility>
#include <vector>
 
#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/append.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <range/v3/algorithm/all_of.hpp>
#include <range/v3/algorithm/min.hpp>
#include <range/v3/all.hpp>
#include <range/v3/numeric/accumulate.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/drop.hpp>
#include <range/v3/view/enumerate.hpp>
#include <range/v3/view/filter.hpp>
#include <range/v3/view/reverse.hpp>
#include <range/v3/view/transform.hpp>
#include <range/v3/view/zip_with.hpp>
 
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/cmaps/colormaps.h>
#include <SimoxUtility/shapes/OrientedBox.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/Layer.h>
#include <RobotAPI/components/ArViz/Client/elements/Color.h>
#include <RobotAPI/components/ArViz/Client/elements/Line.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/NavigationStackGeneralConfig.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/human/types.h>
#include <armarx/navigation/memory/types.h>
#include <armarx/navigation/safety_guard/SafetyGuard.h>
#include <armarx/navigation/safety_guard/aron/LaserBasedProximityParams.aron.generated.h>
#include <armarx/navigation/safety_guard/aron_conversions.h>
#include <armarx/navigation/safety_guard/core.h>
#include <armarx/navigation/util/geometry.h>
 
namespace rv = ::ranges::views;
 
namespace armarx::navigation::safety_guard
{
 
    namespace
    {
 
        inline core::TwistLimits
        mergeSafetyLimits(const std::vector<std::optional<core::TwistLimits>>& resultsAll)
        {
            const auto isValidFn =
                [](const std::optional<core::TwistLimits>& result) noexcept -> bool
            { return result.has_value(); };
 
            const std::vector<core::TwistLimits> validResults =
                resultsAll | rv::filter(isValidFn) |
                rv::transform(
                    [](const std::optional<core::TwistLimits>& result) noexcept -> core::TwistLimits
                    { return result.value(); }) |
                ranges::to_vector;
 
 
            if (validResults.empty())
            {
                return core::TwistLimits::NoLimits();
            }
 
            const std::vector<float> linearLimits =
                validResults |
                rv::transform([](const core::TwistLimits& result) noexcept -> float
                              { return result.linear; }) |
                ranges::to_vector;
 
            const std::vector<float> angularLimits =
                validResults |
                rv::transform([](const core::TwistLimits& result) noexcept -> float
                              { return result.angular; }) |
                ranges::to_vector;
 
            const core::TwistLimits combinedResult{.linear = ranges::min(linearLimits),
                                                   .angular = ranges::min(angularLimits)};
 
            return combinedResult;
        }
 
        simox::color::Color
        filterReasonColor(const FilterReason reason)
        {
            switch (reason)
            {
                case FilterReason::None:
                    return simox::Color::white();
                case FilterReason::NoPoints:
                    return simox::Color::gray();
                case FilterReason::IgnoredRegion:
                    return simox::Color::magenta();
                case FilterReason::AttachedObject:
                    return simox::Color::cyan();
                case FilterReason::NearFieldArtifact:
                    return simox::Color::yellow();
                case FilterReason::TooFarAway:
                    return simox::Color::blue();
                case FilterReason::MovingAway:
                    return simox::Color::green();
                case FilterReason::Tangential:
                    return simox::Color::orange();
            }
 
            return simox::Color::black();
        }
 
        std::string
        filterReasonName(const FilterReason reason)
        {
            switch (reason)
            {
                case FilterReason::None:
                    return "None";
                case FilterReason::NoPoints:
                    return "NoPoints";
                case FilterReason::IgnoredRegion:
                    return "IgnoredRegion";
                case FilterReason::AttachedObject:
                    return "AttachedObject";
                case FilterReason::NearFieldArtifact:
                    return "NearFieldArtifact";
                case FilterReason::TooFarAway:
                    return "TooFarAway";
                case FilterReason::MovingAway:
                    return "MovingAway";
                case FilterReason::Tangential:
                    return "Tangential";
            }
 
            return "Unknown";
        }
 
    } // namespace
 
    Algorithms
    LaserBasedProximityParams::algorithm() const
    {
        return Algorithms::LaserBasedProximity;
    }
 
    aron::data::DictPtr
    LaserBasedProximityParams::toAron() const
    {
        arondto::LaserBasedProximityParams dto;
 
        armarx::navigation::safety_guard::toAron(dto, *this);
 
        return dto.toAron();
    }
 
    LaserBasedProximityParams
    LaserBasedProximityParams::FromAron(const aron::data::DictPtr& dict)
    {
        arondto::LaserBasedProximityParams dto;
        dto.fromAron(dict);
 
        LaserBasedProximityParams bo;
        fromAron(dto, bo);
 
        return bo;
    }
 
    LaserBasedProximity::LaserBasedProximity(const Params& params,
                                             const core::GeneralConfig& generalConfig,
                                             const core::Scene& scene,
                                             const Context& ctx) :
        SafetyGuard(scene, ctx),
        params(params),
        generalConfig(generalConfig),
        humanColorMap_(simox::color::cmaps::BuPu().reversed()),
        laserColorMap_(simox::color::cmaps::OrRd().reversed())
    {
    }
 
    SafetyGuardResult
    LaserBasedProximity::computeSafetyLimits(const Eigen::Vector2f& global_V_movement)
    {
        ARMARX_CHECK(scene.dynamicScene.has_value());
 
        auto layer = viz.layer("safety_guard");
 
        // Debug only: hide these layers in ArViz when they are not needed.
        auto clusterLayer = viz.layer("safety_guard_clusters");
        auto filteredLayer = viz.layer("safety_guard_filtered");
 
        const std::optional<core::TwistLimits> resultHumans = safetyLimitsHumans(layer);
        const std::optional<core::TwistLimits> resultLaserScanners =
            safetyLimitsLaserScanners(layer, clusterLayer, filteredLayer, global_V_movement);
 
        // Commit always. This ensures that objects eventually will be cleared.
        viz.commit(std::vector<viz::Layer>{layer, clusterLayer, filteredLayer});
 
        const core::TwistLimits combinedResult =
            mergeSafetyLimits({resultHumans, resultLaserScanners});
        ARMARX_VERBOSE << "Safety limits: " << VAROUT(combinedResult.linear)
                       << VAROUT(combinedResult.angular);
 
        return SafetyGuardResult{.twistLimits = combinedResult};
    }
 
    std::optional<core::TwistLimits>
    LaserBasedProximity::safetyLimitsLaserScanners(viz::Layer& layer,
                                                   viz::Layer& clusterLayer,
                                                   viz::Layer& filteredLayer,
                                                   const Eigen::Vector2f& global_V_movement) const
    {
        if (not params.enableLaserScanners)
        {
            return std::nullopt;
        }
 
        if (scene.dynamicScene->laserScannerFeatures.empty())
        {
            ARMARX_INFO << deactivateSpam(5) << "No laser scanner features for SafetyGuard";
 
            return std::nullopt;
        }
 
        ARMARX_CHECK_NOT_NULL(context.debugObserver);
        auto& debugObserver = *context.debugObserver;
 
        debugObserver.setDebugObserverDatafield("numLaserScannerFeatures",
                                                scene.dynamicScene->laserScannerFeatures.size());
        debugObserver.setDebugObserverDatafield(
            "numFirstLaserScannerFeatures",
            scene.dynamicScene->laserScannerFeatures.front().features.size());
 
        // Diagnostics: the size of the smallest cluster of the *current* frame, before any
        // filtering. Note that `closestClusterSize` (see velocityLimitsDirectionDependent) refers
        // to the velocity-constraining obstacle and is computed over the accumulated history of
        // several frames, so the two are not comparable.
        {
            std::size_t smallestClusterSize = std::numeric_limits<std::size_t>::max();
 
            // Only `points` is evaluated here; a feature that carries a convex hull but no points
            // is invisible to this safety guard while still being drawn by the memory visu.
            std::size_t numFeaturesWithoutPoints = 0;
 
            for (const auto& features : scene.dynamicScene->laserScannerFeatures)
            {
                for (const auto& feature : features.features)
                {
                    smallestClusterSize = std::min(smallestClusterSize, feature.points.size());
 
                    if (feature.points.empty())
                    {
                        numFeaturesWithoutPoints++;
                    }
                }
            }
 
            if (smallestClusterSize != std::numeric_limits<std::size_t>::max())
            {
                debugObserver.setDebugObserverDatafield("smallestClusterSize", smallestClusterSize);
            }
 
            debugObserver.setDebugObserverDatafield("numFeaturesWithoutPoints",
                                                    numFeaturesWithoutPoints);
        }
 
        ARMARX_VERBOSE << VAROUT(scene.dynamicScene->laserScannerFeatures.size());
        ARMARX_VERBOSE << VAROUT(scene.dynamicScene->laserScannerFeatures.front().features.size());
 
        const core::Pose global_T_robot{scene.robot->getGlobalPose()};
 
        ARMARX_VERBOSE << VAROUT(global_T_robot.translation());
 
        const core::Pose robot_T_global{global_T_robot.inverse()};
 
        // compute distance based on convex hull of robot
        util::geometry::polygon_type robotConvexHull;
        {
            boost::geometry::model::multi_point<util::geometry::point_type> robotNodes;
            for (const auto& node : scene.robot->getRobotNodes())
            {
                Eigen::Vector2f pos2d = conv::to2D(core::Pose(node->getGlobalPose())).translation();
                boost::geometry::append(robotNodes,
                                        util::geometry::point_type(pos2d.x(), pos2d.y()));
            }
            boost::geometry::convex_hull(robotNodes, robotConvexHull);
        }
 
        // Compute the minimal distance to the robot for a set of features (point clusters)
        const auto minDistanceFn =
            [this, &robotConvexHull, &robot_T_global](const memory::LaserScannerFeatures& features)
            -> std::vector<DistanceAndClosestPoint>
        {
            const core::Pose& global_T_sensor = features.frameGlobalPose;
 
            // Compute the minimal distance to the robot for a single feature (point cluster)
            const auto minDistanceFnInner =
                [this, &robotConvexHull, &global_T_sensor, &robot_T_global](
                    const memory::LaserScannerFeature& feature) -> DistanceAndClosestPoint
            {
                // transform points to 3D global frame
                const std::vector<Eigen::Vector3f> points3dGlobal =
                    feature.points |
                    rv::transform([&global_T_sensor](const Eigen::Vector2f& pt) -> Eigen::Vector3f
                                  { return global_T_sensor * conv::to3D(pt); }) |
                    ranges::to_vector;
 
                return calculateMinDistance(robotConvexHull, robot_T_global, points3dGlobal);
            };
 
            // Compute the minimal distance to the robot for all features (point clusters)
            ARMARX_VERBOSE << VAROUT(features.features.size());
            const std::vector<DistanceAndClosestPoint> distances =
                features.features | rv::transform(minDistanceFnInner) | ranges::to_vector;
 
            return distances;
        };
 
        laserScannerFeatureHistory_.push_back(scene.dynamicScene->laserScannerFeatures);
 
        const std::vector<memory::LaserScannerFeatures> accumulatedLaserScannerFeatures =
            laserScannerFeatureHistory_ | ranges::views::join |
            ranges::to<std::vector<memory::LaserScannerFeatures>>;
 
        // not const: velocityLimitsDirectionDependent() annotates the filter reasons
        std::vector<DistanceAndClosestPoint> minDistanceToObstacles =
            accumulatedLaserScannerFeatures | rv::transform(minDistanceFn) | ranges::views::join |
            ranges::to_vector;
 
        const auto result = [&]() -> std::optional<InternalVelocityLimitResult>
        {
            switch (params.laserScannerProximityField.mode)
            {
                case ProximityFieldParams::Mode::Disabled:
                    return std::nullopt;
                case ProximityFieldParams::Mode::DirectionDependent:
                    return velocityLimitsDirectionDependent(
                        global_V_movement, minDistanceToObstacles, global_T_robot);
                case ProximityFieldParams::Mode::DirectionIndependent:
                    return velocityLimitsDirectionIndependent(minDistanceToObstacles);
            }
 
            ARMARX_ERROR << "Unknown ProximityFieldParams::Mode: "
                         << static_cast<int>(params.laserScannerProximityField.mode);
            return std::nullopt;
        }();
 
        layer.clear();
 
        // visualize robot and obstacle-independent information (always)
        {
            // visualize convex hull of robot
            simox::color::Color color = simox::Color::blue();
            viz::Polygon vizPoly("robot_convex_hull");
            for (const auto& p : rv::reverse(robotConvexHull.outer()))
            {
                Eigen::Vector2f pt(p.x(), p.y());
                vizPoly.addPoint(conv::to3D(pt));
            }
            layer.add(vizPoly.color(color).lineColor(color));
 
            // visualize movement direction
            layer.add(viz::Line("robot_movement_Dir")
                          .fromTo(global_T_robot.translation(),
                                  global_T_robot.translation() +
                                      conv::to3D(global_V_movement).normalized() * 1000)
                          .lineWidth(10.F)
                          .color(simox::Color::blue()));
 
            // visualize ignored regions
            for (const auto& [i, region] : ranges::views::enumerate(params.ignoredRegions))
            {
                viz::Polygon vizRegion("ignored_region_" + std::to_string(i));
                vizRegion.addPoint(Eigen::Vector3f(region.min().x(), region.min().y(), 15));
                vizRegion.addPoint(Eigen::Vector3f(region.max().x(), region.min().y(), 15));
                vizRegion.addPoint(Eigen::Vector3f(region.max().x(), region.max().y(), 15));
                vizRegion.addPoint(Eigen::Vector3f(region.min().x(), region.max().y(), 15));
                layer.add(vizRegion.color(simox::Color::magenta()));
            }
 
            // visualize attached objects
            for (const auto& [i, obj] :
                 ranges::views::enumerate(scene.dynamicScene->attachedObjects))
            {
                const auto& oobb = obj->oobbGlobal();
                ARMARX_CHECK(oobb.has_value());
 
                simox::OrientedBoxf inflatedOobb{
                    oobb->transformation_centered(),
                    oobb->dimensions() + Eigen::Vector3f::Ones() * params.attachedObjectsInflation};
 
                viz::Box vizObj("attached_object_" + std::to_string(i));
                vizObj.set(inflatedOobb);
                layer.add(vizObj.color(simox::Color::magenta()));
            }
        }
 
        // Debug only: visualize every feature of the *current* frame and, on a separate layer,
        // why it was filtered out. Note that the velocity limits are computed over the
        // accumulated feature history, so these counts are lower than the `numObstacles*`
        // datafields. In `DirectionIndependent` mode only the reasons determined by
        // calculateMinDistance() are available.
        {
            const std::size_t numCurrentFrameFeatures = ranges::accumulate(
                scene.dynamicScene->laserScannerFeatures |
                    rv::transform([](const memory::LaserScannerFeatures& features) noexcept -> std::size_t
                                  { return features.features.size(); }),
                std::size_t{0});
 
            ARMARX_CHECK_GREATER_EQUAL(minDistanceToObstacles.size(), numCurrentFrameFeatures);
 
            // the current frame was appended last, see laserScannerFeatureHistory_
            const auto currentFrameFeatures =
                minDistanceToObstacles |
                rv::drop(minDistanceToObstacles.size() - numCurrentFrameFeatures);
 
            const float sphereRadius = 50.F;
            const float textScale = 3.F;
            const Eigen::Vector3f sphereOffset{0, 0, 50};
            const Eigen::Vector3f sizeTextOffset{0, 0, 200};
            const Eigen::Vector3f reasonTextOffset{0, 0, 350};
 
            for (const auto& [i, obstacle] : ranges::views::enumerate(currentFrameFeatures))
            {
                if (obstacle.filterReason == FilterReason::NoPoints)
                {
                    // without points there is no position to draw
                    continue;
                }
 
                const std::string suffix = std::to_string(i);
                const Eigen::Vector3f position = conv::to3D(obstacle.centroid);
 
                clusterLayer.add(viz::Sphere("cluster_" + suffix)
                                     .position(position + sphereOffset)
                                     .radius(sphereRadius)
                                     .color(simox::Color::white()));
                clusterLayer.add(viz::Text("cluster_size_" + suffix)
                                     .position(position + sizeTextOffset)
                                     .text(std::to_string(obstacle.clusterSize))
                                     .scale(textScale)
                                     .color(simox::Color::white()));
 
                if (obstacle.filterReason == FilterReason::None)
                {
                    continue;
                }
 
                const simox::color::Color color = filterReasonColor(obstacle.filterReason);
 
                filteredLayer.add(viz::Sphere("filtered_" + suffix)
                                      .position(position + sphereOffset)
                                      .radius(sphereRadius)
                                      .color(color));
                filteredLayer.add(viz::Text("filtered_reason_" + suffix)
                                      .position(position + reasonTextOffset)
                                      .text(filterReasonName(obstacle.filterReason))
                                      .scale(textScale)
                                      .color(color));
            }
        }
 
        if (not result.has_value())
        {
            return std::nullopt;
        }
 
        // visualize the obstacle that leads to the calculated safety limits and the corresponding velocity limits
        if (result->closestPoint.has_value())
        {
            // increase transparency with increased distance, but never make fully transparent
            const auto color = laserColorMap_.at(
                std::abs(result->twistLimits.linear), 0, generalConfig.maxVel.linear);
 
            layer.add(
                viz::Line("nearest_obstacle")
                    .fromTo(global_T_robot.translation(), conv::to3D(result->closestPoint.value()))
                    .lineWidth(50.F)
                    .color(color));
        }
 
        return result->twistLimits;
    }
 
    std::optional<core::TwistLimits>
    LaserBasedProximity::safetyLimitsHumans(viz::Layer& layer) const
    {
        if (not params.enableHumans)
        {
            return std::nullopt;
        }
 
        const core::Pose global_T_robot{scene.robot->getGlobalPose()};
 
        // Compute the minimal distance to the robot for a human
        // TODO(utetg): is minimum distance = distance to robot center or robot edge
        const auto minimalSegmentDistanceToRobot =
            [&global_T_robot](const human::Human& human) -> float
        {
            const Eigen::Isometry3f global_T_human = conv::to3D(human.pose);
            const Eigen::Isometry3f robot_T_human = global_T_robot.inverse() * global_T_human;
 
            // TODO extension: if also human keypoints are available, use them to compute the minimal distance
            return robot_T_human.translation().norm();
        };
 
        if (scene.dynamicScene->humans.empty())
        {
            ARMARX_VERBOSE << "No humans";
            return std::nullopt;
        }
 
        const auto distanceRobotToHumans = scene.dynamicScene->humans |
                                           rv::transform(minimalSegmentDistanceToRobot) |
                                           ranges::to_vector;
 
        const std::optional<float> minDistanceToHumans = scene.dynamicScene->humans.empty()
                                                             ? std::optional<float>{std::nullopt}
                                                             : ranges::min(distanceRobotToHumans);
 
        // handle if no humans are detected
        if (not minDistanceToHumans.has_value())
        {
            return core::TwistLimits::NoLimits();
        }
 
        const auto result =
            evaluateProximityField(params.humanProximityField, minDistanceToHumans.value());
 
        {
            // increase transparency with increased distance, but never make fully transparent
            const auto color = humanColorMap_(result.second).with_alpha(1 - result.second + 0.1);
 
            layer.add(viz::Cylinder("distance_humans")
                          .position(global_T_robot.translation() + Eigen::Vector3f{0, 0, 10})
                          .direction(Eigen::Vector3f::UnitZ())
                          .radius(minDistanceToHumans.value())
                          .height(10)
                          .color(color));
        }
 
        return result.first;
    }
 
    LaserBasedProximity::DistanceAndClosestPoint
    LaserBasedProximity::calculateMinDistance(
        const util::geometry::polygon_type& convexHull,
        const core::Pose& robot_T_global,
        const std::vector<Eigen::Vector3f>& globalPoints) const
    {
        // Compute the minimal distance to the robot for a list of points
 
        // transform points to 2D global frame
        const std::vector<Eigen::Vector2f> points2dGlobal =
            globalPoints |
            rv::transform([](const Eigen::Vector3f& pt) -> Eigen::Vector2f
                          { return conv::to2D(pt); }) |
            ranges::to_vector;
 
        // a feature that is ignored must never constrain the velocity
        const auto ignoredResult = [&globalPoints](const Eigen::Vector2f& centroid,
                                                   const FilterReason filterReason)
            -> DistanceAndClosestPoint
        {
            return {.distance = std::numeric_limits<float>::max(),
                    .closestPoint = Eigen::Vector2f::Zero(),
                    .clusterSize = globalPoints.size(),
                    .centroid = centroid,
                    .filterReason = filterReason};
        };
 
        if (globalPoints.empty())
        {
            ARMARX_VERBOSE << "Discarding feature: it has no points";
            return ignoredResult(Eigen::Vector2f::Zero(), FilterReason::NoPoints);
        }
 
        const Eigen::Vector2f centroid =
            ranges::accumulate(points2dGlobal,
                               Eigen::Vector2f{Eigen::Vector2f::Zero()},
                               [](const Eigen::Vector2f& lhs, const Eigen::Vector2f& rhs)
                                   -> Eigen::Vector2f { return lhs + rhs; }) /
            static_cast<float>(points2dGlobal.size());
 
        if (const FilterReason ignoreReason =
                featureIgnoreReason(globalPoints, points2dGlobal, centroid);
            ignoreReason != FilterReason::None)
        {
            // featureIgnoreReason() logs the reason
            return ignoredResult(centroid, ignoreReason);
        }
 
        // calculate distance to robot center
        const std::vector<float> pointsDistanceRobotCenter =
            globalPoints |
            rv::transform([&robot_T_global](const Eigen::Vector3f& pt) -> float
                          { return conv::to2D(robot_T_global * pt).norm(); }) |
            ranges::to_vector;
 
        // Small features *close to the robot* are most likely measuring artifacts of the sensors.
        // This filter is deliberately restricted to the near field: further away, a cluster with
        // few points is a real (small or distant) obstacle and must not be discarded.
        if (params.enableLaserScannerFiltering and
            globalPoints.size() <= static_cast<std::size_t>(params.laserScannerFilteringThreshold))
        {
            const auto isCloseToRobot = [this](const float distanceToCenter) noexcept -> bool
            {
                return std::max(distanceToCenter - params.robotRadius, 0.F) <
                       params.laserScannerMaxFilteringDistance;
            };
 
            // distance of the point closest to the robot surface, for logging
            const float minSurfaceDistance =
                std::max(ranges::min(pointsDistanceRobotCenter) - params.robotRadius, 0.F);
 
            if (ranges::all_of(pointsDistanceRobotCenter, isCloseToRobot))
            {
                // the feature is close to the robot
                // -> it should be filtered out
                ARMARX_VERBOSE << "Ignoring small feature with " << globalPoints.size()
                               << " points (<= laserScannerFilteringThreshold "
                               << params.laserScannerFilteringThreshold
                               << "): all points are within laserScannerMaxFilteringDistance "
                               << params.laserScannerMaxFilteringDistance
                               << " mm of the robot surface (closest point at "
                               << minSurfaceDistance
                               << " mm). Distances to robot center: " << pointsDistanceRobotCenter;
                return ignoredResult(centroid, FilterReason::NearFieldArtifact);
            }
 
            ARMARX_VERBOSE << "Keeping small feature with " << globalPoints.size()
                           << " points: not all points are within "
                           << params.laserScannerMaxFilteringDistance
                           << " mm of the robot surface (closest point at " << minSurfaceDistance
                           << " mm)";
        }
 
        const auto distancesAndClosestPoint =
            rv::zip_with(
                [this, &convexHull, &globalPoints, &centroid](
                    const Eigen::Vector2f& pt, float distanceToCenter) -> DistanceAndClosestPoint
                {
                    const auto distanceToRobotConvexHull =
                        static_cast<float>(boost::geometry::distance(
                            util::geometry::point_type(pt.x(), pt.y()), convexHull));
 
                    const float distanceUsingRobotRadius =
                        std::max(distanceToCenter - params.robotRadius, 0.F);
 
                    return {.distance =
                                std::min(distanceToRobotConvexHull, distanceUsingRobotRadius),
                            .closestPoint = pt,
                            .clusterSize = globalPoints.size(),
                            .centroid = centroid,
                            .filterReason = FilterReason::None};
                },
                points2dGlobal,
                pointsDistanceRobotCenter) |
            ranges::to_vector;
 
        return ranges::min(
            distancesAndClosestPoint, std::less{}, &DistanceAndClosestPoint::distance);
    }
 
    std::pair<core::TwistLimits, float>
    LaserBasedProximity::evaluateProximityField(const ProximityFieldParams& proximityField,
                                                float minDistance) const
    {
        if (minDistance < proximityField.safetyDistance)
        {
            return {core::TwistLimits::ZeroLimits(), 0.F};
        }
 
        if (minDistance > proximityField.influenceDistance)
        {
            return {core::TwistLimits::NoLimits(), 1.F};
        }
 
 
        const float proximityRange =
            proximityField.influenceDistance - proximityField.safetyDistance;
        const float clippedDistance =
            std::min(minDistance, proximityField.influenceDistance) - proximityField.safetyDistance;
 
        const float fractionalDistance = std::clamp(clippedDistance / proximityRange, 0.F, 1.F);
 
        if (not proximityField.reduceVelocity)
        {
            return {core::TwistLimits::NoLimits(), fractionalDistance};
        }
 
        const float d_s = std::pow(1 - fractionalDistance, proximityField.k);
 
        const auto permissibleVelocity = [&proximityField](const float d_s,
                                                           const float v_max) -> float
        { return v_max / (1 + proximityField.lambda * d_s); };
 
        const core::TwistLimits result{
            .linear = permissibleVelocity(d_s, generalConfig.maxVel.linear),
            .angular = permissibleVelocity(d_s, generalConfig.maxVel.angular)};
 
        return {result, fractionalDistance};
    }
 
    template <class T, class Func>
    static bool
    allPointsInside(const std::vector<T>& points, Func isInsideFunc)
    {
        for (const auto& p : points)
        {
            if (not isInsideFunc(p))
            {
                // this point is not inside
                return false;
            }
        }
        return true;
    }
 
    FilterReason
    LaserBasedProximity::featureIgnoreReason(
        const std::vector<Eigen::Vector3f>& featurePoints3DGlobal,
        const std::vector<Eigen::Vector2f>& featurePoints2DGlobal,
        const Eigen::Vector2f& centroid) const
    {
        // Note: features are *not* ignored based on their number of points alone. Small clusters
        // are real obstacles (e.g. table legs, poles). Filtering of small clusters that are likely
        // to be sensor artifacts happens in calculateMinDistance() and is restricted to the near
        // field of the robot.
 
        const auto featureInfo = [&]() -> std::string
        {
            return "feature with " + std::to_string(featurePoints3DGlobal.size()) +
                   " points around global (" + std::to_string(centroid.x()) + ", " +
                   std::to_string(centroid.y()) + ")";
        };
 
        // first check against ignored regions
        for (const auto& [i, region] : ranges::views::enumerate(params.ignoredRegions))
        {
            ARMARX_CHECK(not region.isEmpty());
 
            if (allPointsInside(featurePoints2DGlobal,
                                [&region](const Eigen::Vector2f& p) { return region.contains(p); }))
            {
                // the feature lies fully inside this region -> it is ignored
                ARMARX_VERBOSE << "Ignoring " << featureInfo() << ": lies fully inside ignored "
                               << "region " << i << " [" << region.min().transpose() << "; "
                               << region.max().transpose() << "]";
                return FilterReason::IgnoredRegion;
            }
        }
 
        // then check against attached objects
        if (not params.ignoreAttachedObjects)
        {
            ARMARX_VERBOSE << deactivateSpam(5)
                           << "Not checking features against attached objects "
                              "(params.ignoreAttachedObjects is false)";
            return FilterReason::None;
        }
 
        for (const auto& [i, o] : ranges::views::enumerate(scene.dynamicScene->attachedObjects))
        {
            const auto& oobbGlobal = o->oobbGlobal();
            ARMARX_CHECK(oobbGlobal.has_value());
 
            simox::OrientedBoxf inflatedOobb{oobbGlobal->transformation_centered(),
                                             oobbGlobal->dimensions() +
                                                 Eigen::Vector3f::Ones() *
                                                     params.attachedObjectsInflation};
 
            if (allPointsInside(featurePoints3DGlobal,
                                [&inflatedOobb](const Eigen::Vector3f& p)
                                { return inflatedOobb.contains(p); }))
            {
                // the feature lies fully inside the objects oobb -> it is ignored
                ARMARX_VERBOSE << "Ignoring " << featureInfo() << ": lies fully inside the "
                               << "(inflated by " << params.attachedObjectsInflation
                               << " mm) OOBB of attached object " << i;
                return FilterReason::AttachedObject;
            }
        }
 
        ARMARX_VERBOSE << "Keeping " << featureInfo() << ": neither inside one of the "
                       << params.ignoredRegions.size() << " ignored regions nor inside one of the "
                       << scene.dynamicScene->attachedObjects.size() << " attached objects";
 
        return FilterReason::None;
    }
 
    LaserBasedProximity::InternalVelocityLimitResult
    LaserBasedProximity::velocityLimitsDirectionDependent(
        const Eigen::Vector2f& global_V_movement,
        std::vector<DistanceAndClosestPoint>& minDistanceToObstacles,
        const Eigen::Isometry3f& global_T_robot) const
    {
 
        float maxPermissibleRelativeVelocityLinear = generalConfig.maxVel.linear;
 
        std::optional<Eigen::Vector2f> minPoint = std::nullopt;
        std::optional<float> minDistance = std::nullopt;
 
        const Eigen::Isometry3f robot_T_global = global_T_robot.inverse();
 
        ARMARX_CHECK_NOT_NULL(context.debugObserver);
        auto& debugObserver = *context.debugObserver;
 
        debugObserver.setDebugObserverDatafield("numObstacles", minDistanceToObstacles.size());
 
        std::size_t closestClusterSize = 0;
        // ARMARX_VERBOSE << VAROUT(minDistanceToObstacles.size());
 
        // is there at least one obstacle that forces us to stop? if so, only such obstacles are
        // reported as the constraining one.
        bool mustStop = false;
 
        // a zero movement command has no direction. normalized() would yield NaN, which would
        // silently disable all comparisons below.
        const Eigen::Vector2f global_V_movementDirection =
            global_V_movement.norm() > 1e-6F ? global_V_movement.normalized().eval()
                                             : Eigen::Vector2f::Zero();
 
        // why obstacles did not constrain the velocity (reported to the debug observer below)
        std::size_t numFiltered = 0;    // ignored features, see calculateMinDistance()
        std::size_t numTooFarAway = 0;  // outside the influence distance
        std::size_t numMovingAway = 0;  // robot is moving away from them
        std::size_t numTangential = 0;  // robot passes them exactly tangentially
        std::size_t numConstraining = 0;
 
        for (auto& obstacle : minDistanceToObstacles)
        {
            const float distance = obstacle.distance;
            const Eigen::Vector2f& global_P_obstacle_pt = obstacle.closestPoint;
            const std::size_t clusterSize = obstacle.clusterSize;
 
            const float ds = params.laserScannerProximityField.safetyDistance;
            const float di = params.laserScannerProximityField.influenceDistance;
            const float d = distance;
 
            // too far away
            if (d >= di)
            {
                // features that were ignored got a distance of float max, see calculateMinDistance()
                if (d >= std::numeric_limits<float>::max())
                {
                    // keep the reason recorded by calculateMinDistance()
                    numFiltered++;
                }
                else
                {
                    numTooFarAway++;
                    obstacle.filterReason = FilterReason::TooFarAway;
                    ARMARX_VERBOSE << "Obstacle (cluster size " << clusterSize << ") at distance "
                                   << d << " mm is outside the influence distance " << di << " mm";
                }
                continue;
            }
 
            float velocity_damper = generalConfig.maxVel.linear * (d - ds) / (di - ds);
 
            ARMARX_VERBOSE << VAROUT(velocity_damper);
 
            const Eigen::Vector2f minPointGlobal = global_P_obstacle_pt;
 
            ARMARX_VERBOSE << VAROUT(minPointGlobal);
            ARMARX_VERBOSE << VAROUT(robot_T_global.translation().head<2>());
 
            const Eigen::Vector2f directionRobotToObstacle =
                (minPointGlobal - global_T_robot.translation().head<2>()).normalized();
 
            ARMARX_VERBOSE << VAROUT(directionRobotToObstacle);
 
            // Cases:
            // 1) > 0: approaching object
            // 2) < 0: moving away from object
            const float relativeVelocityScaling =
                directionRobotToObstacle.dot(global_V_movementDirection);
 
            ARMARX_VERBOSE << VAROUT(distance);
            ARMARX_VERBOSE << VAROUT(relativeVelocityScaling);
 
            // moving away from obstacle?
            if (relativeVelocityScaling < 0)
            {
                numMovingAway++;
                obstacle.filterReason = FilterReason::MovingAway;
                ARMARX_VERBOSE << "Obstacle (cluster size " << clusterSize << ") at distance " << d
                               << " mm does not constrain the velocity: the robot is moving away "
                                  "from it ("
                               << VAROUT(relativeVelocityScaling) << ")";
                continue;
            }
 
            if (d <= ds) // we must stop, if an obstacle is too close
            {
                numConstraining++;
                maxPermissibleRelativeVelocityLinear = 0;
 
                ARMARX_VERBOSE << "Obstacle (cluster size " << clusterSize << ") at distance " << d
                               << " mm is within the safety distance " << ds
                               << " mm -> the robot must stop";
 
                // report the closest of all obstacles that force us to stop, not the last one
                if (not mustStop or d < minDistance.value())
                {
                    mustStop = true;
                    minPoint = minPointGlobal;
                    minDistance = d;
                    closestClusterSize = clusterSize;
                }
                continue;
            }
 
            // if not exactly tangential
            if (std::abs(relativeVelocityScaling) > 1e-4)
            {
                numConstraining++;
 
                const float thisMaxPermissibileRelVelLinear =
                    velocity_damper / relativeVelocityScaling;
                ARMARX_VERBOSE << VAROUT(thisMaxPermissibileRelVelLinear);
 
                if (thisMaxPermissibileRelVelLinear < maxPermissibleRelativeVelocityLinear)
                {
                    ARMARX_VERBOSE << "New most constraining obstacle (cluster size " << clusterSize
                                   << ") at distance " << d << " mm: limiting the linear velocity "
                                   << "to " << thisMaxPermissibileRelVelLinear << " mm/s";
 
                    maxPermissibleRelativeVelocityLinear = thisMaxPermissibileRelVelLinear;
                    minPoint = minPointGlobal;
                    minDistance = d;
                    closestClusterSize = clusterSize;
                }
            }
            else
            {
                numTangential++;
                obstacle.filterReason = FilterReason::Tangential;
                ARMARX_VERBOSE << "Obstacle (cluster size " << clusterSize << ") at distance " << d
                               << " mm does not constrain the velocity: the robot passes it "
                                  "tangentially ("
                               << VAROUT(relativeVelocityScaling) << ")";
            }
        }
 
        ARMARX_VERBOSE << "Of " << minDistanceToObstacles.size() << " obstacles (accumulated over "
                       << "the feature history): " << numFiltered << " filtered out, "
                       << numTooFarAway << " too far away, " << numMovingAway << " moving away, "
                       << numTangential << " tangential, " << numConstraining << " constraining";
 
        debugObserver.setDebugObserverDatafield("maxPermissibleRelativeVelocityLinear",
                                                maxPermissibleRelativeVelocityLinear);
        debugObserver.setDebugObserverDatafield("numObstaclesFiltered", numFiltered);
        debugObserver.setDebugObserverDatafield("numObstaclesTooFarAway", numTooFarAway);
        debugObserver.setDebugObserverDatafield("numObstaclesMovingAway", numMovingAway);
        debugObserver.setDebugObserverDatafield("numObstaclesTangential", numTangential);
        debugObserver.setDebugObserverDatafield("numObstaclesConstraining", numConstraining);
 
        if (minDistance.has_value())
        {
            debugObserver.setDebugObserverDatafield("minDistance", minDistance.value());
            debugObserver.setDebugObserverDatafield("closestClusterSize", closestClusterSize);
        }
 
        core::TwistLimits result{.linear = std::max<float>(0, maxPermissibleRelativeVelocityLinear),
                                 .angular = generalConfig.maxVel.angular};
        return {.twistLimits = result, .minDistance = minDistance, .closestPoint = minPoint};
    }
 
    LaserBasedProximity::InternalVelocityLimitResult
    LaserBasedProximity::velocityLimitsDirectionIndependent(
        const std::vector<DistanceAndClosestPoint>& minDistanceToObstacles) const
    {
        const DistanceAndClosestPoint minDistance =
            ranges::min(minDistanceToObstacles, std::less{}, &DistanceAndClosestPoint::distance);
 
        const auto result =
            evaluateProximityField(params.laserScannerProximityField, minDistance.distance);
 
        return {.twistLimits = result.first,
                .minDistance = minDistance.distance,
                .closestPoint = minDistance.closestPoint};
    }
} // namespace armarx::navigation::safety_guard