Costmap3D.cpp

Go to the documentation of this file.

#include "Costmap3D.h"
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <limits>
#include <optional>
#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/util/CPPUtility/trace.h>
 
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/core/basic_types.h>
 
namespace armarx::navigation::algorithms::orientation_aware
{
 
 
    Costmap3D::Costmap3D(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_Costmap3D(origin)
    {
        validateSizes();
    }
 
    void
    Costmap3D::validateSizes() const
    {
        // Check that the costmap for all orientations has the same size
        if (grid.size() > 0)
        {
            const auto size = getSize();
            auto rows = size.x();
            auto columns = size.y();
            // In case there is a mask, also check if that has the same size
            if (mask.has_value())
            {
                rows = mask->rows();
                columns = mask->cols();
            }
 
            for (const auto& g : grid)
            {
                ARMARX_CHECK_EQUAL(g.rows(), rows);
                ARMARX_CHECK_EQUAL(g.cols(), columns);
            }
        }
    }
 
    Costmap3D::Position
    Costmap3D::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;
    }
 
    Costmap3D::Position
    Costmap3D::toPositionGlobal(const Costmap3D::Index& index) const
    {
        const Costmap3D::Position Costmap3D_P_pos = toPositionLocal(index);
        return global_T_Costmap3D * Costmap3D_P_pos;
    }
 
    bool
    Costmap3D::isValid(const Costmap3D::Index& index) const noexcept
    {
        if (not isWithinRange(index))
        {
            return false;
        }
 
        if (mask.has_value())
        {
            if (not mask.value()(index.x(), index.y()))
            {
                ARMARX_DEBUG << "Index masked out (mask)";
            }
            return mask.value()(index.x(), index.y());
        }
 
        return true;
    }
 
    bool
    Costmap3D::isWithinRange(const Costmap3D::Index& index) const noexcept
    {
        if (index.x() < 0 || index.y() < 0)
        {
            ARMARX_VERBOSE << "Index out of bounds (< 0)";
            return false;
        }
 
        if (index.x() >= getSize().x() || index.y() >= getSize().y())
        {
            ARMARX_VERBOSE << "Index out of bounds (>= grid size)";
            return false;
        }
        return true;
    }
 
    bool
    Costmap3D::isMaskedOut(const Costmap3D::Index& index) const noexcept
    {
        if (mask.has_value())
        {
            if (not mask.value()(index.x(), index.y()))
            {
                ARMARX_DEBUG << "Index masked out (mask)";
            }
            return not mask.value()(index.x(), index.y());
        }
        return false;
    }
 
    Costmap3D::Vertex
    Costmap3D::toVertex(const Eigen::Vector2f& globalPosition) const
    {
        const auto localPosition = global_T_Costmap3D.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 auto size = getSize();
        const int iXSan = std::clamp<int>(iX, 0, size.x() - 1);
        const int iYSan = std::clamp<int>(iY, 0, size.y() - 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};
    }
 
    Costmap3D::Rotation
    Costmap3D::rotationFromIndex(const RotationIndex index) const
    {
        const float degrees = (360.F / parameters.orientations) * index;
        return {.index = index, .degrees = degrees};
    }
 
    float
    rotationFrom0To360(float degrees)
    {
        float deg = std::fmod(degrees, 360.F);
        if (deg < 0.F)
        {
            deg += 360.F;
        }
        return deg;
    }
 
    Costmap3D::Rotation
    Costmap3D::closestRotationFromDegrees(RotationDegrees degrees) const
    {
        degrees = rotationFrom0To360(degrees);
        //const float tmp = (degrees - (180.F / parameters.orientations)) / 360.F; // add half of discretization
        const float tmp = (degrees + (180.F / parameters.orientations)) /
                          (360.F / parameters.orientations); // normalize to [0, orientations)
        int index = tmp; // round up
        index = std::min(index, parameters.orientations - 1);
        return {.index = index, .degrees = degrees};
    }
 
    const SceneBounds&
    Costmap3D::getLocalSceneBounds() const noexcept
    {
        return sceneBounds;
    }
 
    Costmap3D::Optimum
    Costmap3D::optimum(Rotation rotation) 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::value_type::Index row = 0;
        Grid::value_type::Index col = 0;
        const auto size = getSize();
 
        // value of the min element
        float minVal = std::numeric_limits<float>::max();
 
        for (int r = 0; r < size.x(); r++)
        {
            for (int c = 0; c < size.y(); c++)
            {
                if (mask.has_value())
                {
                    if (not mask.value()(r, c)) // skip invalid cells
                    {
                        continue;
                    }
 
                    if (rotation.index == ALL_ORIENTATIONS.index)
                    {
                        for (const auto& g : grid)
                        {
                            // min value across all orientations
                            const float currentVal = g(r, c);
                            if (currentVal < minVal)
                            {
                                minVal = currentVal;
                                row = r;
                                col = c;
                            }
                        }
                    }
                    else
                    {
                        const auto& g = grid[rotation.index];
                        // min value across all orientations
                        const float currentVal = g(r, c);
                        if (currentVal < minVal)
                        {
                            minVal = currentVal;
                            row = r;
                            col = c;
                        }
                    }
                }
            }
        }
 
        return {.value = minVal, .index = {row, col}, .position = toPositionGlobal({row, col})};
    }
 
    const Costmap3D::Parameters&
    Costmap3D::params() const noexcept
    {
        return parameters;
    }
 
    const Costmap3D::Grid&
    Costmap3D::getGrid() const
    {
        return grid;
    }
 
    bool
    Costmap3D::isInCollision(const Position& p, std::optional<Rotation> rotation) const
    {
        const auto v = toVertex(p);
        if (rotation.has_value())
        {
            return grid.at(rotation->index)(v.index.x(), v.index.y()) <= 0.F;
        }
 
        // No rotation given
        // So check all rotations for collision
        return std::any_of(grid.begin(),
                           grid.end(),
                           [&v](const auto& grid2d)
                           { return grid2d(v.index.x(), v.index.y()) <= 0.F; });
    }
 
    const std::optional<Costmap3D::Mask>&
    Costmap3D::getMask() const noexcept
    {
        return mask;
    }
 
    std::optional<Costmap3D::Mask>&
    Costmap3D::getMutableMask() noexcept
    {
        return mask;
    }
 
    float
    Costmap3D::value_ignore_mask(const Index& index, const RotationIndex rot_index) const
    {
        return grid.at(rot_index)(index.x(), index.y());
    }
 
    std::optional<float>
    Costmap3D::value(const Costmap3D::Index& index, const Costmap3D::RotationIndex rot_index) const
    {
        if (not isValid(index))
        {
            ARMARX_IMPORTANT << "Requested index " << index << " but it is masked out!";
            return std::nullopt;
        }
        if (rot_index < 0 || rot_index > this->grid.size() - 1)
        {
            ARMARX_IMPORTANT << "Requested rotation index " << rot_index
                             << " but it is not within allowed range (0 .. "
                             << this->grid.size() - 1 << ")";
            return std::nullopt;
        }
 
        return grid.at(rot_index)(index.x(), index.y());
    }
 
    std::optional<float>
    Costmap3D::value(const Position& position,
                     const Costmap3D::RotationDegrees rotation_degrees) const
    {
        ARMARX_DEBUG << "value ...";
 
        const auto v = toVertex(position);
        const auto r = closestRotationFromDegrees(rotation_degrees);
        return value(v.index, r.index);
    }
 
    void
    Costmap3D::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;
 
        // update grids
        for (auto& grid2d : grid)
        {
            const auto newGrid =
                grid2d.block(min.x(), min.y(), max.x() - min.x() + 1, max.y() - min.y() + 1);
            grid2d = 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;
    }
 
    std::size_t
    Costmap3D::numberOfValidElements() const
    {
        if (not mask.has_value())
        {
            const auto size = getSize();
            return size.y() * size.x();
        }
 
        return mask->array().cast<int>().sum();
    }
 
    Costmap3D::Index
    Costmap3D::findClosestValidIndex(const Costmap3D::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 = Costmap3D::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<Costmap3D::Index, Costmap3D::Index>
    Costmap3D::getValidBoundingBox() const
    {
        if (not mask.has_value())
        {
            return {Index{0, 0}, getSize() - Index{1, 1}};
        }
 
        ARMARX_VERBOSE << "Fraction of valid elements: "
                       << mask->cast<float>().sum() / mask->size();
 
        ARMARX_TRACE;
 
        const Index size = getSize();
        Index min = getSize();
        Index max = Index{0, 0};
 
        for (int x = 0; x < size.x(); x++)
        {
            for (int y = 0; y < size.y(); 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};
    }
 
    Costmap3D::Index
    Costmap3D::getSize() const
    {
        return {grid[0].rows(), grid[0].cols()};
    }
 
    Costmap
    Costmap3D::costmapForOrientation(RotationIndex rotationIndex, bool initializeMask) const
    {
        Costmap c{grid[rotationIndex],
                  Costmap::Parameters{
                      .binaryGrid = false,
                      .cellSize = parameters.cellSize,
                      .robotRadius = 0.F,
                      .sceneBoundsMargin = parameters.sceneBoundsMargin,
                  },
                  sceneBounds};
        // c.getMutableGrid().array() += 2000.0;
        if (initializeMask)
        {
            c.getMutableMask() = c.getGrid().array() > 0.0F;
        }
        return c;
    }
 
    Costmap3D
    Costmap3D::WithSameDimsAs(const Costmap3D& other,
                              const Grid& grid,
                              const std::optional<Mask>& mask)
    {
        return {grid, other.params(), other.getLocalSceneBounds(), mask, other.origin()};
    }
 
    void
    Costmap3D::convertToPseudoSDF()
    {
        for (int rotIdx = 0; rotIdx < parameters.orientations; rotIdx++)
        {
            ARMARX_INFO << "Converting to pseudo SDF for rotation index " << rotIdx;
            convertToPseudoSDFForRotation(rotIdx);
        }
 
        // remove mask
        mask.reset();
    }
 
    void
    Costmap3D::convertToPseudoSDFForRotation(RotationIndex rotationIndex)
    {
        ARMARX_TRACE;
        // Idea: Apply the mask, remove the mask and then do flood-fill to generate negative cell values for nodes in collision
        // For the filling, we use a list of nodes in the masked-out region bordering the un-masked region as seeds
        // We can then calculate a (negative) distance value based on the neighboring un-masked cell values
        if (not mask.has_value())
        {
            ARMARX_INFO << "Costmap3D::convertToPseudoSDF: No mask present, nothing to do.";
            return;
        }
 
        // copy mask to local variable for easier access
        Costmap3D::Mask workingMask{this->getMask().value()};
 
        std::vector<Costmap3D::Index> borderingIndices;
        std::vector<Costmap3D::Index> nextBorderingIndices;
 
        const auto isValid = [this, rotationIndex](const Costmap3D::Index& idx)
        {
            if (this->isWithinRange(idx) && this->value_ignore_mask(idx, rotationIndex) == 0.0)
            {
                return false;
            }
 
            return this->isValid(idx);
        };
 
        // first fill of borderingIndices, idea: go through all nodes and see if they are masked out and are bordering one that is not
        for (int x = 0; x < getSize().x(); x++)
        {
            for (int y = 0; y < getSize().y(); y++)
            {
                const Costmap3D::Index idx{x, y};
                if (not isValid(idx))
                {
                    workingMask(x, y) = false;
 
                    // check neighbors
                    const std::vector<Costmap3D::Index> neighbors = {
                        {x + 1, y}, {x - 1, y}, {x, y + 1}, {x, y - 1}};
                    for (const auto& nIdx : neighbors)
                    {
                        if (isValid(nIdx))
                        {
                            borderingIndices.push_back(idx);
                            break;
                        }
                    }
                }
            }
        }
 
        ARMARX_INFO << "Found " << borderingIndices.size()
                    << " bordering indices to start flood-fill.";
        const float cellSize = parameters.cellSize;
        // now do flood-fill
        int iteration = 0;
        while (not borderingIndices.empty())
        {
            ARMARX_DEBUG << "Flood-fill iteration " << iteration
                         << ", current border size: " << borderingIndices.size();
            for (const auto& idx : borderingIndices)
            {
                if (workingMask(idx.x(), idx.y()))
                {
                    // already processed
                    continue;
                }
 
                float minNeighborValue = std::numeric_limits<float>::min();
                // check neighbors
                const std::vector<Costmap3D::Index> neighbors = {{idx.x() + 1, idx.y()},
                                                                 {idx.x() - 1, idx.y()},
                                                                 {idx.x(), idx.y() + 1},
                                                                 {idx.x(), idx.y() - 1}};
                for (const auto& nIdx : neighbors)
                {
                    if (isWithinRange(nIdx) && workingMask(nIdx.x(), nIdx.y()))
                    {
                        const float neighborValue = value_ignore_mask(nIdx, rotationIndex);
                        if (neighborValue != 0.0 && neighborValue > minNeighborValue)
                        {
                            minNeighborValue = neighborValue;
                        }
                    }
                }
                // set value to negative distance
                //const float newValue = minNeighborValue - cellSize;
                const float newValue = -1.0F * iteration * cellSize;
                grid[rotationIndex](idx.x(), idx.y()) = newValue;
                // mark as valid now
                workingMask(idx.x(), idx.y()) = true;
                // add neighbors to nextBorderingIndices
                for (const auto& nIdx : neighbors)
                {
                    if (isWithinRange(nIdx) && not workingMask(nIdx.x(), nIdx.y()))
                    {
                        nextBorderingIndices.push_back(nIdx);
                    }
                }
            }
            borderingIndices = nextBorderingIndices;
            nextBorderingIndices.clear();
            iteration++;
        }
 
        // check if sum of mask == num cells
        if (workingMask.array().cast<int>().sum(), getSize().x() * getSize().y())
        {
            ARMARX_WARNING << "After flood-fill, not all cells are valid!";
        }
    }
} // namespace armarx::navigation::algorithms::orientation_aware