Costmap.cpp

Go to the documentation of this file.

#include "Costmap.h"
 
#include <math.h>
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <limits>
#include <optional>
#include <unordered_set>
#include <utility>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <range/v3/view/zip.hpp>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/core/UserException.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/core/basic_types.h>
 
namespace armarx::navigation::algorithms
{
 
 
    Costmap::Costmap(const Grid& grid,
                     const Parameters& parameters,
                     const SceneBounds& sceneBounds,
                     const std::optional<Mask>& mask,
                     const core::Pose2D& origin) :
        grid(grid),
        mask(mask),
        sceneBounds(sceneBounds),
        parameters(parameters),
        global_T_costmap(origin)
    {
        validateSizes();
    }
 
    void
    Costmap::validateSizes() const
    {
        if (mask.has_value())
        {
            ARMARX_VERBOSE << VAROUT(grid.rows());
            ARMARX_VERBOSE << VAROUT(grid.cols());
            ARMARX_VERBOSE << VAROUT(mask->rows());
            ARMARX_VERBOSE << VAROUT(mask->cols());
 
            ARMARX_CHECK_EQUAL(grid.rows(), mask->rows());
            ARMARX_CHECK_EQUAL(grid.cols(), mask->cols());
        }
    }
 
    Costmap::Position
    Costmap::toPositionLocal(const Index& index) const
    {
        ARMARX_TRACE;
 
        Eigen::Vector2f posLocal;
        posLocal.x() =
            sceneBounds.min.x() + index.x() * parameters.cellSize + parameters.cellSize / 2;
        posLocal.y() =
            sceneBounds.min.y() + index.y() * parameters.cellSize + parameters.cellSize / 2;
 
        return posLocal;
    }
 
    Costmap::Position
    Costmap::toPositionGlobal(const Costmap::Index& index) const
    {
        const Costmap::Position costmap_P_pos = toPositionLocal(index);
        return global_T_costmap * costmap_P_pos;
    }
 
    bool
    Costmap::isValid(const Costmap::Index& index) const noexcept
    {
 
        if (index.x() < 0 || index.y() < 0)
        {
            ARMARX_VERBOSE << "Index out of bounds (< 0)";
            return false;
        }
 
        if (index.x() >= grid.rows() || index.y() >= grid.cols())
        {
            ARMARX_VERBOSE << "Index out of bounds (>= grid size)";
            return false;
        }
 
        if (mask.has_value())
        {
            if (grid.rows() != mask->rows() or grid.cols() != mask->cols())
            {
                ARMARX_ERROR << "Grid and mask have different dimensions!";
                return false;
            }
 
            if (not mask.value()(index.x(), index.y()))
            {
                ARMARX_DEBUG << "Index masked out (mask)";
            }
            return mask.value()(index.x(), index.y());
        }
 
        return true;
    }
 
    std::pair<int, int>
    Costmap::toIndexUnsanitized(const Eigen::Vector2f& globalPosition) const
    {
        const auto localPosition = global_T_costmap.inverse() * globalPosition;
 
        const float vX = (localPosition.x() - parameters.cellSize / 2 - sceneBounds.min.x()) /
                         parameters.cellSize;
        const float vY = (localPosition.y() - parameters.cellSize / 2 - sceneBounds.min.y()) /
                         parameters.cellSize;
 
        const int iX = std::round(vX - 0.01);
        const int iY = std::round(vY - 0.01);
 
        return {iX, iY};
    }
 
    std::optional<Costmap::Vertex>
    Costmap::toVertexOrInvalid(const Eigen::Vector2f& globalPosition) const
    {
        const auto [iX, iY] = toIndexUnsanitized(globalPosition);
 
        if (iX < 0 || iY < 0 || iX >= grid.rows() || iY >= grid.cols())
        {
            // calculated vertex outside costmap
            return std::nullopt;
        }
 
        return Vertex{.index = Index{iX, iY}, .position = globalPosition};
    }
 
    Costmap::Vertex
    Costmap::toVertex(const Eigen::Vector2f& globalPosition) const
    {
        const auto [iX, iY] = toIndexUnsanitized(globalPosition);
 
        const int iXSan = std::clamp<int>(iX, 0, grid.rows() - 1);
        const int iYSan = std::clamp<int>(iY, 0, grid.cols() - 1);
 
        // FIXME accept one cell off
 
        // ARMARX_CHECK_GREATER(iX, 0);
        // ARMARX_CHECK_GREATER(iY, 0);
 
        // ARMARX_CHECK_LESS_EQUAL(iX, grid.rows() - 1);
        // ARMARX_CHECK_LESS_EQUAL(iY, grid.cols() - 1);
 
        return Vertex{.index = Index{iXSan, iYSan}, .position = globalPosition};
    }
 
    const SceneBounds&
    Costmap::getLocalSceneBounds() const noexcept
    {
        return sceneBounds;
    }
 
    Costmap::Optimum
    Costmap::optimum() const
    {
        ARMARX_TRACE;
 
        if (mask.has_value())
        {
            ARMARX_CHECK_GREATER(mask->array().sum(), 0)
                << "At least one element has to be valid. Here, all elements are masked out!";
        }
 
        // index of the min element
        Grid::Index row = 0;
        Grid::Index col = 0;
 
        // value of the min element
        float minVal = std::numeric_limits<float>::max();
 
        for (int r = 0; r < grid.rows(); r++)
        {
            for (int c = 0; c < grid.cols(); c++)
            {
                if (mask.has_value())
                {
                    if (not mask.value()(r, c)) // skip invalid cells
                    {
                        continue;
                    }
 
                    const float currentVal = grid(r, c);
                    if (currentVal < minVal)
                    {
                        minVal = currentVal;
                        row = r;
                        col = c;
                    }
                }
            }
        }
 
        if (minVal == std::numeric_limits<float>::max())
        {
            return {.value = -1, .index = {-1, -1}, .position = {-1, -1}};
        };
 
        return {.value = minVal, .index = {row, col}, .position = toPositionGlobal({row, col})};
    }
 
    void
    Costmap::forEachCell(const std::function<void(const Index&)>& f) const
    {
        for (int r = 0; r < grid.rows(); r++)
        {
            for (int c = 0; c < grid.cols(); c++)
            {
                f({r, c});
            }
        }
    }
 
    void
    Costmap::forEachCellParallel(const std::function<void(const Index&)>& f) const
    {
#pragma omp parallel for schedule(static)
        for (int r = 0; r < grid.rows(); r++)
        {
            for (int c = 0; c < grid.cols(); c++)
            {
                f({r, c});
            }
        }
    }
 
    void
    Costmap::filter(const Filter& filter)
    {
        ARMARX_TRACE;
 
        ARMARX_CHECK(filter.matrix.rows() % 2 == 1 and filter.matrix.cols() % 2 == 1);
 
        if (mask.has_value())
        {
            ARMARX_CHECK_GREATER(mask->array().sum(), 0)
                << "At least one element has to be valid. Here, all elements are masked out!";
        }
 
        Grid newGrid(grid.rows(), grid.cols());
        const double max = grid.maxCoeff();
        newGrid.setConstant(max);
        for (int r = 0; r < grid.rows(); r++)
        {
            for (int c = 0; c < grid.cols(); c++)
            {
                if (mask.has_value())
                {
                    if (not mask.value()(r, c)) // skip invalid cells
                    {
                        continue;
                    }
 
                    Eigen::MatrixXf valueMatrix(filter.matrix.rows(), filter.matrix.cols());
                    const int radius_r = (filter.matrix.rows() - 1) / 2;
                    const int radius_c = (filter.matrix.cols() - 1) / 2;
                    valueMatrix.setConstant(max);
                    for (int i = -radius_r; i <= radius_r; i++)
                    {
                        for (int j = -radius_c; j <= radius_c; j++)
                        {
                            const int ri = r + i;
                            const int cj = c + j;
                            if (ri > 0 and ri < grid.rows() and cj > 0 and cj < grid.cols() and
                                mask.value()(ri, cj))
                            {
                                valueMatrix(radius_r + i, radius_c + j) = grid(ri, cj);
                            }
                        }
                    }
                    const double sum = filter.matrix.sum();
                    if (sum > 0)
                    {
                        const float filteredValue =
                            (valueMatrix.cwiseProduct(filter.matrix)).sum() / sum;
                        newGrid(r, c) =
                            filter.useMinimum ? std::min(grid(r, c), filteredValue) : filteredValue;
                    }
                }
            }
        }
        grid = newGrid;
    }
 
    const Costmap::Parameters&
    Costmap::params() const noexcept
    {
        return parameters;
    }
 
    const Costmap::Grid&
    Costmap::getGrid() const
    {
        return grid;
    }
 
    bool
    Costmap::isInCollision(const Position& p) const
    {
        const auto v = toVertex(p);
        return grid(v.index.x(), v.index.y()) == 0.F;
    }
 
    bool
    Costmap::addCostmapWithSameSizeAndPose(const Costmap& other, const AddMode mode)
    {
        ARMARX_TRACE;
        validateSizes();
        other.validateSizes();
        ARMARX_CHECK_EQUAL(grid.rows(), other.grid.rows());
        ARMARX_CHECK_EQUAL(grid.cols(), other.grid.cols());
 
        ARMARX_CHECK_EQUAL(global_T_costmap.matrix(), other.global_T_costmap.matrix());
 
        // add the costs of the other grid to this one by either min or max operator
        switch (mode)
        {
            case AddMode::MIN:
                grid.array() = other.grid.array().cwiseMin(grid.array());
                break;
 
            case AddMode::MAX:
                grid.array() = other.grid.array().cwiseMax(grid.array());
                break;
        }
 
 
        if (not mask.has_value())
        {
            mask = Mask(grid.rows(), grid.cols());
            mask->setOnes();
        }
 
        Mask otherMask(grid.rows(), grid.cols());
        otherMask.setOnes();
 
        if (other.mask.has_value())
        {
            ARMARX_TRACE;
 
            // union of masks
            ARMARX_DEBUG << "Union of masks";
            otherMask.array() *= other.mask->array();
        }
 
        ARMARX_TRACE;
        mask->array() *= otherMask.array();
 
        ARMARX_VERBOSE << "Fraction of valid cells: "
                       << mask->array().cast<float>().sum() / mask->size();
 
        // apply mask to grid => all invalid points will be 0
        ARMARX_TRACE;
        grid.array() *= mask->cast<float>().array();
 
        return true;
    }
 
    bool
    Costmap::add(const Costmap& other, const float weight)
    {
        // FIXME: This function is broken when global_T_costmap is not an identity matrix
        // Explanation:
        // - If both costmap have different rotation, just iterating the grids is not possible
        // - If both have the same rotation, then we would need a more clever approach to determine
        //   the regions where we can combine the values.
        // ensure that both grid and mask are the same size
 
        ARMARX_TRACE;
 
        validateSizes();
        other.validateSizes();
 
        const auto startIdx = toVertex(other.sceneBounds.min);
 
        // merge other grid into this one => use max costs
 
        // It might happen that this costmap is not large enough to cover the other costmap
        // In this case, only add the part to this costmap that is possible.
        const int rows = std::min(other.grid.rows(), grid.rows() - startIdx.index.x());
        const int cols = std::min(other.grid.cols(), grid.cols() - startIdx.index.y());
 
        ARMARX_VERBOSE << "Adding other grid to region (" << startIdx.index.x() << ", "
                       << startIdx.index.y() << "), " << " (" << startIdx.index.x() + rows << ", "
                       << startIdx.index.y() + cols << ")";
 
        ARMARX_TRACE;
 
        // add the costs of the other mask to this one by a weighting factor
        grid.block(startIdx.index.x(), startIdx.index.y(), rows, cols).array() +=
            weight * other.grid.block(0, 0, rows, cols).array();
 
        if (not mask.has_value())
        {
            mask = Mask(grid.rows(), grid.cols());
            mask->setOnes();
        }
 
        ARMARX_VERBOSE << "Fraction of valid cells before merge: "
                       << mask->array().cast<float>().sum() / mask->size();
 
        Mask otherMask(grid.rows(), grid.cols());
        otherMask.setZero();
        otherMask.block(startIdx.index.x(), startIdx.index.y(), rows, cols).setOnes();
 
        if (other.mask.has_value())
        {
            ARMARX_TRACE;
 
            // union of masks
            ARMARX_DEBUG << "Union of masks";
            otherMask.block(startIdx.index.x(), startIdx.index.y(), rows, cols).array() *=
                other.mask->block(0, 0, rows, cols).array();
        }
 
        ARMARX_TRACE;
        mask->array() *= otherMask.array();
 
        ARMARX_VERBOSE << "Fraction of valid cells: "
                       << mask->array().cast<float>().sum() / mask->size();
 
        // apply mask to grid => all invalid points will be 0
        ARMARX_TRACE;
        grid.array() *= mask->cast<float>().array();
 
        return true;
    }
 
    bool
    Costmap::add(const Costmap& other, const AddMode mode, const bool keepThis)
    {
        ARMARX_TRACE;
        validateSizes();
        other.validateSizes();
 
        // General implementation: The costmaps can be rotated differently and have different origins
        // Thus, we need to find the closest corresponding entry for each entry of the other costmap
 
        if (not mask.has_value())
        {
            mask = Mask(grid.rows(), grid.cols());
            mask->setOnes();
        }
        Mask otherMask;
        if (other.mask.has_value())
        {
            otherMask = other.mask.value();
        }
        else
        {
            otherMask = Mask(other.grid.rows(), other.grid.cols());
            otherMask.setOnes();
        }
 
        this->forEachCell(
            [&](const Index& thisIdx)
            {
                const Position pos = this->toPositionGlobal(thisIdx);
                const auto otherVOpt = other.toVertexOrInvalid(pos);
 
                // Caution: the behavior for masked out cells may be different to previous implementation
                // Especially masked out cells were not merged in some implementations
                if (not otherVOpt.has_value())
                {
                    // position not in other costmap
                    if (not keepThis)
                    {
                        (*this->mask)(thisIdx.x(), thisIdx.y()) = false;
                    }
                    return;
                }
 
                const Index otherIdx = otherVOpt->index;
 
                // merge masks
                if (other.mask.has_value())
                {
                    (*this->mask)(thisIdx.x(), thisIdx.y()) &=
                        other.mask->operator()(otherIdx.x(), otherIdx.y());
                }
 
                // add the costs of the other grid to this one by either min or max operator
                switch (mode)
                {
                    case AddMode::MIN:
                        this->grid(thisIdx.x(), thisIdx.y()) =
                            std::min(other.grid(otherIdx.x(), otherIdx.y()),
                                     this->grid(thisIdx.x(), thisIdx.y()));
                        break;
                    case AddMode::MAX:
                        this->grid(thisIdx.x(), thisIdx.y()) =
                            std::max(other.grid(otherIdx.x(), otherIdx.y()),
                                     this->grid(thisIdx.x(), thisIdx.y()));
                        break;
                    case AddMode::MULTIPLY:
                        this->grid(thisIdx.x(), thisIdx.y()) *=
                            other.grid(otherIdx.x(), otherIdx.y());
                        break;
                    default:
                        throw armarx::InvalidArgumentException{
                            "AddMode is not handled in Costmap::add(...)"};
                }
            });
 
        // apply mask to grid => all invalid points will be 0
        ARMARX_TRACE;
        grid.array() *= mask->cast<float>().array();
 
        return true;
    }
 
    const std::optional<Costmap::Mask>&
    Costmap::getMask() const noexcept
    {
        return mask;
    }
 
    std::optional<Costmap::Mask>&
    Costmap::getMutableMask() noexcept
    {
        return mask;
    }
 
    std::optional<float>
    Costmap::value(const Costmap::Index& index) const
    {
        if (not isValid(index))
        {
            ARMARX_IMPORTANT << "Requested index " << index << " but it is masked out!";
            return std::nullopt;
        }
 
        return grid(index.x(), index.y());
    }
 
    std::optional<float>
    Costmap::value(const Position& position) const
    {
        ARMARX_DEBUG << "value ...";
 
        const auto v = toVertex(position);
        return value(v.index);
    }
 
    void
    Costmap::cutToValidBoundingBox()
    {
        ARMARX_TRACE;
 
        const auto [min, max] = getValidBoundingBox();
 
        // calculate new scene bounds based on bounding box
        const auto newSceneBounds = SceneBounds{
            .min = sceneBounds.min + parameters.cellSize * Eigen::Vector2f{min.cast<float>()},
            .max = sceneBounds.min + parameters.cellSize * Eigen::Vector2f{max.cast<float>()}};
 
        ARMARX_VERBOSE << "Cutting grid to valid bounding box: " << min << " and " << max;
 
        const auto newGrid =
            grid.block(min.x(), min.y(), max.x() - min.x() + 1, max.y() - min.y() + 1);
        grid = newGrid;
 
        if (mask.has_value())
        {
            ARMARX_VERBOSE << "Cutting mask to valid bounding box: " << min << " and " << max;
 
            const auto newMask =
                mask->block(min.x(), min.y(), max.x() - min.x() + 1, max.y() - min.y() + 1);
            mask = newMask;
        }
 
        // update scene bounds
        sceneBounds.min = newSceneBounds.min;
        sceneBounds.max = newSceneBounds.max;
 
        ARMARX_VERBOSE << "New scene bounds: " << sceneBounds.min << " and " << sceneBounds.max;
    }
 
    Costmap
    Costmap::mergeInto(const std::vector<Costmap>& costmaps,
                       const std::vector<float>& weights) const
    {
        ARMARX_CHECK_EQUAL(costmaps.size() + 1, weights.size());
 
        const std::vector<float> costmapWeights(weights.begin(), weights.end() - 1);
        ARMARX_CHECK_EQUAL(costmapWeights.size(), costmaps.size());
 
        ARMARX_VERBOSE << "Merging into with weights " << weights;
 
        // create copy of this costmap
        Costmap mergedCostmap = *this;
 
        ARMARX_CHECK(mergedCostmap.mask.has_value());
 
        // we iterate over all cells of THIS costmap. For each position, we perform a lookup in all other
        // costmaps and merge the values according to the weights.
        for (int x = 0; x < mergedCostmap.getGrid().rows(); x++)
        {
            for (int y = 0; y < mergedCostmap.getGrid().cols(); y++)
            {
                ARMARX_TRACE;
                const Costmap::Position position = mergedCostmap.toPositionGlobal(Index{x, y});
 
                // merge masks
                for (const auto& costmap : costmaps)
                {
                    ARMARX_TRACE;
                    mergedCostmap.mask.value()(x, y) &=
                        costmap.isValid(costmap.toVertex(position).index);
                }
 
                if (mergedCostmap.isValid(Index{x, y}))
                {
                    float newVal = 0;
                    for (const auto& [weight, costmap] :
                         ranges::views::zip(costmapWeights, costmaps))
                    {
                        const auto otherCostmapVal = costmap.value(position);
                        ARMARX_CHECK(otherCostmapVal.has_value());
 
                        newVal += weight * otherCostmapVal.value();
                    }
 
                    newVal += weights.back() * mergedCostmap.grid(x, y);
                    mergedCostmap.grid(x, y) = newVal;
                }
                else
                {
                    mergedCostmap.grid(x, y) = 0;
                }
            }
        }
 
        return mergedCostmap;
    }
 
    Costmap::Filter
    Costmap::Filter::gaussian(int radius, float sigma)
    {
        ARMARX_CHECK(radius > 0 and sigma > 0.);
        Filter gaussianFilter{.matrix = Eigen::MatrixXf::Zero(radius * 2 + 1, radius * 2 + 1)};
        for (int i = -radius; i <= radius; i++)
        {
            for (int j = -radius; j <= radius; j++)
            {
                gaussianFilter.matrix(radius + i, radius + j) =
                    std::exp(-0.5 * (std::pow(i / sigma, 2.0) + std::pow(j / sigma, 2.0))) /
                    (2 * M_PI * sigma * sigma);
            }
        }
        return gaussianFilter;
    }
 
    std::size_t
    Costmap::numberOfValidElements() const
    {
        if (not mask.has_value())
        {
            return grid.cols() * grid.rows();
        }
 
        return mask->array().cast<int>().sum();
    }
 
    Costmap::Index
    Costmap::findClosestValidIndex(const Costmap::Index& index, int radius)
    {
        ARMARX_TRACE;
 
        if (isValid(index))
        {
            return index;
        }
 
        const auto x = index.x();
        const auto y = index.y();
 
        // Check all cells in a radius around the given index
        for (int r = 1; r < radius; r++)
        {
            for (int i = -r; i <= r; i++)
            {
                for (int j = -r; j <= r; j++)
                {
                    const auto idx = Costmap::Index{x + i, y + j};
                    if (isValid(idx))
                    {
                        return idx;
                    }
                }
            }
        }
 
        ARMARX_ERROR << "No valid index found in the vicinity of " << index;
        return index;
    }
 
    std::pair<Costmap::Index, Costmap::Index>
    Costmap::getValidBoundingBox() const
    {
        if (not mask.has_value())
        {
            return {Index{0, 0}, Index{grid.rows() - 1, grid.cols() - 1}};
        }
 
        ARMARX_VERBOSE << "Fraction of valid elements: "
                       << mask->cast<float>().sum() / mask->size();
 
        ARMARX_TRACE;
 
        Index min = Index{grid.rows(), grid.cols()};
        Index max = Index{0, 0};
 
        for (int x = 0; x < grid.rows(); x++)
        {
            for (int y = 0; y < grid.cols(); y++)
            {
                if (mask.value()(x, y))
                {
                    min.x() = std::min(min.x(), x);
                    min.y() = std::min(min.y(), y);
                    max.x() = std::max(max.x(), x);
                    max.y() = std::max(max.y(), y);
                }
            }
        }
 
        return {min, max};
    }
 
    void
    Costmap::excludeUnreachable(const Position& position)
    {
        ARMARX_TRACE;
 
        // Exclude all cells that are not reachable from the given position
        // That is, they are not reachable over cells with a value > 0
        // This is done by a simple flood fill algorithm
 
        const auto v = toVertex(position);
        auto startIdx = v.index;
 
        // Find a valid start index
        const auto [aabbMin, aabbMax] = getValidBoundingBox();
        startIdx.x() = std::max(startIdx.x(), aabbMin.x());
        startIdx.y() = std::max(startIdx.y(), aabbMin.y());
        startIdx.x() = std::min(startIdx.x(), aabbMax.x());
        startIdx.y() = std::min(startIdx.y(), aabbMax.y());
 
        startIdx = findClosestValidIndex(startIdx);
 
        // TODO: Good start index if the position is not valid
 
        // Check validity before resetting mask
        ARMARX_INFO << "Start index: " << startIdx;
        ARMARX_CHECK(isValid(startIdx));
 
        // Reset mask
        grid.array() *= mask->cast<float>().array();
        auto newMask = mask.value();
        newMask.setZero();
 
        struct pair_hash
        {
            inline std::size_t
            operator()(const std::pair<int, int>& v) const noexcept
            {
                return v.first * 31 + v.second;
            }
        };
 
        std::unordered_set<std::pair<int, int>, pair_hash> stack;
        auto visited = newMask;
        stack.emplace(startIdx.x(), startIdx.y());
        int i = 0;
        while (not stack.empty())
        {
            if (i++ > 1000)
            {
                ARMARX_INFO << "After 1000 iterations, current element is " << stack.begin()->first
                            << ", " << stack.begin()->second;
                i = 0;
            }
            const auto it = stack.begin();
            const auto idx = Index{it->first, it->second};
            stack.erase(it);
 
            // print all elements
            /*for (const auto& element : stack)
            {
                ARMARX_INFO << "Stack element: " << element.first << ", " << element.second;
            }*/
 
            visited(idx.x(), idx.y()) = 1;
 
            //ARMARX_INFO << "Checking " << idx;
 
            if (not isValid(idx))
            {
                continue;
            }
 
            if (grid(idx.x(), idx.y()) == 0.F)
            {
                continue;
            }
 
            //ARMARX_INFO << "Including " << idx;
 
            newMask.array()(idx.x(), idx.y()) = 1;
 
            if (const std::pair<int, int> element = {idx.x() + 1, idx.y()};
                element.first < visited.rows() && not visited(element.first, element.second))
            {
                stack.emplace(element);
            }
            if (const std::pair<int, int> element = {idx.x() - 1, idx.y()};
                element.first > 0 && not visited(element.first, element.second))
            {
                stack.emplace(element);
            }
            if (const std::pair<int, int> element = {idx.x(), idx.y() + 1};
                element.second < visited.cols() && not visited(element.first, element.second))
            {
                stack.emplace(element);
            }
            if (const std::pair<int, int> element = {idx.x(), idx.y() - 1};
                element.second > 0 && not visited(element.first, element.second))
            {
                stack.emplace(element);
            }
        }
 
        mask.emplace(newMask);
    }
 
    Costmap
    Costmap::WithSameDimsAs(const Costmap& other, const Grid& grid, const std::optional<Mask>& mask)
    {
        return {grid, other.params(), other.getLocalSceneBounds(), mask, other.origin()};
    }
 
    std::optional<Costmap::Vertex>
    Costmap::findClosestCollisionFreeVertex(const Position& position, float maxDistance) const
    {
        ARMARX_TRACE;
 
        // Already collision-free?
        if (!isInCollision(position))
        {
            return toVertex(position);
        }
 
        const Vertex startVertex = toVertex(position);
        const int maxRadiusCells = static_cast<int>(std::ceil(maxDistance / parameters.cellSize));
 
        // Search in expanding circles
        for (int radius = 1; radius <= maxRadiusCells; ++radius)
        {
            std::optional<Vertex> bestVertex;
            float bestDistanceSq = std::numeric_limits<float>::max();
 
            // Check perimeter of current radius (optimization)
            for (int i = -radius; i <= radius; ++i)
            {
                for (int j = -radius; j <= radius; ++j)
                {
                    // Only check perimeter cells
                    if (std::abs(i) != radius && std::abs(j) != radius)
                    {
                        continue;
                    }
 
                    const Index idx{startVertex.index.x() + i, startVertex.index.y() + j};
 
                    // Check bounds and validity
                    if (!isValid(idx))
                    {
                        continue;
                    }
 
                    // Check if collision-free (grid value > 0)
                    if (grid(idx.x(), idx.y()) > 0.F)
                    {
                        const Position candidatePos = toPositionGlobal(idx);
                        const float distSq = (candidatePos - position).squaredNorm();
 
                        if (distSq < bestDistanceSq)
                        {
                            bestDistanceSq = distSq;
                            bestVertex = Vertex{.index = idx, .position = candidatePos};
                        }
                    }
                }
            }
 
            // Found at thisradius? Return it (closest will be at smallest radius)
            if (bestVertex)
            {
                return bestVertex;
            }
        }
 
        // No collision-free vertex found
        return std::nullopt;
    }
 
} // namespace armarx::navigation::algorithms