ScanClustering.cpp

Go to the documentation of this file.

#include "ScanClustering.h"
 
#include <cmath>
#include <vector>
 
#include <RobotAPI/interface/units/LaserScannerUnit.h>
 
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
    ScanClustering::ScanClustering(const Params& params) : params(params)
    {
    }
 
    bool
    ScanClustering::add(const LaserScanStep& scanStep)
    {
        if (scan.empty())
        {
            scan.push_back(scanStep);
            return true;
        }
 
        if (not supports(scanStep))
        {
            return false;
        }
 
        scan.push_back(scanStep);
        return true;
    }
 
    std::vector<LaserScan>
    ScanClustering::detectClusters(const LaserScan& scan)
    {
        const auto isInvalid = [&](const LaserScanStep& step) -> bool
        { return step.distance > params.maxDistance; };
 
        std::vector<LaserScan> clusters;
        for (const auto& laserScanStep : scan)
        {
            if (isInvalid(laserScanStep))
            {
                continue;
            }
 
            // cluster finished
            if (not add(laserScanStep))
            {
                if (not cluster().empty())
                {
                    clusters.push_back(cluster());
                    clear();
 
                    // work on a new cluster
                    add(laserScanStep);
                }
            }
        }
 
        // need to finish last open cluster?
        if (not cluster().empty())
        {
            clusters.push_back(cluster());
            clear();
        }
 
        return clusters;
    }
 
    const LaserScan&
    ScanClustering::cluster() const
    {
        return scan;
    }
 
    void
    ScanClustering::clear()
    {
        scan.clear();
    }
 
    bool
    ScanClustering::supports(const LaserScanStep& scanStep)
    {
        // OK to create a new cluster if it's empty
        if (scan.empty())
        {
            return true;
        }
 
        const float distanceDiff = std::fabs(scanStep.distance - scan.back().distance);
        const bool isWithinDistanceRange = distanceDiff < params.distanceThreshold;
 
        const float angleDiff = std::fabs(scanStep.angle - scan.back().angle);
        const bool isWithinAngleRange = angleDiff < params.angleThreshold;
 
        return (isWithinDistanceRange and isWithinAngleRange);
    }
} // namespace armarx::navigation::components::laser_scanner_feature_extraction