CableClusterFilter.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    Navigation::ArmarXObjects::LaserScannerFeatureExtraction
 * @author     Niklas Arlt ( niklas dot arlt at student dot kit dot edu )
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CableClusterFilter.h"
 
#include <math.h>
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <limits>
#include <utility>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
 
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
 
    namespace
    {
        float
        distanceToSegment(const Eigen::Vector2f& pt,
                          const Eigen::Vector2f& a,
                          const Eigen::Vector2f& b)
        {
            const Eigen::Vector2f ab = b - a;
            const float squaredLength = ab.squaredNorm();
            if (squaredLength < std::numeric_limits<float>::epsilon())
            {
                return (pt - a).norm();
            }
 
            const float t = std::clamp((pt - a).dot(ab) / squaredLength, 0.F, 1.F);
            return (pt - (a + t * ab)).norm();
        }
    } // namespace
 
    float
    distanceToConvexHullBoundary(const Eigen::Vector2f& pt,
                                 const VirtualRobot::MathTools::ConvexHull2D& hull)
    {
        const auto& vertices = hull.vertices;
        ARMARX_CHECK_GREATER_EQUAL(vertices.size(), 3);
 
        // The vertices of a convex hull are ordered along the boundary, so the point
        // is inside iff it lies on the same side of all edges.
        bool anyPositive = false;
        bool anyNegative = false;
 
        float minEdgeDistance = std::numeric_limits<float>::max();
 
        for (std::size_t i = 0; i < vertices.size(); i++)
        {
            const Eigen::Vector2f& a = vertices[i];
            const Eigen::Vector2f& b = vertices[(i + 1) % vertices.size()];
 
            const Eigen::Vector2f edge = b - a;
            const Eigen::Vector2f toPt = pt - a;
            const float cross = edge.x() * toPt.y() - edge.y() * toPt.x();
 
            anyPositive |= cross > 0;
            anyNegative |= cross < 0;
 
            minEdgeDistance = std::min(minEdgeDistance, distanceToSegment(pt, a, b));
        }
 
        const bool inside = not(anyPositive and anyNegative);
        return inside ? 0.F : minEdgeDistance;
    }
 
    CableClusterFilter::CableClusterFilter(const Params& params, DistanceToHullFn distanceToHull) :
        params(params), distanceToHull(std::move(distanceToHull))
    {
        ARMARX_CHECK(this->distanceToHull);
 
        const float plugDistance = params.plugPosition.norm();
        angularTestValid = plugDistance > 1.F; // [mm]
        if (angularTestValid)
        {
            outward = params.plugPosition / plugDistance;
        }
    }
 
    bool
    CableClusterFilter::isCandidate(const std::vector<Eigen::Vector2f>& clusterPoints) const
    {
        if (clusterPoints.empty())
        {
            return false;
        }
 
        const float cosHalfWindowAngle = std::cos(params.windowAngle / 2);
 
        for (const Eigen::Vector2f& pt : clusterPoints)
        {
            if (angularTestValid)
            {
                const Eigen::Vector2f fromPlug = pt - params.plugPosition;
                const float distanceToPlug = fromPlug.norm();
 
                // points at the plug-in point itself always pass the angular test
                if (distanceToPlug > 1.F // [mm]
                    and fromPlug.dot(outward) / distanceToPlug < cosHalfWindowAngle)
                {
                    return false;
                }
            }
 
            if (distanceToHull(pt) > params.maxDistanceToHull)
            {
                return false;
            }
        }
 
        return thickness(clusterPoints) <= params.maxThickness;
    }
 
    float
    CableClusterFilter::thickness(const std::vector<Eigen::Vector2f>& points)
    {
        if (points.size() < 2)
        {
            return 0.F;
        }
 
        Eigen::Vector2f mean = Eigen::Vector2f::Zero();
        for (const Eigen::Vector2f& pt : points)
        {
            mean += pt;
        }
        mean /= static_cast<float>(points.size());
 
        Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero();
        for (const Eigen::Vector2f& pt : points)
        {
            const Eigen::Vector2f centered = pt - mean;
            covariance += centered * centered.transpose();
        }
        covariance /= static_cast<float>(points.size());
 
        // eigenvalues are sorted in increasing order -> col(0) is the minor axis
        const Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> solver(covariance);
        const Eigen::Vector2f minorAxis = solver.eigenvectors().col(0);
 
        float minProjection = std::numeric_limits<float>::max();
        float maxProjection = std::numeric_limits<float>::lowest();
        for (const Eigen::Vector2f& pt : points)
        {
            const float projection = (pt - mean).dot(minorAxis);
            minProjection = std::min(minProjection, projection);
            maxProjection = std::max(maxProjection, projection);
        }
 
        return maxProjection - minProjection;
    }
 
    std::vector<Eigen::Vector2f>
    CableClusterFilter::regionPolygon(std::size_t numArcSamples) const
    {
        ARMARX_CHECK_GREATER_EQUAL(numArcSamples, 2);
 
        std::vector<Eigen::Vector2f> polygon;
 
        if (not angularTestValid)
        {
            return polygon;
        }
 
        polygon.reserve(2 * numArcSamples);
 
        const float centerAngle = std::atan2(outward.y(), outward.x());
 
        // generous upper bound for the outer boundary: even a ray grazing the hull
        // leaves the max-distance band before this radius
        const float maxRadius = 4 * (params.plugPosition.norm() + params.maxDistanceToHull);
 
        // The distance to a convex hull is monotonically non-decreasing along an
        // outward ray, so the largest radius whose point is still within
        // maxAllowedDistance of the hull is found by bisection. With
        // maxAllowedDistance = 0, this yields the crossing of the robot outline.
        const auto boundaryRadius =
            [this, maxRadius](const Eigen::Vector2f& direction, float maxAllowedDistance)
        {
            float lo = 0.F;
            float hi = maxRadius;
            for (int iteration = 0; iteration < 32; iteration++)
            {
                const float mid = (lo + hi) / 2;
                if (distanceToHull(params.plugPosition + mid * direction) <= maxAllowedDistance)
                {
                    lo = mid;
                }
                else
                {
                    hi = mid;
                }
            }
            return lo;
        };
 
        const auto windowDirection = [this, centerAngle](std::size_t i, std::size_t n)
        {
            const float angle =
                centerAngle - params.windowAngle / 2 +
                params.windowAngle * static_cast<float>(i) / static_cast<float>(n - 1);
            return Eigen::Vector2f(std::cos(angle), std::sin(angle));
        };
 
        // outer arc of the angular window on the max-distance boundary
        for (std::size_t i = 0; i < numArcSamples; i++)
        {
            const Eigen::Vector2f direction = windowDirection(i, numArcSamples);
            polygon.push_back(params.plugPosition +
                              boundaryRadius(direction, params.maxDistanceToHull) * direction);
        }
 
        // inner arc along the robot outline, traversed backwards to close the polygon
        for (std::size_t i = 0; i < numArcSamples; i++)
        {
            const Eigen::Vector2f direction =
                windowDirection(numArcSamples - 1 - i, numArcSamples);
            polygon.push_back(params.plugPosition + boundaryRadius(direction, 0.F) * direction);
        }
 
        return polygon;
    }
 
} // namespace armarx::navigation::components::laser_scanner_feature_extraction