FeatureMerger.cpp

Go to the documentation of this file.

#include "FeatureMerger.h"
 
#include <cstddef>
#include <optional>
#include <utility>
#include <vector>
 
#include <range/v3/view/enumerate.hpp>
#include <range/v3/view/sliding.hpp>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h>
#include <armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h>
#include <armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.h>
 
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
 
 
    FeatureMerger::FeatureMerger(std::vector<Features>&& features,
                                 const ChainApproximation::Params& chainApproximationParams) :
        inputFeatures(std::move(features)), chainApproximationParams(chainApproximationParams)
    {
    }
 
    FeatureMerger::FeatureMerger(const std::vector<Features>& features,
                                 const ChainApproximation::Params& chainApproximationParams) :
        inputFeatures(features), chainApproximationParams(chainApproximationParams)
    {
    }
 
    std::vector<Features>
    FeatureMerger::merge()
    {
        ARMARX_DEBUG << "Starting merge of " << inputFeatures.size() << " features";
        UnionFind uf{inputFeatures.size()};
 
        for (const auto [i, f] : ranges::views::enumerate(inputFeatures))
        {
            std::optional<std::size_t> lastFeature = std::nullopt;
            for (const auto& p : f.points)
            {
                if (auto res = checkPoint(p, i, lastFeature); res)
                {
                    uf.unite(i, res.value());
                    lastFeature = res.value();
                }
            }
        }
 
        for (const auto [i, f] : ranges::views::enumerate(inputFeatures))
        {
            std::size_t representative = uf.find(i);
            if (i != representative)
            {
                ARMARX_DEBUG << "add feature " << i << " to feature " << representative;
                addToFeature(inputFeatures[representative], f);
            }
        }
 
        std::vector<Features> merged;
        for (const auto [i, f] : ranges::views::enumerate(inputFeatures))
        {
            if (i == uf.find(i))
            {
                ARMARX_DEBUG << "feature " << i << " is representative";
                merged.push_back(f);
            }
        }
        // recalculate convex hull etc. for all merged features
        recalculateFeatures(merged);
 
        ARMARX_DEBUG << "Merged " << inputFeatures.size() << " features into " << merged.size();
        return merged;
    }
 
    bool
    FeatureMerger::insideConvexPoly(const Eigen::Vector2f& p,
                                    const std::vector<Eigen::Vector2f>& vertices)
    {
        // algorithm to check whether a point is inside a convex polygon
        // see https://stackoverflow.com/questions/1119627/how-to-test-if-a-point-is-inside-of-a-convex-polygon-in-2d-integer-coordinates
 
        const auto signum = [](float x)
        {
            if (x == 0)
                return 0;
            return x > 0 ? 1 : -1;
        };
 
        const auto checkSide = [&](const Eigen::Vector2f& a, const Eigen::Vector2f& b)
        { return signum((p.x() - a.x()) * (b.y() - a.y()) - (p.y() - a.y()) * (b.x() - a.x())); };
 
        float previousSide = 0;
        for (const auto edge : vertices | ranges::views::sliding(2))
        {
            int d = checkSide(edge[0], edge[1]);
            if (d == 0)
                return true;
 
            if (previousSide != 0 && d != previousSide)
            {
                return false;
            }
            previousSide = d;
        }
        int d = checkSide(vertices.back(), vertices.front());
        if (d == 0)
            return true;
        return d == previousSide;
    }
 
    std::optional<std::size_t>
    FeatureMerger::checkPoint(const Eigen::Vector2f p,
                              std::size_t ignore,
                              const std::optional<std::size_t> checkFirst)
    {
        ARMARX_TRACE;
        // when try first is given check this feature before all others
        if (checkFirst)
        {
 
            ARMARX_TRACE;
            ARMARX_CHECK_LESS(checkFirst.value(), inputFeatures.size());
            const auto convexHullOpt = inputFeatures[checkFirst.value()].convexHull;
 
            if (convexHullOpt.has_value() and insideConvexPoly(p, convexHullOpt->vertices))
            {
                return checkFirst.value();
            }
        }
 
        for (const auto [i, f] : ranges::views::enumerate(inputFeatures))
        {
            ARMARX_TRACE;
            if (i == ignore || (checkFirst && i == checkFirst.value()))
            {
                continue;
            }
 
            const auto convexHullOpt = f.convexHull;
 
            if (convexHullOpt.has_value() and insideConvexPoly(p, convexHullOpt->vertices))
            {
                return i;
            }
        }
        return std::nullopt;
    }
 
    void
    FeatureMerger::addToFeature(Features& f, const Features& toAdd)
    {
        f.points.insert(f.points.end(), toAdd.points.begin(), toAdd.points.end());
 
        f.convexHull = std::nullopt;
        f.circle = std::nullopt;
        f.ellipsoid = std::nullopt;
        f.chain = std::nullopt;
    }
 
    void
    FeatureMerger::recalculateFeatures(std::vector<Features>& features)
    {
        ARMARX_TRACE;
        for (auto& f : features)
        {
            if (!f.convexHull)
            {
                f.convexHull = FeatureExtractor::convexHull(f.points);
            }
 
            if (!f.circle)
            {
                f.circle = FeatureExtractor::circle(f.points);
            }
 
            // if (!f.ellipsoid)
            // {
            //    f.ellipsoid = FeatureExtractor::ellipsoid(f.convexHull);
            // }
 
            if (!f.chain)
            {
                f.chain = FeatureExtractor::chainApproximation(
                    f.points, f.convexHull, chainApproximationParams);
            }
        }
    }
 
 
} // namespace armarx::navigation::components::laser_scanner_feature_extraction