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 <opencv2/imgproc.hpp>
 
#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};
    }
 
    std::pair<Costmap3D::Rotation, Costmap3D::Rotation>
    Costmap3D::bracketingRotationsFromDegrees(RotationDegrees degrees) const
    {
        const float deg = rotationFrom0To360(degrees);
        const float step = 360.F / parameters.orientations;
 
        int lowIndex = static_cast<int>(deg / step);
        lowIndex = std::min(lowIndex, parameters.orientations - 1);
        const int highIndex = (lowIndex + 1) % parameters.orientations;
 
        return {rotationFromIndex(lowIndex), rotationFromIndex(highIndex)};
    }
 
    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; });
    }
 
    bool
    Costmap3D::isFreeWithClearance(const Position& p,
                                   const RotationDegrees orientationDeg,
                                   const float clearance) const
    {
        const auto v = toVertex(p);
        if (not isValid(v.index))
        {
            return false;
        }
 
        const auto [low, high] = bracketingRotationsFromDegrees(orientationDeg);
        return grid.at(low.index)(v.index.x(), v.index.y()) > clearance and
               grid.at(high.index)(v.index.x(), v.index.y()) > clearance;
    }
 
    std::optional<Costmap3D::Vertex>
    Costmap3D::findClosestCollisionFreeVertex(const Position& position,
                                              const RotationDegrees orientationDeg,
                                              const float maxDistance,
                                              const float minClearance) const
    {
        ARMARX_TRACE;
 
        const auto [low, high] = bracketingRotationsFromDegrees(orientationDeg);
 
        // Overlap the two bracketing orientation slices: a cell is only free if the robot
        // fits at both discrete orientations, i.e. also at the continuous orientation in
        // between. Shifting by minClearance turns the 2D search's `> 0` test into
        // `> minClearance`; clamping at 0 is required because Costmap::isInCollision
        // treats exactly 0 (not negative values) as collision.
        const Eigen::MatrixXf combined =
            (grid.at(low.index).cwiseMin(grid.at(high.index)).array() - minClearance)
                .cwiseMax(0.F)
                .matrix();
 
        const Costmap combinedCostmap{combined,
                                      Costmap::Parameters{
                                          .binaryGrid = false,
                                          .cellSize = parameters.cellSize,
                                          .robotRadius = 0.F,
                                          .sceneBoundsMargin = parameters.sceneBoundsMargin,
                                      },
                                      sceneBounds,
                                      mask,
                                      global_T_Costmap3D};
 
        const auto vertex = combinedCostmap.findClosestCollisionFreeVertex(position, maxDistance);
        if (not vertex.has_value())
        {
            return std::nullopt;
        }
 
        return Vertex{.index = vertex->index, .position = vertex->position};
    }
 
    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_DEBUG << "Converting to Euclidean SDF for rotation index " << rotIdx;
            convertToPseudoSDFForRotation(rotIdx);
        }
 
        // remove mask
        mask.reset();
    }
 
    void
    Costmap3D::convertToPseudoSDFForRotation(RotationIndex rotationIndex)
    {
        ARMARX_TRACE;
        if (not mask.has_value())
        {
            ARMARX_DEBUG << "Costmap3D::convertToPseudoSDF: No mask present, nothing to do.";
            return;
        }
 
        const Eigen::MatrixXf& grid2d = grid[rotationIndex];
        const int nx = getSize().x();
        const int ny = getSize().y();
 
        // Build binary mask for cv::distanceTransform.
        // OpenCV Mat uses (row=y, col=x); Eigen uses (row=x, col=y).
        // Free cells (positive distance) = 255; collision/masked = 0.
        cv::Mat mask8U(ny, nx, CV_8U);
        for (int x = 0; x < nx; ++x)
        {
            for (int y = 0; y < ny; ++y)
            {
                bool isFree = mask.value()(x, y) && grid2d(x, y) > 0.0f;
                mask8U.at<uchar>(y, x) = isFree ? 255 : 0;
            }
        }
 
        // Compute exact Euclidean distance (in pixels) from every zero pixel
        // to the nearest non-zero pixel.
        cv::Mat distImage;
        cv::distanceTransform(mask8U, distImage, cv::DIST_L2, cv::DIST_MASK_PRECISE, CV_32F);
 
        const float cellSize = parameters.cellSize;
 
        // Overwrite collision cells with negative Euclidean distance (in mm).
        // Free cells keep their original positive obstacle distances.
        for (int x = 0; x < nx; ++x)
        {
            for (int y = 0; y < ny; ++y)
            {
                if (grid2d(x, y) <= 0.0f)
                {
                    float distPixels = distImage.at<float>(y, x);
                    grid[rotationIndex](x, y) = -distPixels * cellSize;
                }
            }
        }
 
        ARMARX_DEBUG << "Converted orientation " << rotationIndex
                     << " to true Euclidean SDF. Range: [" << grid[rotationIndex].minCoeff() << ", "
                     << grid[rotationIndex].maxCoeff() << "]";
    }
} // namespace armarx::navigation::algorithms::orientation_aware