Costmap3DBuilder.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     Niklas Arlt ( niklas dot arlt at kit dot edu )
 * @date       2025
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
// Most of the code is taken from algorithms/CostmapBuilder.cpp
 
#include "Costmap3DBuilder.h"
 
#include <algorithm>
#include <cstddef>
#include <functional>
#include <optional>
#include <string>
#include <vector>
 
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Nodes/RobotNode.h> // IWYU pragma: keep
#include <VirtualRobot/Robot.h> // IWYU pragma: keep
#include <VirtualRobot/SceneObjectSet.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/ScopedStopWatch.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/Room.h>
#include <armarx/navigation/algorithms/orientation_aware/Costmap3D.h>
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/algorithms/util.h>
#include <armarx/navigation/core/basic_types.h>
 
namespace armarx::navigation::algorithms::orientation_aware
{
 
 
    Costmap3DBuilder::Costmap3DBuilder(
        const VirtualRobot::RobotPtr& robot,
        const VirtualRobot::SceneObjectSetPtr& obstacles,
        const std::vector<VirtualRobot::RobotPtr>& articulatedObjects,
        const std::vector<Room>& rooms,
        const Costmap3D::Parameters& parameters,
        const std::string& robotCollisonModelName,
        const Costmap3DBuilderParams& builderParameters) :
        robot(robot),
        obstacles(obstacles),
        articulatedObjects(articulatedObjects),
        rooms(rooms),
        parameters(parameters),
        robotCollisionModelName(robotCollisonModelName),
        builderParameters(builderParameters)
    {
        ARMARX_CHECK_NOT_NULL(robot) << "Robot must be set";
        ARMARX_CHECK_NOT_NULL(obstacles);
 
        if (not robotCollisonModelName.empty())
        {
            ARMARX_CHECK(robot->hasRobotNode(robotCollisionModelName)) << robotCollisionModelName;
        }
 
        if (not parameters.robotModel.rootNode.empty())
        {
            ARMARX_INFO << "Using alternative root node " << QUOTED(parameters.robotModel.rootNode);
            auto node = robot->getRobotNode(parameters.robotModel.rootNode);
            ARMARX_CHECK_NOT_NULL(node) << parameters.robotModel.rootNode;
 
            const Eigen::Isometry3f root_T_used_root(node->getPoseInRootFrame());
 
            // Obtain 2D transform. We make some strong assumptions that need to hold here.
            ARMARX_CHECK_LESS(std::abs(root_T_used_root.translation().z()), 1)
                << "Nodes must be at the same hight.";
 
            const Eigen::Matrix3f R = root_T_used_root.linear();
 
            // both the third row and column should be [0,0,1]
            ARMARX_TRACE;
            const float a = Eigen::Vector3f{R.row(2)}.dot(Eigen::Vector3f::UnitZ());
            ARMARX_TRACE;
            const float b = Eigen::Vector3f{R.col(2)}.dot(Eigen::Vector3f::UnitZ());
 
            ARMARX_CHECK_CLOSE(a, 1, 0.01);
            ARMARX_CHECK_CLOSE(b, 1, 0.01);
 
            // Now we know that the nodes are in a plane
 
            ARMARX_INFO << "Alternative root node is valid.";
 
            root_T_used_root_2d = Eigen::Isometry2f::Identity();
            ARMARX_TRACE;
            root_T_used_root_2d.translation() = root_T_used_root.translation().head<2>();
            ARMARX_TRACE;
            root_T_used_root_2d.linear() = R.topLeftCorner<2, 2>();
            // TODO ensure valid rotation matrix (cross product.)
 
            ARMARX_TRACE;
 
            // FIXME maybe we have to invert root_T_used_root_2d based on its usage
            root_T_used_root_2d = root_T_used_root_2d.inverse();
 
            ARMARX_INFO << "Root transform: " << root_T_used_root_2d.matrix();
        }
 
        // Room selection
        const auto& enabledRooms = this->builderParameters.roomEnableList;
        ARMARX_INFO << "Rooms:";
        for (const auto& room : this->rooms)
        {
            ARMARX_INFO << " - " << room.name;
        }
        ARMARX_INFO << "";
 
        if (not enabledRooms.empty())
        {
            // if roomEnableList is specified, remove all other rooms from the provided list
            this->rooms.erase(std::remove_if(this->rooms.begin(),
                                             this->rooms.end(),
                                             [&enabledRooms](const Room& room)
                                             {
                                                 const bool roomIncluded =
                                                     std::find(enabledRooms.cbegin(),
                                                               enabledRooms.cend(),
                                                               room.name) != enabledRooms.cend();
                                                 if (not roomIncluded)
                                                 {
                                                     ARMARX_VERBOSE
                                                         << "Room " << room.name
                                                         << " was found but is not included";
                                                 }
                                                 return not roomIncluded;
                                             }),
                              this->rooms.end());
        }
 
        if (builderParameters.restrictToRooms)
        {
            ARMARX_CHECK_NOT_EMPTY(this->rooms)
                << "No rooms enabled, even though restrict to rooms is true";
        }
    }
 
    Costmap3D
    Costmap3DBuilder::extend(Costmap costmap)
    {
        armarx::core::time::ScopedStopWatch sw(
            [](const armarx::core::time::Duration& duration)
            { ARMARX_INFO << "Creating Costmap3D took " << duration; });
 
        // We do not restrict the costmap to rooms here. Instead, we only update the costmap
        // inside the rooms below (if this is requested).
 
        std::optional<Costmap3D> inflatedCostmap;
        {
            armarx::core::time::ScopedStopWatch sw(
                [](const armarx::core::time::Duration& duration)
                { ARMARX_INFO << "Filling Costmap3D took " << duration; });
 
            ARMARX_VERBOSE << "Filling grid with size (" << costmap.getGrid().rows() << "/"
                           << costmap.getGrid().cols() << ") and resolution "
                           << costmap.params().cellSize;
 
            inflatedCostmap.emplace(inflateRotationDimension(costmap));
 
            if (builderParameters.restrictToRooms)
            {
                ARMARX_INFO << "Restricting to rooms";
                extendGridCosts(*inflatedCostmap, rooms);
            }
            else
            {
                extendGridCosts(*inflatedCostmap, {});
            }
 
            ARMARX_VERBOSE << "Filled grid";
        }
 
        initializeMask(inflatedCostmap.value());
        ARMARX_VERBOSE << "Initialized mask";
 
        return inflatedCostmap.value();
    }
 
    Costmap3D
    Costmap3DBuilder::create(const SceneBounds& init)
    {
        armarx::core::time::ScopedStopWatch sw(
            [](const armarx::core::time::Duration& duration)
            { ARMARX_INFO << "Creating Costmap3D took " << duration; });
 
        ARMARX_INFO << articulatedObjects.size() << " articulated objects";
        ARMARX_INFO << obstacles->getSize() << " objects";
 
        const auto sceneBounds = computeSceneBounds(obstacles,
                                                    articulatedObjects,
                                                    init,
                                                    rooms,
                                                    parameters.sceneBoundsMargin,
                                                    builderParameters.restrictToRooms);
        const auto grid = createUniformGrid(sceneBounds, parameters);
 
        ARMARX_VERBOSE << "Creating grid";
        Costmap3D costmap(grid, parameters, sceneBounds);
 
        if (builderParameters.restrictToRooms)
        {
            initializeEmptyMask(costmap);
 
            ARMARX_CHECK_NOT_EMPTY(rooms);
            ARMARX_VERBOSE << "Restricting to rooms:";
            for (const auto& room : rooms)
            {
                ARMARX_VERBOSE << " - " << room.name;
            }
 
            invalidateOutsideRooms(rooms, costmap);
 
            ARMARX_VERBOSE << "Restricted to rooms. Fraction of valid elements: "
                           << costmap.mask->cast<float>().sum() / costmap.mask->size();
        }
 
        {
            armarx::core::time::ScopedStopWatch sw(
                [](const armarx::core::time::Duration& duration)
                { ARMARX_INFO << "Filling Costmap3D took " << duration; });
 
            ARMARX_VERBOSE << "Filling grid with size (" << costmap.getGrid()[0].rows() << "/"
                           << costmap.getGrid()[0].cols() << "/rot" << costmap.params().orientations
                           << ") and resolution " << costmap.params().cellSize;
            fillGridCosts(costmap);
            ARMARX_VERBOSE << "Filled grid";
        }
 
        // Mask is broken for now
        //initializeMask(costmap);
        ARMARX_VERBOSE << "Initialized mask";
 
        return costmap;
    }
 
    void
    Costmap3DBuilder::initializeMask(Costmap3D& costmap)
    {
        ARMARX_CHECK(costmap.grid.size() > 0);
 
        costmap.mask = costmap.grid[0].array() > 0.F;
 
        ARMARX_VERBOSE << "Initializing mask: Fraction of valid elements: "
                       << costmap.mask->cast<float>().sum() / costmap.mask->size();
    }
 
    std::optional<Costmap3DBuilder::DebugOutput2D>
    Costmap3DBuilder::getDebugOutput2D()
    {
        return debugOutput2D;
    }
 
    void
    Costmap3DBuilder::initializeEmptyMask(Costmap3D& costmap)
    {
        ARMARX_CHECK(costmap.grid.size() > 0);
 
        costmap.mask = costmap.grid[0].array() >= -42.F; // FIXME: magic number, just same size
        costmap.mask->setOnes();
 
        ARMARX_VERBOSE << "Initializing empty mask: Fraction of valid elements: "
                       << costmap.mask->cast<float>().sum() / costmap.mask->size();
    }
 
    Costmap3D::Grid
    Costmap3DBuilder::createUniformGrid(const SceneBounds& sceneBounds,
                                        const Costmap3D::Parameters& parameters)
    {
        Costmap3D::Grid grid;
        grid.reserve(parameters.orientations);
        for (int i = 0; i < parameters.orientations; ++i)
        {
            grid.push_back(createUniformGridSingleOrientation(sceneBounds, parameters));
        }
 
        return grid;
    }
 
    Eigen::MatrixXf
    Costmap3DBuilder::createUniformGridSingleOrientation(const SceneBounds& sceneBounds,
                                                         const Costmap3D::Parameters& parameters)
    {
        ARMARX_TRACE;
 
        ARMARX_VERBOSE << "Scene bounds are " << sceneBounds.min << " and " << sceneBounds.max;
 
        //+1 for explicit rounding up
        size_t c_x = (sceneBounds.max.x() - sceneBounds.min.x()) / parameters.cellSize + 1;
        size_t c_y = (sceneBounds.max.y() - sceneBounds.min.y()) / parameters.cellSize + 1;
 
        ARMARX_VERBOSE << "Grid size: " << c_x << ", " << c_y;
 
        ARMARX_VERBOSE << "Resetting grid";
        Eigen::MatrixXf grid(c_x, c_y);
        grid.setZero();
 
        return grid;
    }
 
    float
    Costmap3DBuilder::computeCost(const core::Pose2D& global_T_root,
                                  const Robot2D& robot2d,
                                  const Scene2D& scene)
    {
        const core::Pose2D global_T_used_root = global_T_root * root_T_used_root_2d;
 
        const auto polygon = robot2d.getRobotAtPose(global_T_used_root);
        return scene.distance(polygon);
    }
 
    Costmap3D
    Costmap3DBuilder::inflateRotationDimension(Costmap& costmap)
    {
        // TODO(niklas): For efficient implementation: Use a wrapper with reference to old costmap and don't allocate anything
 
        std::vector<Costmap::Grid> new_grid;
        new_grid.reserve(parameters.orientations);
        for (int i = 0; i < parameters.orientations; ++i)
        {
            new_grid.push_back(costmap.getGrid());
        }
 
        Costmap3D costmap3d{new_grid,
                            parameters,
                            costmap.getLocalSceneBounds(),
                            costmap.getMask(),
                            costmap.origin()};
        return costmap3d;
    }
 
    void
    Costmap3DBuilder::fillGridCosts2D(Costmap3D& costmap)
    {
        filteredObstacles = filterObjectsForCostmap(costmap);
        const auto fn = [&](const MapStructRobot2D& m) -> float
        {
            const auto degrees = costmap.rotationFromIndex(m.c.rotIdx).degrees;
            const auto pose = costmap.globalPose(m.c.posIdx, degrees);
            ARMARX_CHECK(costmap.rotationDegrees(pose) - degrees < 0.001)
                << "Rotation degrees do not match: " << costmap.rotationDegrees(pose)
                << " != " << degrees;
            if (m.c.isMaskedOut())
            {
                return 0.F;
            }
            float distance = computeCost(pose, m.robot, m.scene);
            if (distance < builderParameters.obstacleSafetyMargin)
            {
                distance = 0.F;
            }
            return distance;
        };
 
        applyFnToCostmap(costmap, fn);
    }
 
    void
    Costmap3DBuilder::fillGridCosts(Costmap3D& costmap)
    {
        filteredObstacles = filterObjectsForCostmap(costmap);
        ARMARX_INFO << VAROUT(builderParameters.obstacleSafetyMargin);
        if (builderParameters.approximation2D)
        {
            fillGridCosts2D(costmap);
            return;
        }
 
        const auto fn = [&](const MapStruct& m) -> float
        {
            const auto pose =
                costmap.globalPose(m.c.posIdx, costmap.rotationFromIndex(m.c.rotIdx).degrees);
            if (m.c.isMaskedOut())
            {
                return 0.F;
            }
            float distance = m.distanceCalculator.distanceToObstacles(pose);
            if (distance < builderParameters.obstacleSafetyMargin)
            {
                distance = 0.F;
            }
            return distance;
        };
 
        applyFnToCostmap(costmap, fn);
    }
 
    void
    Costmap3DBuilder::extendGridCosts(Costmap3D& costmap, const std::vector<Room>& rooms)
    {
        // Note: Currently not implemented
        // How can we create a 2D collision model footprint to check collisions in the costmap?
    }
 
    void
    Costmap3DBuilder::applyFnToCostmap(Costmap3D& costmap, const MapFn& fn)
    {
        SceneRepresentation sceneRepresentation{.staticObjects = filteredObstacles,
                                                .articulatedObjects = articulatedObjects};
        DistanceCalculator distanceCalculator{sceneRepresentation};
 
        if (costmap.mask.has_value())
        {
            ARMARX_VERBOSE << "Costmap3D provides mask.";
        }
 
        ARMARX_CHECK(costmap.grid.size() > 0);
        const std::size_t c_x = costmap.grid[0].rows();
        const std::size_t c_y = costmap.grid[0].cols();
 
        robot->setUpdateVisualization(false);
 
#pragma omp parallel for schedule(static) firstprivate(distanceCalculator) default(shared)
        for (unsigned int x = 0; x < c_x; x++)
        {
            // initialize if needed
            if (not distanceCalculator.isRobotInitialized())
            {
#pragma omp critical
                distanceCalculator.initializeRobot(
                    robot,
                    Robot3D::Params{.collisionModelName = robotCollisionModelName,
                                    .scaleFactor = builderParameters.collisionModelScaleFactor,
                                    .primitiveApproximationIDs =
                                        builderParameters.primitiveApproximationIDs,
                                    .rootNode = parameters.robotModel.rootNode});
            }
            ARMARX_CHECK(distanceCalculator.isRobotInitialized());
 
            for (unsigned int y = 0; y < c_y; y++)
            {
                for (int rot_idx = 0; rot_idx < parameters.orientations; rot_idx++)
                {
                    const Costmap3D::Index index{x, y};
 
                    const auto maskVal = costmap.mask.has_value()
                                             ? std::make_optional(costmap.mask.value()(x, y))
                                             : std::nullopt;
 
                    const MapStruct m{
                        .c{
                            .maskOpt = maskVal,
                            .cost = costmap.grid[rot_idx](x, y),
                            .posIdx = index,
                            .rotIdx = rot_idx,
                        },
                        .distanceCalculator = distanceCalculator,
                    };
                    costmap.grid[rot_idx](x, y) = fn(m);
                }
            }
        }
    }
 
    void
    Costmap3DBuilder::applyFnToCostmap(Costmap3D& costmap, const MapFnRobot2D& fn)
    {
        Robot3D distanceRobot = Robot3D::fromSimoxRobot(
            robot,
            Robot3D::Params{.collisionModelName = robotCollisionModelName,
                            .scaleFactor = builderParameters.collisionModelScaleFactor,
                            .primitiveApproximationIDs =
                                builderParameters.primitiveApproximationIDs,
                            .rootNode = costmap.parameters.robotModel.rootNode});
        ARMARX_CHECK(distanceRobot.isInitialized())
            << "Robot3D is not initialized. Make sure to call initializeRobot() before using it!";
        Robot2D robot2d{distanceRobot.robot, distanceRobot.collisionModels};
 
        SceneRepresentation scene3d{
            .staticObjects = filteredObstacles,
            .articulatedObjects = articulatedObjects,
        };
        Scene2D scene2d{scene3d, builderParameters.approx2DignoreVerticesOver};
 
        this->debugOutput2D = {
            .robot = robot2d, .scene = scene2d, .root_T_used_root_2d = root_T_used_root_2d};
 
        if (costmap.mask.has_value())
        {
            ARMARX_VERBOSE << "Costmap3D provides mask.";
        }
 
        ARMARX_CHECK(costmap.grid.size() > 0);
        const std::size_t c_x = costmap.grid[0].rows();
        const std::size_t c_y = costmap.grid[0].cols();
 
        robot->setUpdateVisualization(false);
 
#pragma omp parallel for schedule(static) shared(robot2d) default(shared)
        for (unsigned int x = 0; x < c_x; x++)
        {
            for (unsigned int y = 0; y < c_y; y++)
            {
                for (int rot_idx = 0; rot_idx < parameters.orientations; rot_idx++)
                {
                    const Costmap3D::Index index{x, y};
 
                    const auto maskVal = costmap.mask.has_value()
                                             ? std::make_optional(costmap.mask.value()(x, y))
                                             : std::nullopt;
 
                    const MapStructRobot2D m{
                        .c{
                            .maskOpt = maskVal,
                            .cost = costmap.grid[rot_idx](x, y),
                            .posIdx = index,
                            .rotIdx = rot_idx,
                        },
                        .robot = robot2d,
                        .scene = scene2d,
                    };
                    costmap.grid[rot_idx](x, y) = fn(m);
                }
            }
        }
    }
 
    VirtualRobot::SceneObjectSetPtr
    Costmap3DBuilder::filterObjectsForCostmap(const Costmap3D& costmap)
    {
        armarx::core::time::ScopedStopWatch sw(
            [](const armarx::core::time::Duration& duration)
            { ARMARX_INFO << "Filtering objects took " << duration; });
 
        VirtualRobot::SceneObjectSetPtr filtered(new VirtualRobot::SceneObjectSet);
 
        using Point = util::geometry::point_type;
        using Box = boost::geometry::model::box<Point>;
        using Polygon = util::geometry::polygon_type;
 
        const auto toPoint = [](const Eigen::Vector3f& vec) { return Point(vec.x(), vec.y()); };
        const auto toBox = [&toPoint](const VirtualRobot::BoundingBox& bb)
        { return Box(toPoint(bb.getMin()), toPoint(bb.getMax())); };
 
        Polygon costmapBB;
        {
            ARMARX_CHECK(costmap.grid.size() > 0);
            Polygon cornerPoints = util::geometry::toPolygon(std::vector<Eigen::Vector2f>{
                costmap.toPositionGlobal(Costmap::Index(0, 0)),
                costmap.toPositionGlobal(Costmap::Index(0, costmap.grid[0].cols() - 1)),
                costmap.toPositionGlobal(Costmap::Index(costmap.grid[0].rows() - 1, 0)),
                costmap.toPositionGlobal(
                    Costmap::Index(costmap.grid[0].rows() - 1, costmap.grid[0].cols() - 1))});
            boost::geometry::convex_hull(cornerPoints, costmapBB);
        }
 
 
        for (const auto& object : this->obstacles->getSceneObjects())
        {
            const auto& colModel = object->getCollisionModel();
 
            if (colModel)
            {
                Box bb = toBox(colModel->getGlobalBoundingBox());
 
                // if bb of object is disjoint to bb of costmap we check the distance between them
                // otherwise, we always include the object
                if (boost::geometry::disjoint(costmapBB, bb))
                {
                    float distance = boost::geometry::distance(costmapBB, bb);
                    if (distance > builderParameters.maxFilterDistance)
                    {
                        // if the distance is greater than configured, we skip this object
                        continue;
                    }
                }
 
                filtered->addSceneObject(object);
            }
        }
        const auto prevSize = this->obstacles->getSize();
        const auto filteredSize = filtered->getSize();
 
        ARMARX_INFO << "Remaining objects for costmap calculation: " << filteredSize << "/"
                    << prevSize << " (" << filteredSize * 100 / static_cast<float>(prevSize) << "%)";
 
        for (const auto& object : filtered->getSceneObjects())
        {
            ARMARX_VERBOSE << "- " << object->getName();
        }
 
 
        return filtered;
    }
 
} // namespace armarx::navigation::algorithms::orientation_aware