CableClusterFilter.h

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
 */
 
#pragma once
 
#include <cstddef>
#include <functional>
#include <vector>
 
#include <Eigen/Core>
 
#include <VirtualRobot/MathTools.h>
 
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
 
    /**
     * @brief Detects the laser scan cluster caused by the robot's charging cable.
     *
     * The cable is plugged in at a fixed point on the robot, so its points can
     * only appear within an angular window emanating from the plug-in point,
     * stay close to the robot hull, and form a very thin cluster. The plug-in
     * point is placed inside the robot outline; the filter region is the part
     * of the angular window between the robot outline and the max-distance
     * band around it. A cluster is a cable candidate iff all of its points (in
     * the robot root frame) pass the angular window and distance-to-hull tests
     * and the cluster as a whole is thin enough.
     */
    class CableClusterFilter
    {
    public:
        struct Params
        {
            /// Plug-in point of the charging cable in the robot root frame [mm].
            /// Should lie inside the robot outline; the filter region is clipped
            /// at the outline.
            Eigen::Vector2f plugPosition = Eigen::Vector2f::Zero();
 
            /// Full opening angle [rad] of the angular window. The window has its
            /// apex at the plug-in point and is centered on the outward direction
            /// from the robot origin through the plug-in point.
            float windowAngle = 0.F;
 
            /// Maximum distance [mm] of cable points from the robot hull boundary.
            float maxDistanceToHull = 0.F;
 
            /// Maximum cluster extent [mm] along its minor principal axis.
            float maxThickness = 0.F;
        };
 
        /// Distance from a point (robot root frame) to the robot hull boundary; 0 if inside.
        using DistanceToHullFn = std::function<float(const Eigen::Vector2f&)>;
 
        CableClusterFilter(const Params& params, DistanceToHullFn distanceToHull);
 
        /// Whether the cluster fulfills all cable criteria. Points must be given
        /// in the robot root frame.
        bool isCandidate(const std::vector<Eigen::Vector2f>& clusterPoints) const;
 
        /// Extent of the point set along its minor principal axis (PCA).
        static float thickness(const std::vector<Eigen::Vector2f>& points);
 
        /// Sampled boundary polygon of the filter region (robot root frame) for
        /// visualization: the outer arc of the angular window on the max-distance
        /// boundary followed by the inner arc along the robot outline.
        std::vector<Eigen::Vector2f> regionPolygon(std::size_t numArcSamples = 24) const;
 
    private:
        Params params;
        DistanceToHullFn distanceToHull;
 
        /// Unit direction from the robot origin through the plug-in point.
        Eigen::Vector2f outward;
        /// False if the plug-in point coincides with the robot origin (no
        /// well-defined outward direction); the angular test is skipped then.
        bool angularTestValid = false;
    };
 
    /// Distance from a point to the boundary of a convex hull; 0 if the point is inside.
    float distanceToConvexHullBoundary(const Eigen::Vector2f& pt,
                                       const VirtualRobot::MathTools::ConvexHull2D& hull);
 
} // namespace armarx::navigation::components::laser_scanner_feature_extraction