ChainApproximation.cpp

Go to the documentation of this file.

#include "ChainApproximation.h"
 
#include <iterator>
#include <numeric>
 
#include <Eigen/Geometry>
#include <SimoxUtility/algorithm/apply.hpp>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include "range/v3/algorithm/find.hpp"
#include "range/v3/numeric/iota.hpp"
 
namespace armarx::navigation::algorithm
{
 
    ChainApproximation::ChainApproximation(const Points& points, const Params& params) :
        points(points), params(params)
    {
        // fill indices
        indices.resize(points.size());
        ranges::iota(indices, 0);
    }
 
    ChainApproximation::ApproximationResult
    ChainApproximation::approximate()
    {
 
        int iterations = 0;
 
        const auto maxIterConditionReached = [&]()
        {
            // inactive?
            if (params.maxIterations <= 0)
            {
                return false;
            }
 
            return iterations >= params.maxIterations;
        };
 
        while (true)
        {
            if (maxIterConditionReached())
            {
                return ApproximationResult{
                    .condition = ApproximationResult::TerminationCondition::IterationLimit,
                    .iterations = iterations};
            }
 
            if (not approximateStep())
            {
                return ApproximationResult{
                    .condition = ApproximationResult::TerminationCondition::Converged,
                    .iterations = iterations,
                    .reductionFactor = 1.F - static_cast<float>(indices.size()) /
                                                 static_cast<float>(points.size())};
            }
 
            iterations++;
        }
    }
 
    ChainApproximation::Triplets
    ChainApproximation::getTriplets() const
    {
        const int nIndices = static_cast<int>(indices.size());
 
        if (nIndices < 3)
        {
            return {};
        }
 
        Triplets triplets;
        triplets.reserve(indices.size());
 
        // Here, we iterate over all elements under consideration.
        // The aim is to create a view - a sliding window - over the
        // indices. i will always point to the centered element.
 
        // the first element
        triplets.emplace_back(indices.back(), indices.front(), indices.at(1));
 
        // intermediate elements
        for (int i = 1; i < (nIndices - 1); i++)
        {
            triplets.emplace_back(indices.at(i - 1), indices.at(i), indices.at(i + 1));
        }
 
        // the last element
        triplets.emplace_back(indices.back(), indices.front(), indices.at(1));
 
        return triplets;
    }
 
    std::vector<float>
    ChainApproximation::computeDistances(const ChainApproximation::Triplets& triplets)
    {
        std::vector<float> distances;
        distances.reserve(triplets.size());
 
        std::transform(triplets.begin(),
                       triplets.end(),
                       std::back_inserter(distances),
                       [&](const auto& triplet) { return computeDistance(triplet); });
 
        return distances;
    }
    float
    ChainApproximation::computeDistance(const ChainApproximation::Triplet& triplet) const
    {
        using Line = Eigen::ParametrizedLine<float, 2>;
 
        const Eigen::Vector2f& ptBefore = points.at(triplet.a);
        const Eigen::Vector2f& ptPivot = points.at(triplet.b);
        const Eigen::Vector2f& ptAfter = points.at(triplet.c);
 
        const auto line = Line::Through(ptBefore, ptAfter);
        return line.distance(ptPivot);
    }
 
    bool
    ChainApproximation::approximateStep()
    {
        const size_t nIndices = indices.size();
        if (nIndices <= 3)
        {
            return false;
        }
 
        const Triplets triplets = getTriplets();
        const std::vector<float> distances = computeDistances(triplets);
 
        ARMARX_CHECK_EQUAL(triplets.size(), distances.size());
        const int n = static_cast<int>(triplets.size());
 
        std::vector<int> indicesToBeRemoved;
 
        // TODO(fabian.reister): consider boundaries
        for (int i = 1; i < n - 1; i++)
        {
            const auto& distance = distances.at(i);
 
            // check distance criterion (necessary conditio)
            if (distance >= params.distanceTh)
            {
                continue;
            }
 
            // better remove this element than those left and right (sufficient condition)
            if (distance <= std::min(distances.at(i - 1), distances.at(i + 1)))
            {
                indicesToBeRemoved.emplace_back(triplets.at(i).b);
            }
        }
 
        // termination condition
        if (indicesToBeRemoved.empty())
        {
            return false;
        }
 
        const auto isMatch = [&](const int& idx) -> bool
        { return ranges::find(indicesToBeRemoved, idx) != indicesToBeRemoved.end(); };
 
        indices.erase(std::remove_if(indices.begin(), indices.end(), isMatch), indices.end());
 
        return true;
    }
    ChainApproximation::Points
    ChainApproximation::approximatedChain() const
    {
        return simox::alg::apply(indices, [&](const auto& idx) { return points.at(idx); });
    }
 
    std::ostream&
    detail::operator<<(std::ostream& str, const ApproximationResult& res)
    {
        using TerminationCondition = ApproximationResult::TerminationCondition;
 
        const std::string condStr = [&res]() -> std::string
        {
            std::string repr;
 
            switch (res.condition)
            {
                case TerminationCondition::Converged:
                    repr = "Converged";
                    break;
                case TerminationCondition::IterationLimit:
                    repr = "IterationLimit";
                    break;
            }
            return repr;
        }();
 
        str << "ApproximationResult: ["
            << "condition: " << condStr << " | "
            << "iterations: " << res.iterations << " | "
            << "reduction: " << res.reductionFactor * 100 << "%]";
 
        return str;
    }
} // namespace armarx::navigation::algorithm