Costmap.cpp

Go to the documentation of this file.

#include "Costmap.h"
 
#include <algorithm>
#include <cmath>
#include <limits>
#include <optional>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <IceUtil/Time.h>
 
#include <VirtualRobot/BoundingBox.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/XML/ObjectIO.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/algorithms/util.h>
#include <armarx/navigation/conversions/eigen.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)
        {
            return false;
        }
 
        if (index.x() >= grid.rows() || index.y() >= grid.cols())
        {
            return false;
        }
 
        if (mask.has_value())
        {
            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);
    }
 
    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();
    }
 
 
} // namespace armarx::navigation::algorithms