util.cpp

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "util.h"
 
#include <algorithm>
#include <cstddef>
#include <cstdint>
#include <optional>
#include <vector>
 
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/imgproc.hpp>
 
#include <VirtualRobot/BoundingBox.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h> // IWYU pragma: keep
#include <VirtualRobot/Random.h>
#include <VirtualRobot/Robot.h> // IWYU pragma: keep
#include <VirtualRobot/SceneObjectSet.h> // IWYU pragma: keep
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Workspace/WorkspaceGrid.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/algorithms/Room.h>
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/core/basic_types.h>
 
#include <range/v3/algorithm/for_each.hpp>
#include <range/v3/view/zip.hpp>
 
namespace armarx::navigation::algorithms
{
 
 
    SceneBounds
    computeSceneBounds(const VirtualRobot::SceneObjectSetPtr& obstacles,
                       const std::vector<VirtualRobot::RobotPtr>& articulatedObjects,
                       const SceneBounds& init,
                       const std::vector<Room>& rooms,
                       const float margin,
                       const bool restrictToRooms)
    {
        SceneBounds bounds = init;
 
        if (restrictToRooms)
        {
            ARMARX_INFO << "Computing scene bounds from rooms";
 
            ARMARX_CHECK_NOT_EMPTY(rooms);
 
            for (const auto& room : rooms)
            {
                for (const auto& point : room.polygon)
                {
                    bounds.min.x() = std::min(point.x(), bounds.min.x());
                    bounds.min.y() = std::min(point.y(), bounds.min.y());
                    bounds.max.x() = std::max(point.x(), bounds.max.x());
                    bounds.max.y() = std::max(point.y(), bounds.max.y());
                }
            }
 
            return bounds;
        }
 
        // if no rooms are given, use the obstacles and articulated objects
        {
 
            ARMARX_CHECK_NOT_NULL(obstacles);
 
            ARMARX_CHECK((not obstacles->getCollisionModels().empty()) or
                         (not articulatedObjects.empty()));
 
            const auto expandBounds = [&bounds](const VirtualRobot::BoundingBox&& bb)
            {
                if (!bb.getMin().allFinite() || !bb.getMax().allFinite())
                {
                    // non-finite bounding box, e.g. from an object with a missing collision
                    // mesh - ignore it rather than poisoning the scene bounds with NaN
                    return;
                }
                bounds.min.x() = std::min(bb.getMin().x(), bounds.min.x());
                bounds.min.y() = std::min(bb.getMin().y(), bounds.min.y());
                bounds.max.x() = std::max(bb.getMax().x(), bounds.max.x());
                bounds.max.y() = std::max(bb.getMax().y(), bounds.max.y());
            };
 
            // non-articulated objects
            for (const auto& colModel : obstacles->getCollisionModels())
            {
                expandBounds(colModel->getBoundingBox());
            }
 
            ARMARX_VERBOSE << "non-articulated objects: scene bounds: " << bounds.min << " and "
                           << bounds.max;
 
            // articulated objects
            for (const auto& articulatedObject : articulatedObjects)
            {
                if (articulatedObject->getCollisionModels().empty())
                {
                    ARMARX_WARNING << "The articulated object `" << articulatedObject->getType()
                                   << "/" << articulatedObject->getName()
                                   << "` does not provide any collision model!";
                }
 
                ARMARX_DEBUG << "Articulated object `" << articulatedObject->getType() << "/"
                             << articulatedObject->getName();
 
                for (const auto& colModel : articulatedObject->getCollisionModels())
                {
                    expandBounds(colModel->getBoundingBox());
                    ARMARX_DEBUG << VAROUT(colModel->getBoundingBox().getMin()) << " and "
                                 << VAROUT(colModel->getBoundingBox().getMax());
                    ARMARX_DEBUG << bounds.min << " and " << bounds.max;
                }
            }
 
            ARMARX_VERBOSE << "articulated objects: scene bounds: " << bounds.min << " and "
                           << bounds.max;
        }
 
        // expand bounds by margin
        bounds.min.x() -= margin;
        bounds.min.y() -= margin;
        bounds.max.x() += margin;
        bounds.max.y() += margin;
 
        return bounds;
    }
 
    SceneBounds
    computeSceneBounds(const std::vector<Eigen::Vector2f>& points,
                       const SceneBounds& init,
                       const float margin)
    {
        SceneBounds bounds = init;
 
        for (const auto& point : points)
        {
            bounds.min.x() = std::min(point.x(), bounds.min.x());
            bounds.min.y() = std::min(point.y(), bounds.min.y());
            bounds.max.x() = std::max(point.x(), bounds.max.x());
            bounds.max.y() = std::max(point.y(), bounds.max.y());
        }
 
        // expand bounds by margin
        bounds.min.x() -= margin;
        bounds.min.y() -= margin;
        bounds.max.x() += margin;
        bounds.max.y() += margin;
 
        return bounds;
    }
 
    SceneBounds
    toSceneBounds(const VirtualRobot::WorkspaceGrid::Extends& extends)
    {
        return {.min = Eigen::Vector2f{extends.minX, extends.minY},
                .max = Eigen::Vector2f{extends.maxX, extends.maxY}};
    }
 
    SceneBounds
    merge(const std::vector<SceneBounds>& sceneBounds)
    {
        SceneBounds bounds;
 
        const auto expandBounds = [&bounds](const SceneBounds& sb)
        {
            bounds.min.x() = std::min(bounds.min.x(), sb.min.x());
            bounds.min.y() = std::min(bounds.min.y(), sb.min.y());
 
            bounds.max.x() = std::max(bounds.max.x(), sb.max.x());
            bounds.max.y() = std::max(bounds.max.y(), sb.max.y());
 
            return bounds;
        };
 
        ranges::for_each(sceneBounds, expandBounds);
 
        return bounds;
    }
 
    Costmap
    toCostmap(const VirtualRobot::WorkspaceGrid& workspaceGrid)
    {
        const Costmap::Grid grid(workspaceGrid.getCells().x, workspaceGrid.getCells().y);
        const SceneBounds sceneBounds = toSceneBounds(workspaceGrid.getExtends());
 
        const Costmap::Parameters parameters{.binaryGrid = false,
                                             .cellSize = workspaceGrid.getDiscretizeSize()};
 
        return {grid, parameters, sceneBounds};
    }
 
    // Costmap
    // mergeUnaligned(const std::vector<Costmap>& costmaps,
    //                const std::vector<float>& weights,
    //                float cellSize,
    //                const Eigen::Array2f& offset)
    // {
    //     ARMARX_TRACE;
 
    //     ARMARX_CHECK_NOT_EMPTY(costmaps);
    //     ARMARX_CHECK_EQUAL(costmaps.size(), weights.size());
 
    //     // if unset, use the smallest cell size
    //     if (cellSize < 0)
    //     {
    //         const auto compCellSize = [](const Costmap& a, const Costmap& b) -> bool
    //         { return a.params().cellSize < b.params().cellSize; };
 
    //         cellSize = ranges::min_element(costmaps, compCellSize)->params().cellSize;
    //     }
 
    //     // scale each costmap
    //     std::vector<Costmap> scaledCostmaps =
    //         simox::alg::apply(costmaps,
    //                           [&cellSize](const Costmap& costmap) -> Costmap
    //                           { return scaleCostmap(costmap, cellSize); });
 
    //     // merge costmaps into one
    //     ARMARX_VERBOSE << "Merging " << scaledCostmaps.size() << " costmaps";
 
    //     // - combine scene bounds
    //     std::vector<SceneBounds> sceneBoundsAll = simox::alg::apply(
    //         scaledCostmaps, [](const Costmap& costmap) { return costmap.getSceneBounds(); });
 
    //     SceneBounds sceneBounds = merge(sceneBoundsAll);
    //     sceneBounds.min.x() -= 0.5 * cellSize;
    //     sceneBounds.max.x() -= 0.5 * cellSize;
    //     sceneBounds.min.y() -= cellSize;
    //     sceneBounds.max.y() -= cellSize;
 
 
    //     sceneBounds.min.x() += offset.x();
    //     sceneBounds.max.x() += offset.x();
    //     sceneBounds.min.y() += offset.y();
    //     sceneBounds.max.y() += offset.y();
 
    //     // - create grid
    //     const auto largeGrid = CostmapBuilder::createUniformGrid(
    //         sceneBounds, Costmap::Parameters{.binaryGrid = false, .cellSize = cellSize});
 
    //     Costmap largeCostmap(
    //         largeGrid, Costmap::Parameters{.binaryGrid = false, .cellSize = cellSize}, sceneBounds);
 
    //     // TODO save each combo individually
 
    //     static int i = 0;
    //     // - add all costmaps to this one
    //     ranges::for_each(ranges::views::zip(scaledCostmaps, weights),
    //                      [&largeCostmap](const auto& p)
    //                      {
    //                          ARMARX_INFO << "adding to costmap";
    //                          const auto& [costmap, weight] = p;
    //                          largeCostmap.add(costmap, weight);
 
    //                          armarx::navigation::algorithms::save(
    //                              costmap, "/tmp/costmap" + std::to_string(i++));
    //                      });
 
    //     armarx::navigation::algorithms::save(largeCostmap,
    //                                          "/tmp/large-costmap" + std::to_string(i++));
 
    //     return largeCostmap;
    // }
 
    void
    checkSameSize(const std::vector<Costmap>& costmaps)
    {
        ARMARX_CHECK_NOT_EMPTY(costmaps);
 
        const auto assertSameSize = [&costmaps](const Costmap& costmap)
        {
            ARMARX_CHECK_EQUAL(costmap.getGrid().rows(), costmaps.front().getGrid().rows());
            ARMARX_CHECK_EQUAL(costmap.getGrid().rows(), costmaps.front().getGrid().rows());
        };
 
        ranges::for_each(costmaps, assertSameSize);
    }
 
    Costmap
    mergeAligned(const std::vector<Costmap>& costmaps, CostmapMergeMode mode)
    {
        ARMARX_CHECK_NOT_EMPTY(costmaps);
        checkSameSize(costmaps);
 
 
        // average mode: fixed weight
        if (mode == CostmapMergeMode::AVERAGE)
        {
            const std::vector<float> weights(costmaps.size(), 1.F / costmaps.size());
            return mergeAligned(costmaps, weights);
        }
 
        const auto grid = CostmapBuilder::createUniformGrid(costmaps.front().getLocalSceneBounds(),
                                                            costmaps.front().params());
 
        Costmap mergedCostmap = Costmap::WithSameDimsAs(costmaps.front(), grid);
 
        const auto addMode = [&]() -> Costmap::AddMode
        {
            Costmap::AddMode addMode{};
 
            switch (mode)
            {
                case CostmapMergeMode::AVERAGE:
                    ARMARX_CHECK(false) << "This case should already be handled.";
                    break;
                case CostmapMergeMode::MIN:
                    addMode = Costmap::AddMode::MIN;
                    break;
                case CostmapMergeMode::MAX:
                    addMode = Costmap::AddMode::MAX;
                    break;
            }
 
            return addMode;
        }();
 
        ranges::for_each(costmaps,
                         [&mergedCostmap, &addMode](const auto& costmap)
                         { mergedCostmap.add(costmap, addMode); });
 
        return mergedCostmap;
    }
 
    Costmap
    mergeAligned(const std::vector<Costmap>& costmaps, const std::vector<float>& weights)
    {
        ARMARX_CHECK_EQUAL(costmaps.size(), weights.size());
        ARMARX_CHECK_NOT_EMPTY(costmaps);
        checkSameSize(costmaps);
 
 
        const auto grid = CostmapBuilder::createUniformGrid(costmaps.front().getLocalSceneBounds(),
                                                            costmaps.front().params());
 
        Costmap mergedCostmap = Costmap::WithSameDimsAs(costmaps.front(), grid);
 
        // foreach pair (costmap, weight): add it to there merged costmap
        ranges::for_each(ranges::views::zip(costmaps, weights),
                         [&mergedCostmap](const auto& p)
                         {
                             const auto& [costmap, weight] = p;
                             mergedCostmap.add(costmap, weight);
                         });
 
        return mergedCostmap;
    }
 
    Costmap
    scaleCostmap(const Costmap& costmap, float cellSize)
    {
        const float scale = costmap.params().cellSize / cellSize;
        ARMARX_VERBOSE << "Scaling grid by a factor of " << scale;
 
        cv::Mat src;
        cv::eigen2cv(costmap.getGrid(), src);
 
        cv::Mat dst;
        cv::resize(src, dst, cv::Size{0, 0}, scale, scale);
 
        Eigen::MatrixXf scaledGrid;
        cv::cv2eigen(dst, scaledGrid);
 
 
        std::optional<Costmap::Mask> scaledMask;
        if (costmap.getMask().has_value())
        {
            ARMARX_TRACE;
            ARMARX_VERBOSE << "Scaling mask by a factor of " << scale;
 
            // just an int large enough to check if resizing causes
            // issues on boundary to invalid regions
            constexpr int someInt = 100;
 
            Eigen::Matrix<std::uint8_t, Eigen::Dynamic, Eigen::Dynamic> mask =
                someInt * costmap.getMask().value().cast<std::uint8_t>();
 
            cv::Mat maskMat;
            cv::eigen2cv(mask, maskMat);
 
            cv::Mat maskScaled;
            cv::resize(maskMat, maskScaled, cv::Size{0, 0}, scale, scale);
 
            // only those points are valid where the original value '10' is preserved.
            // all other points are boundary points to the invalid area
            // cv::threshold(maskMat, maskMat, 10.9, 1, cv::THRESH_BINARY);
 
            Eigen::MatrixXi scaledMaskInt; //(scaledGrid.rows(), scaledGrid.cols());
            cv::cv2eigen(maskScaled, scaledMaskInt);
 
            Costmap::Mask m = scaledMaskInt.array() >= someInt;
 
            scaledMask = m;
        }
 
        ARMARX_VERBOSE << "Original size (" << costmap.getGrid().rows() << ", "
                       << costmap.getGrid().cols() << ")";
        ARMARX_VERBOSE << "Resized to (" << scaledGrid.rows() << ", " << scaledGrid.cols() << ")";
 
        return {scaledGrid,
                Costmap::Parameters{.binaryGrid = false, .cellSize = cellSize},
                costmap.getLocalSceneBounds(),
                scaledMask,
                costmap.origin()};
    }
 
    std::optional<core::Pose2D>
    sampleValidPositionInMap(const algorithms::Costmap& costmap)
    {
        const auto sizeX = costmap.getGrid().cols();
        const auto sizeY = costmap.getGrid().rows();
 
        constexpr std::size_t maxIterations = 1000;
 
        // sample a valid pose in the costmap
 
        for (std::size_t iteration = 0; iteration < maxIterations; iteration++)
        {
            const float iX = VirtualRobot::RandomFloat() * static_cast<float>(sizeX);
            const float iY = VirtualRobot::RandomFloat() * static_cast<float>(sizeY);
 
            algorithms::Costmap::Index idx(iX, iY);
 
            if (not costmap.isValid(idx))
            {
                continue;
            }
 
            core::Pose2D pose = core::Pose2D::Identity();
            pose.translation() = costmap.toPositionGlobal(idx);
            return pose;
        }
 
        ARMARX_ERROR << "Failed to sample pose in costmap!";
        return std::nullopt;
    }
 
    static void
    invalidateOutsideRoomsImpl(
        const std::vector<Room>& rooms,
        std::function<Costmap::Position(const Costmap::Index&)> indexToGlobal,
        Costmap::Mask& mask,
        const float footprintRadius)
    {
        const std::size_t c_x = mask.rows();
        const std::size_t c_y = mask.cols();
 
        const auto isInsideRoom = [&rooms, footprintRadius](const Eigen::Vector2f& pos) -> bool
        {
            return std::any_of(rooms.begin(),
                               rooms.end(),
                               [&pos, footprintRadius](const Room& room) -> bool
                               { return room.isInside(pos, footprintRadius); });
        };
 
        for (unsigned int x = 0; x < c_x; x++)
        {
            for (unsigned int y = 0; y < c_y; y++)
            {
                const Costmap::Index index{x, y};
                const Costmap::Position position = indexToGlobal(index);
 
                if (not isInsideRoom(position))
                {
                    mask(x, y) = false;
                }
            }
        }
    }
 
    void
    invalidateOutsideRooms(const std::vector<Room>& rooms,
                           Costmap& costmap,
                           const float footprintRadius)
    {
        ARMARX_CHECK(costmap.getMutableMask().has_value());
        const auto indexToGlobal = [&costmap](auto idx) { return costmap.toPositionGlobal(idx); };
 
        invalidateOutsideRoomsImpl(
            rooms, indexToGlobal, costmap.getMutableMask().value(), -1.F);
 
        // make sure small gaps between rooms are included
        costmap.getMutableMask() =
            dilateMask(costmap.getMask().value(),
                       static_cast<int>(std::max(
                           1.F, costmap.params().sceneBoundsMargin / costmap.params().cellSize)));
 
        // apply the footprint erosion last so the gap dilation does not undo it
        if (footprintRadius > 0.F)
        {
            invalidateOutsideRoomsImpl(
                rooms, indexToGlobal, costmap.getMutableMask().value(), footprintRadius);
        }
    }
 
    void
    invalidateOutsideRooms(const std::vector<Room>& rooms,
                           orientation_aware::Costmap3D& costmap,
                           const float footprintRadius)
    {
        ARMARX_CHECK(costmap.getMutableMask().has_value());
        invalidateOutsideRoomsImpl(
            rooms,
            [&costmap](auto idx) { return costmap.toPositionGlobal(idx); },
            costmap.getMutableMask().value(),
            footprintRadius);
    }
 
    Costmap::Mask
    dilateMask(const Costmap::Mask& mask, int numIterations)
    {
        cv::Mat cvMask;
        cv::eigen2cv(mask, cvMask);
 
        cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
        cv::dilate(cvMask, cvMask, kernel, cv::Point(-1, -1), numIterations);
 
        Costmap::Mask dst;
        cv::cv2eigen(cvMask, dst);
        return dst;
    }
 
} // namespace armarx::navigation::algorithms