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 <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/core/basic_types.h>
 
#include <range/v3/view/zip.hpp>
 
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_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 (not mask.value()(index.x(), index.y()))
            {
                ARMARX_VERBOSE << "Index out of bounds (mask)";
            }
            return mask.value()(index.x(), index.y());
        }
 
        return true;
    }
 
    Costmap::Vertex
    Costmap::toVertex(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);
 
        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;
                    }
                }
            }
        }
 
        return {.value = minVal, .index = {row, col}, .position = toPositionGlobal({row, col})};
    }
 
    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::add(const Costmap& other, const float weight)
    {
        // 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)
    {
        // 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 grid to this one by either min or max operator
        switch (mode)
        {
            case AddMode::MIN:
                grid.block(startIdx.index.x(), startIdx.index.y(), rows, cols).array() =
                    other.grid.block(0, 0, rows, cols)
                        .array()
                        .cwiseMin(
                            grid.block(startIdx.index.x(), startIdx.index.y(), rows, cols).array());
                break;
 
            case AddMode::MAX:
                grid.block(startIdx.index.x(), startIdx.index.y(), rows, cols).array() =
                    other.grid.block(0, 0, rows, cols)
                        .array()
                        .cwiseMax(
                            grid.block(startIdx.index.x(), startIdx.index.y(), rows, cols).array());
                break;
        }
 
 
        if (not mask.has_value())
        {
            mask = Mask(grid.rows(), grid.cols());
            mask->setOnes();
        }
 
        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;
    }
 
    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;
    }
 
    /**
    * @brief Merge a list of costmaps into this costmap
    * 
    * @param costmaps list of other costmaps
    * @param weights [weights of other costmaps..., weights of this costmap]
    * @return this 
    */
    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;
 
        Costmap mergedCostmap = *this;
 
        ARMARX_CHECK(mergedCostmap.mask.has_value());
 
        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)
    {
        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 < 10; 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
            {
                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);
    }
 
 
} // namespace armarx::navigation::algorithms