28 #include <opencv2/core.hpp>
29 #include <opencv2/core/eigen.hpp>
30 #include <opencv2/imgproc.hpp>
32 #include <SimoxUtility/algorithm/apply.hpp>
33 #include <VirtualRobot/BoundingBox.h>
34 #include <VirtualRobot/CollisionDetection/CollisionModel.h>
35 #include <VirtualRobot/Random.h>
36 #include <VirtualRobot/Robot.h>
37 #include <VirtualRobot/SceneObjectSet.h>
38 #include <VirtualRobot/VirtualRobot.h>
39 #include <VirtualRobot/Workspace/WorkspaceGrid.h>
49 #include <range/v3/all.hpp>
64 ARMARX_CHECK(not obstacles->getCollisionModels().empty());
66 const auto expandBounds = [&bounds](
const VirtualRobot::BoundingBox&& bb)
75 for (
const auto& colModel : obstacles->getCollisionModels())
77 expandBounds(colModel->getBoundingBox());
80 ARMARX_VERBOSE <<
"non-articulated objects: scene bounds: " << bounds.
min <<
" and "
86 if (articulatedObject->getCollisionModels().empty())
88 ARMARX_WARNING <<
"The articulated object `" << articulatedObject->getType() <<
"/"
89 << articulatedObject->getName()
90 <<
"` does not provide any collision model!";
93 for (
const auto& colModel : articulatedObject->getCollisionModels())
95 expandBounds(colModel->getBoundingBox());
103 bounds.
min.x() -= margin;
104 bounds.
min.y() -= margin;
105 bounds.
max.x() += margin;
106 bounds.
max.y() += margin;
114 return {.
min = Eigen::Vector2f{extends.minX, extends.minY},
115 .max = Eigen::Vector2f{extends.maxX, extends.maxY}};
119 merge(
const std::vector<SceneBounds>& sceneBounds)
123 const auto expandBounds = [&bounds](
const SceneBounds& sb)
134 ranges::for_each(sceneBounds, expandBounds);
140 toCostmap(
const VirtualRobot::WorkspaceGrid& workspaceGrid)
142 const Costmap::Grid grid(workspaceGrid.getCells().x, workspaceGrid.getCells().y);
146 .cellSize = workspaceGrid.getDiscretizeSize()};
148 return {grid, parameters, sceneBounds};
229 const auto assertSameSize = [&costmaps](
const Costmap& costmap)
235 ranges::for_each(costmaps, assertSameSize);
248 const std::vector<float> weights(costmaps.size(), 1.F / costmaps.size());
253 costmaps.front().params());
256 grid, costmaps.front().params(), costmaps.front().getLocalSceneBounds());
265 ARMARX_CHECK(
false) <<
"This case should already be handled.";
278 ranges::for_each(costmaps,
279 [&mergedCostmap, &addMode](
const auto& costmap)
280 { mergedCostmap.
add(costmap, addMode); });
282 return mergedCostmap;
286 mergeAligned(
const std::vector<Costmap>& costmaps,
const std::vector<float>& weights)
294 costmaps.front().params());
297 grid, costmaps.front().params(), costmaps.front().getLocalSceneBounds());
300 ranges::for_each(ranges::views::zip(costmaps, weights),
301 [&mergedCostmap](
const auto& p)
303 const auto& [costmap, weight] = p;
304 mergedCostmap.
add(costmap, weight);
307 return mergedCostmap;
320 cv::resize(src, dst, cv::Size{0, 0}, scale, scale);
322 Eigen::MatrixXf scaledGrid;
326 std::optional<Costmap::Mask> scaledMask;
327 if (costmap.
getMask().has_value())
334 constexpr
int someInt = 100;
337 someInt * costmap.
getMask().value().cast<std::uint8_t>();
343 cv::resize(maskMat, maskScaled, cv::Size{0, 0}, scale, scale);
349 Eigen::MatrixXi scaledMaskInt;
358 << costmap.
getGrid().cols() <<
")";
359 ARMARX_VERBOSE <<
"Resized to (" << scaledGrid.rows() <<
", " << scaledGrid.cols() <<
")";
367 std::optional<core::Pose2D>
370 const auto sizeX = costmap.
getGrid().cols();
371 const auto sizeY = costmap.
getGrid().rows();
373 constexpr std::size_t maxIterations = 1000;
377 for (std::size_t iteration = 0; iteration < maxIterations; iteration++)
379 const float iX = VirtualRobot::RandomFloat() *
static_cast<float>(sizeX);
380 const float iY = VirtualRobot::RandomFloat() *
static_cast<float>(sizeY);