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/SceneObjectSet.h>
37 #include <VirtualRobot/Workspace/WorkspaceGrid.h>
46 #include <range/v3/all.hpp>
59 ARMARX_CHECK(not obstacles->getCollisionModels().empty());
61 for (
size_t i = 0; i < obstacles->getCollisionModels().size(); i++)
63 VirtualRobot::BoundingBox bb = obstacles->getCollisionModels()[i]->getBoundingBox();
77 return {.
min = Eigen::Vector2f{extends.minX, extends.minY},
78 .max = Eigen::Vector2f{extends.maxX, extends.maxY}};
82 merge(
const std::vector<SceneBounds>& sceneBounds)
86 const auto expandBounds = [&bounds](
const SceneBounds& sb)
97 ranges::for_each(sceneBounds, expandBounds);
104 toCostmap(
const VirtualRobot::WorkspaceGrid& workspaceGrid)
106 const Costmap::Grid grid(workspaceGrid.getCells().x, workspaceGrid.getCells().y);
110 .cellSize = workspaceGrid.getDiscretizeSize()};
112 return {grid, parameters, sceneBounds};
194 const auto assertSameSize = [&costmaps](
const Costmap& costmap)
200 ranges::for_each(costmaps, assertSameSize);
214 const std::vector<float> weights(costmaps.size(), 1.F / costmaps.size());
219 costmaps.front().params());
222 grid, costmaps.front().params(), costmaps.front().getLocalSceneBounds());
231 ARMARX_CHECK(
false) <<
"This case should already be handled.";
244 ranges::for_each(costmaps,
245 [&mergedCostmap, &addMode](
const auto& costmap)
246 { mergedCostmap.
add(costmap, addMode); });
248 return mergedCostmap;
252 mergeAligned(
const std::vector<Costmap>& costmaps,
const std::vector<float>& weights)
260 costmaps.front().params());
263 grid, costmaps.front().params(), costmaps.front().getLocalSceneBounds());
266 ranges::for_each(ranges::views::zip(costmaps, weights),
267 [&mergedCostmap](
const auto& p)
269 const auto& [costmap, weight] = p;
270 mergedCostmap.
add(costmap, weight);
273 return mergedCostmap;
286 cv::resize(src, dst, cv::Size{0, 0}, scale, scale);
288 Eigen::MatrixXf scaledGrid;
292 std::optional<Costmap::Mask> scaledMask;
293 if (costmap.
getMask().has_value())
300 constexpr
int someInt = 100;
303 someInt * costmap.
getMask().value().cast<std::uint8_t>();
309 cv::resize(maskMat, maskScaled, cv::Size{0, 0}, scale, scale);
315 Eigen::MatrixXi scaledMaskInt;
324 << costmap.
getGrid().cols() <<
")";
325 ARMARX_VERBOSE <<
"Resized to (" << scaledGrid.rows() <<
", " << scaledGrid.cols() <<
")";
334 std::optional<core::Pose2D>
337 const auto sizeX = costmap.
getGrid().cols();
338 const auto sizeY = costmap.
getGrid().rows();
340 constexpr std::size_t maxIterations = 1000;
344 for (std::size_t iteration = 0; iteration < maxIterations; iteration++)
346 const float iX = VirtualRobot::RandomFloat() *
static_cast<float>(sizeX);
347 const float iY = VirtualRobot::RandomFloat() *
static_cast<float>(sizeY);