CostmapBuilder.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 "CostmapBuilder.h"
 
#include <algorithm>
#include <cstddef>
#include <functional>
#include <memory>
#include <optional>
#include <string>
#include <variant>
#include <vector>
 
#include <boost/geometry/algorithms/convex_hull.hpp>
#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/geometries/box.hpp>
 
#include <SimoxUtility/meta/enum/EnumNames.hpp>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Nodes/RobotNode.h> // IWYU pragma: keep
#include <VirtualRobot/Robot.h> // IWYU pragma: keep
#include <VirtualRobot/RobotFactory.h>
#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/util/CPPUtility/trace.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/Room.h>
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/algorithms/util.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/util/geometry.h>
 
#include <omp.h>
 
#if COSTMAP_BUILDER_SIMOX_CONTROL
#include <simox/control/environment/CollisionRobot.h>
#include <simox/control/impl/simox/robot/Robot.h>
#include <simox/control/impl/simox/utils/conversion.h>
 
#include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h>
#endif
 
namespace armarx::navigation::algorithms
{
 
#if COSTMAP_BUILDER_SIMOX_CONTROL
    namespace sc = simox::control;
#endif
 
 
    // helper type for the visitor
    template <class... Ts>
    struct overloaded : Ts...
    {
        using Ts::operator()...;
    };
    // explicit deduction guide (not needed as of C++20)
    template <class... Ts>
    overloaded(Ts...) -> overloaded<Ts...>;
 
    const simox::meta::EnumNames<CostmapBuilder::DistanceCalculator>
        CostmapBuilder::DistanceCalculatorNames{
            {CostmapBuilder::DistanceCalculator::SimoxControl, "SimoxControl"},
            {CostmapBuilder::DistanceCalculator::Simox, "Simox"}};
 
    CostmapBuilder::CostmapBuilder(const VirtualRobot::RobotPtr& robot,
                                   const VirtualRobot::SceneObjectSetPtr& obstacles,
                                   const std::vector<VirtualRobot::RobotPtr>& articulatedObjects,
                                   const std::vector<Room>& rooms,
                                   const Costmap::Parameters& parameters,
                                   const std::string& robotCollisonModelName,
                                   const CostmapBuilderParams& 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);
        ARMARX_CHECK(robot->hasRobotNode(robotCollisionModelName));
 
        ARMARX_CHECK_NONNEGATIVE(this->builderParameters.numThreads);
 
        const auto& enabledRooms = this->builderParameters.roomEnableList;
        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";
        }
    }
 
    Costmap
    CostmapBuilder::create(const SceneBounds& init)
    {
        armarx::core::time::ScopedStopWatch sw(
            [](const armarx::core::time::Duration& duration)
            { ARMARX_INFO << "Creating costmap 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 << "Created grid";
        Costmap 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 costmap took " << duration; });
 
            ARMARX_VERBOSE << "Filling grid with size (" << costmap.getGrid().rows() << "/"
                           << costmap.getGrid().cols() << ") and resolution "
                           << costmap.params().cellSize;
            fillGridCosts(costmap);
            ARMARX_VERBOSE << "Filled grid";
        }
 
        updateMask(costmap);
        ARMARX_VERBOSE << "Initialized mask";
 
        return costmap;
    }
 
    Costmap
    CostmapBuilder::extend(Costmap costmap)
    {
        armarx::core::time::ScopedStopWatch sw(
            [](const armarx::core::time::Duration& duration)
            { ARMARX_INFO << "Extending costmap took " << duration; });
 
        if (builderParameters.restrictToRooms)
        {
            // mask out any cell not within a room
            ARMARX_VERBOSE << "Restricting to rooms";
            invalidateOutsideRooms(rooms, costmap);
        }
 
        {
            armarx::core::time::ScopedStopWatch sw(
                [](const armarx::core::time::Duration& duration)
                { ARMARX_INFO << "Filling costmap took " << duration; });
 
            ARMARX_VERBOSE << "Extending grid with size (" << costmap.getGrid().rows() << "/"
                           << costmap.getGrid().cols() << ") and resolution "
                           << costmap.params().cellSize;
            extendGridCosts(costmap);
            ARMARX_VERBOSE << "Filled grid";
        }
 
        updateMask(costmap);
        ARMARX_VERBOSE << "Initialized mask";
 
        return costmap;
    }
 
    void
    CostmapBuilder::updateMask(Costmap& costmap)
    {
        costmap.mask = costmap.grid.array() > 0.F;
 
        ARMARX_VERBOSE << "Update mask: Fraction of valid elements: "
                       << costmap.mask->cast<float>().sum() / costmap.mask->size();
    }
 
    void
    CostmapBuilder::initializeEmptyMask(Costmap& costmap)
    {
        costmap.mask = costmap.grid.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();
    }
 
    Eigen::MatrixXf
    CostmapBuilder::createUniformGrid(const SceneBounds& sceneBounds,
                                      const Costmap::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
    CostmapBuilder::computeCost(const Costmap::Position& position,
                                const CollisionSetup& collisionSetupVariant)
    {
        float cost = std::visit(
            overloaded{
                [&position](const CollisionSetupSC& collisionSetup) -> float
                {
        // SimoxControl implementation to calculate distance
#if COSTMAP_BUILDER_SIMOX_CONTROL
                    ARMARX_CHECK_NOT_NULL(collisionSetup.robot);
                    ARMARX_CHECK_NOT_NULL(collisionSetup.collisionRobot);
                    ARMARX_CHECK_NOT_NULL(collisionSetup.obstacleCollisionManager);
 
                    // Remember: SimoxControl uses meters.
                    const core::Pose globalPose(Eigen::Translation3f(conv::to3D(position)));
                    collisionSetup.robot->setGlobalPose(
                        sc::simox::utils::from_simox(globalPose.matrix()), true);
 
                    collisionSetup.collisionRobot->update();
 
                    for (const auto& o : collisionSetup.colObjects)
                    {
                        ARMARX_CHECK_NOT_NULL(o);
                        o->update();
                    }
 
                    collisionSetup.collisionRobot->getCollisionManager()->update();
                    collisionSetup.obstacleCollisionManager->update();
 
                    hpp::fcl::DistanceCallBackDefault defaultCallback;
                    collisionSetup.obstacleCollisionManager->distance(
                        collisionSetup.collisionRobot->getCollisionManager(), &defaultCallback);
 
                    const double minDistance = defaultCallback.data.result.min_distance;
 
                    return static_cast<float>(std::max(minDistance, 0.) * 1000);
#else
                    ARMARX_ERROR
                        << "SimoxControl distance calculation requested but not supported. "
                           "Please compile with SimoxControl to use this calculation method.";
                    return 0.F;
#endif
                },
                [&position, this](const CollisionSetupSx& collisionSetup) -> float
                {
                    const auto& collisionRobot = collisionSetup.collisionRobot;
                    const auto& robotCollisionModel = collisionSetup.robotCollisionModel;
 
                    ARMARX_TRACE;
                    ARMARX_CHECK_NOT_NULL(collisionRobot);
                    ARMARX_CHECK_NOT_NULL(robotCollisionModel);
                    ARMARX_CHECK_NOT_NULL(
                        VirtualRobot::CollisionChecker::getGlobalCollisionChecker());
 
                    const VirtualRobot::SceneObjectSetPtr& actualObstacles = [&]()
                    {
                        if (collisionSetup.filteredObstacles)
                        {
                            return collisionSetup.filteredObstacles;
                        }
                        else
                        {
                            return this->obstacles;
                        }
                    }();
                    ARMARX_CHECK_NOT_NULL(actualObstacles);
 
                    const core::Pose globalPose(Eigen::Translation3f(conv::to3D(position)));
                    collisionRobot->setGlobalPose(globalPose.matrix());
 
                    // distance to non-articulated objects
                    float distanceNonArticulated = std::numeric_limits<float>::max();
                    if (actualObstacles->getSize() > 0)
                    {
                        distanceNonArticulated =
                            VirtualRobot::CollisionChecker::getGlobalCollisionChecker()
                                ->calculateDistance(robotCollisionModel, actualObstacles);
                    }
 
                    // distance to articulated objects
                    float distanceArticulatedMin = std::numeric_limits<float>::max();
                    for (const auto& articulatedObject : articulatedObjects)
                    {
                        for (const auto& colModel : articulatedObject->getCollisionModels())
                        {
                            const float distanceArticulated =
                                VirtualRobot::CollisionChecker::getGlobalCollisionChecker()
                                    ->calculateDistance(robotCollisionModel, colModel);
                            distanceArticulatedMin =
                                std::min(distanceArticulated, distanceArticulatedMin);
                        }
                    }
 
                    return std::min(distanceNonArticulated, distanceArticulatedMin);
 
 
                    // Eigen::Vector3f P1;
                    // Eigen::Vector3f P2;
                    // int id1, id2;
 
                    // float minDistance = std::numeric_limits<float>::max();
 
                    // // TODO omp...
                    // for (size_t i = 0; i < obstacles->getSize(); i++)
                    // {
                    //     // cheap collision check
                    //     // VirtualRobot::BoundingBox obstacleBbox =
                    //     //     obstacles->getSceneObject(i)->getCollisionModel()->getBoundingBox(true);
                    //     // if (not intersects(robotBbox, obstacleBbox))
                    //     // {
                    //     //     continue;
                    //     // }
 
                    //     // precise collision check
                    //     const float dist =
                    //         VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
                    //             robotCollisionModel,
                    //             obstacles->getSceneObject(i)->getCollisionModel(),
                    //             P1,
                    //             P2,
                    //             &id1,
                    //             &id2);
 
                    //     // check if objects collide
                    //     if ((dist <= parameters.cellSize / 2) or
                    //         VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->checkCollision(
                    //             robotCollisionModel, obstacles->getSceneObject(i)->getCollisionModel()))
                    //     {
                    //         minDistance = 0;
                    //         break;
                    //     }
 
                    //     minDistance = std::min(minDistance, dist);
                    // }
                    // // return n->position.x() >= sceneBounds.min.x() && n->position.x() <= sceneBounds.max.x() &&
                    // //        n->position.y() >= sceneBounds.min.y() && n->position.y() <= sceneBounds.max.y();
 
                    // return minDistance;
                },
                [](const std::monostate& _) -> float
                {
                    ARMARX_ERROR << "Invalid collision setup";
                    return 0.F;
                }},
            collisionSetupVariant);
 
        // Cap distance at maxDistance
        cost = std::min(cost, builderParameters.maxDistance);
        return cost;
    }
 
    void
    CostmapBuilder::fillGridCosts(Costmap& costmap)
    {
        applyFnToCostmap(costmap,
                         [&](const CollisionSetup& collisionSetup,
                             const std::optional<float> maskOpt,
                             const float /*cost*/,
                             const Costmap::Index& index) -> float
                         {
                             const auto position = costmap.toPositionGlobal(index);
 
                             // consider mask if available and skip if not valid
                             if (maskOpt.has_value())
                             {
                                 if (not maskOpt.value())
                                 {
                                     // important to set a value <= 0 here, so updateMask wont override mask later
                                     return 0.F;
                                 }
                             }
 
                             if( builderParameters.fakeModeForTesting)
                             {
                                return builderParameters.maxDistance; // similar distance  everywhere
                             }
 
                             return computeCost(position, collisionSetup);
                         });
    }
 
    void
    CostmapBuilder::extendGridCosts(Costmap& costmap)
    {
        applyFnToCostmap(
            costmap,
            [&](const CollisionSetup& collisionSetup,
                const std::optional<float> maskOpt,
                const float cost,
                const Costmap::Index& index) -> float
            {
                const auto position = costmap.toPositionGlobal(index);
 
                // consider mask if available and skip if not valid
                // as mask is updated to respect rooms above, this will also ignore areas outside any rooms
                if (maskOpt.has_value())
                {
                    if (not maskOpt.value())
                    {
                        return std::min(0.F, cost);
                    }
                }
 
                if (cost < 0.F)
                {
                    return cost;
                }
 
                return std::min(cost, computeCost(position, collisionSetup));
            });
    }
 
    void
    CostmapBuilder::applyFnToCostmap(Costmap& costmap,
                                     const std::function<float(const CollisionSetup& collisionSetup,
                                                               const std::optional<float>,
                                                               const float,
                                                               const Costmap::Index&)>& fn)
    {
 
        // filter scene objects once, not in every thread
        const VirtualRobot::SceneObjectSetPtr filteredObjects = filterObjectsForCostmap(costmap);
 
        // called from every thread to initialize the private collision data
        const auto initializeCollisionSetup = [&]() -> CollisionSetup
        {
            armarx::core::time::ScopedStopWatch sw(
                [](const armarx::core::time::Duration& duration)
                {
                    ARMARX_INFO << "Initializing collision setup on thread " << omp_get_thread_num()
                                << " took: " << duration;
                });
 
            switch (builderParameters.calculationMethod)
            {
                case DistanceCalculator::SimoxControl:
                {
#if COSTMAP_BUILDER_SIMOX_CONTROL
                    ARMARX_VERBOSE << "Initializing collision setup";
                    CollisionSetupSC collisionSetup;
 
                    using ScRobot = sc::simox::robot::Robot;
                    using ScCollisionRobot = sc::environment::CollisionRobot<hpp::fcl::OBBRSS>;
 
                    // create copy of this robot instance
                    ARMARX_DEBUG << "Copying robot";
                    ARMARX_CHECK_NOT_NULL(robot);
                    VirtualRobot::RobotPtr clonedRobot =
                        robot->clone("collision_robot_" + std::to_string(omp_get_thread_num()));
                    ARMARX_CHECK_NOT_NULL(clonedRobot);
 
                    // replace the standard collision model by our custom one
                    clonedRobot->setPrimitiveApproximationModel({"navigation"}, false);
 
                    clonedRobot->setUpdateVisualization(false);
 
                    // will copy RobotPtr and keep it alive after leaving the scope
                    collisionSetup.robot = ScRobot::CREATE_SIMPLE_WRAPPER(clonedRobot);
                    ARMARX_CHECK_NOT_NULL(collisionSetup.robot);
                    collisionSetup.collisionRobot =
                        std::make_unique<ScCollisionRobot>(*collisionSetup.robot,
                                                           std::string(),
                                                           false,
                                                           std::vector<std::string>{"navigation"});
 
                    collisionSetup.obstacleCollisionManager =
                        std::make_unique<hpp::fcl::DynamicAABBTreeCollisionManager>();
 
                    const auto convertVRObstacle =
                        [&collisionSetup](const VirtualRobot::RobotPtr& r)
                    {
                        const auto& obstacle = collisionSetup.scObstacles.emplace_back(
                            ScRobot::CREATE_SIMPLE_WRAPPER(r));
 
                        try
                        {
                            const auto& colObject = collisionSetup.colObjects.emplace_back(
                                std::make_unique<ScCollisionRobot>(*obstacle));
 
                            // add collision objects to manager
                            for (auto& node : colObject->getNodes())
                            {
                                for (std::size_t i = 0; i < node.size(); i++)
                                {
                                    collisionSetup.obstacleCollisionManager->registerObject(
                                        &node.getColObject(i));
                                }
                            }
                        }
                        catch (const std::exception& ex)
                        {
#pragma omp master
                            ARMARX_VERBOSE << "Failed to convert object `" << r->getName()
                                           << "` to collision robot.\n"
                                           << ex.what();
                        }
                    };
 
                    // static obstacles first have to be converted to a Robot, then converted to simox control
                    for (const auto& o : this->obstacles->getSceneObjects())
                    {
                        // Here, the scene objects are ManipulationObjects
                        const auto casted =
                            std::dynamic_pointer_cast<VirtualRobot::GraspableSensorizedObject>(o);
                        ARMARX_CHECK_NOT_NULL(casted)
                            << "Scene object is not a graspable sensorized object";
                        const VirtualRobot::RobotPtr asRobot =
                            VirtualRobot::RobotFactory::createRobot(*casted);
                        convertVRObstacle(asRobot);
                    }
 
                    // articulated objects already are Robots, thus they can be immediately converted to simox control
                    for (const auto& o : this->articulatedObjects)
                    {
                        convertVRObstacle(o);
                    }
 
                    // only print once on the main thread
#pragma omp master
                    ARMARX_INFO << "Converted " << collisionSetup.colObjects.size() << "/"
                                << (this->obstacles->getSize() + this->articulatedObjects.size())
                                << " in total";
 
                    collisionSetup.obstacleCollisionManager->setup();
 
                    return collisionSetup;
#else
                    ARMARX_ERROR
                        << "SimoxControl distance calculation requested but not supported. "
                           "Please compile with SimoxControl to use this calculation method.";
                    return {};
#endif
                }
                case DistanceCalculator::Simox:
                {
                    CollisionSetupSx collisionSetup;
 
                    ARMARX_DEBUG << "Copying robot";
                    ARMARX_CHECK_NOT_NULL(robot);
                    collisionSetup.collisionRobot =
                        robot->clone("collision_robot_" + std::to_string(omp_get_thread_num()));
 
                    // replace the standard collision model by our custom one
                    collisionSetup.collisionRobot->setPrimitiveApproximationModel({"navigation"},
                                                                                  false);
 
                    collisionSetup.collisionRobot->setUpdateVisualization(false);
                    ARMARX_DEBUG << "Copying done";
 
                    ARMARX_CHECK_NOT_NULL(collisionSetup.collisionRobot);
                    ARMARX_CHECK(
                        collisionSetup.collisionRobot->hasRobotNode(robotCollisionModelName));
 
                    const auto collisionRobotNode =
                        collisionSetup.collisionRobot->getRobotNode(robotCollisionModelName);
                    ARMARX_CHECK_NOT_NULL(collisionRobotNode);
 
                    collisionSetup.robotCollisionModel = collisionRobotNode->getCollisionModel();
                    ARMARX_CHECK_NOT_NULL(collisionSetup.robotCollisionModel)
                        << "Collision model not available. "
                           "Make sure that you load the robot correctly!";
 
                    // scale collision model
                    collisionSetup.robotCollisionModel->scale(
                        builderParameters.collisionModelScaleFactor);
 
                    collisionSetup.filteredObstacles = filteredObjects;
 
                    ARMARX_CHECK_NOT_NULL(collisionSetup.collisionRobot);
                    ARMARX_CHECK_NOT_NULL(collisionSetup.robotCollisionModel);
 
                    return collisionSetup;
                }
                default:
                    ARMARX_ERROR << "Invalid distance calculator specified";
                    return {};
            }
        };
 
        if (costmap.mask.has_value())
        {
            ARMARX_VERBOSE << "Costmap provides mask.";
        }
 
        const std::size_t c_x = costmap.grid.rows();
        const std::size_t c_y = costmap.grid.cols();
 
        robot->setUpdateVisualization(false);
 
        // a per-thread local variable (through `private` directive below)
        CollisionSetup collisionSetup;
 
        const int threadNumDefault = omp_get_max_threads();
        const int actualThreads = this->builderParameters.numThreads == 0
                                      ? threadNumDefault
                                      : this->builderParameters.numThreads;
        ARMARX_INFO << "Using " << actualThreads << " threads.";
 
 
#pragma omp parallel for num_threads(actualThreads)                                                \
    schedule(static) private(collisionSetup) default(shared)
        for (unsigned int x = 0; x < c_x; x++)
        {
            // any exception (like those generated by ARMARX_CHECK) need to be caught in the executing thread
            try
            {
 
                // we have to initialize the collision setup for each thread
                if (std::holds_alternative<std::monostate>(collisionSetup))
                {
                    collisionSetup = initializeCollisionSetup();
                    ARMARX_VERBOSE << "Collision setup created successfully";
                }
                ARMARX_CHECK(not std::holds_alternative<std::monostate>(collisionSetup));
 
 
                for (unsigned int y = 0; y < c_y; y++)
                {
                    const Costmap::Index index{x, y};
 
                    const auto maskVal = costmap.mask.has_value()
                                             ? std::make_optional(costmap.mask.value()(x, y))
                                             : std::nullopt;
                    costmap.grid(x, y) = fn(collisionSetup, maskVal, costmap.grid(x, y), index);
                }
            }
            catch (const std::exception& e)
            {
                ARMARX_ERROR << "Error during costmap calculation: " << e.what();
            }
        }
    }
 
    VirtualRobot::SceneObjectSetPtr
    CostmapBuilder::filterObjectsForCostmap(const Costmap& 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;
        {
            Polygon cornerPoints = util::geometry::toPolygon(std::vector<Eigen::Vector2f>{
                costmap.toPositionGlobal(Costmap::Index(0, 0)),
                costmap.toPositionGlobal(Costmap::Index(0, costmap.grid.cols() - 1)),
                costmap.toPositionGlobal(Costmap::Index(costmap.grid.rows() - 1, 0)),
                costmap.toPositionGlobal(
                    Costmap::Index(costmap.grid.rows() - 1, costmap.grid.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 std::size_t prevSize = this->obstacles->getSize();
        const std::size_t filteredSize = filtered->getSize();
 
        ARMARX_INFO << "Remaining objects for costmap calculation: " << filteredSize << "/"
                    << prevSize << " ("
                    << static_cast<float>(filteredSize) * 100.f / static_cast<float>(prevSize)
                    << "%)";
 
        for (const auto& object : filtered->getSceneObjects())
        {
            ARMARX_VERBOSE << "- " << object->getName();
        }
 
 
        return filtered;
    }
 
} // namespace armarx::navigation::algorithms