util.cpp
Go to the documentation of this file.
1 /**
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @author Fabian Reister ( fabian dot reister at kit dot edu )
17  * @date 2022
18  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
19  * GNU General Public License
20  */
21 
22 #include "util.h"
23 
24 #include <algorithm>
25 #include <cstddef>
26 #include <iterator>
27 
28 #include <opencv2/core.hpp>
29 #include <opencv2/core/eigen.hpp>
30 #include <opencv2/imgproc.hpp>
31 
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>
40 
43 
48 
49 #include <range/v3/all.hpp>
50 
52 {
53 
54 
55  SceneBounds
56  computeSceneBounds(const VirtualRobot::SceneObjectSetPtr& obstacles,
57  const std::vector<VirtualRobot::RobotPtr>& articulatedObjects,
58  const SceneBounds& init,
59  const float margin)
60  {
61  SceneBounds bounds = init;
62 
63  ARMARX_CHECK_NOT_NULL(obstacles);
64  ARMARX_CHECK(not obstacles->getCollisionModels().empty());
65 
66  const auto expandBounds = [&bounds](const VirtualRobot::BoundingBox&& bb)
67  {
68  bounds.min.x() = std::min(bb.getMin().x(), bounds.min.x());
69  bounds.min.y() = std::min(bb.getMin().y(), bounds.min.y());
70  bounds.max.x() = std::max(bb.getMax().x(), bounds.max.x());
71  bounds.max.y() = std::max(bb.getMax().y(), bounds.max.y());
72  };
73 
74  // non-articulated objects
75  for (const auto& colModel : obstacles->getCollisionModels())
76  {
77  expandBounds(colModel->getBoundingBox());
78  }
79 
80  ARMARX_VERBOSE << "non-articulated objects: scene bounds: " << bounds.min << " and "
81  << bounds.max;
82 
83  // articulated objects
84  for (const auto& articulatedObject : articulatedObjects)
85  {
86  if (articulatedObject->getCollisionModels().empty())
87  {
88  ARMARX_WARNING << "The articulated object `" << articulatedObject->getType() << "/"
89  << articulatedObject->getName()
90  << "` does not provide any collision model!";
91  }
92 
93  for (const auto& colModel : articulatedObject->getCollisionModels())
94  {
95  expandBounds(colModel->getBoundingBox());
96  }
97  }
98 
99  ARMARX_VERBOSE << "articulated objects: scene bounds: " << bounds.min << " and "
100  << bounds.max;
101 
102  // expand bounds by margin
103  bounds.min.x() -= margin;
104  bounds.min.y() -= margin;
105  bounds.max.x() += margin;
106  bounds.max.y() += margin;
107 
108  return bounds;
109  }
110 
111  SceneBounds
112  toSceneBounds(const VirtualRobot::WorkspaceGrid::Extends& extends)
113  {
114  return {.min = Eigen::Vector2f{extends.minX, extends.minY},
115  .max = Eigen::Vector2f{extends.maxX, extends.maxY}};
116  }
117 
118  SceneBounds
119  merge(const std::vector<SceneBounds>& sceneBounds)
120  {
121  SceneBounds bounds;
122 
123  const auto expandBounds = [&bounds](const SceneBounds& sb)
124  {
125  bounds.min.x() = std::min(bounds.min.x(), sb.min.x());
126  bounds.min.y() = std::min(bounds.min.y(), sb.min.y());
127 
128  bounds.max.x() = std::max(bounds.max.x(), sb.max.x());
129  bounds.max.y() = std::max(bounds.max.y(), sb.max.y());
130 
131  return bounds;
132  };
133 
134  ranges::for_each(sceneBounds, expandBounds);
135 
136  return bounds;
137  }
138 
139  Costmap
140  toCostmap(const VirtualRobot::WorkspaceGrid& workspaceGrid)
141  {
142  const Costmap::Grid grid(workspaceGrid.getCells().x, workspaceGrid.getCells().y);
143  const SceneBounds sceneBounds = toSceneBounds(workspaceGrid.getExtends());
144 
145  const Costmap::Parameters parameters{.binaryGrid = false,
146  .cellSize = workspaceGrid.getDiscretizeSize()};
147 
148  return {grid, parameters, sceneBounds};
149  }
150 
151  // Costmap
152  // mergeUnaligned(const std::vector<Costmap>& costmaps,
153  // const std::vector<float>& weights,
154  // float cellSize,
155  // const Eigen::Array2f& offset)
156  // {
157  // ARMARX_TRACE;
158 
159  // ARMARX_CHECK_NOT_EMPTY(costmaps);
160  // ARMARX_CHECK_EQUAL(costmaps.size(), weights.size());
161 
162  // // if unset, use the smallest cell size
163  // if (cellSize < 0)
164  // {
165  // const auto compCellSize = [](const Costmap& a, const Costmap& b) -> bool
166  // { return a.params().cellSize < b.params().cellSize; };
167 
168  // cellSize = ranges::min_element(costmaps, compCellSize)->params().cellSize;
169  // }
170 
171  // // scale each costmap
172  // std::vector<Costmap> scaledCostmaps =
173  // simox::alg::apply(costmaps,
174  // [&cellSize](const Costmap& costmap) -> Costmap
175  // { return scaleCostmap(costmap, cellSize); });
176 
177  // // merge costmaps into one
178  // ARMARX_VERBOSE << "Merging " << scaledCostmaps.size() << " costmaps";
179 
180  // // - combine scene bounds
181  // std::vector<SceneBounds> sceneBoundsAll = simox::alg::apply(
182  // scaledCostmaps, [](const Costmap& costmap) { return costmap.getSceneBounds(); });
183 
184  // SceneBounds sceneBounds = merge(sceneBoundsAll);
185  // sceneBounds.min.x() -= 0.5 * cellSize;
186  // sceneBounds.max.x() -= 0.5 * cellSize;
187  // sceneBounds.min.y() -= cellSize;
188  // sceneBounds.max.y() -= cellSize;
189 
190 
191  // sceneBounds.min.x() += offset.x();
192  // sceneBounds.max.x() += offset.x();
193  // sceneBounds.min.y() += offset.y();
194  // sceneBounds.max.y() += offset.y();
195 
196  // // - create grid
197  // const auto largeGrid = CostmapBuilder::createUniformGrid(
198  // sceneBounds, Costmap::Parameters{.binaryGrid = false, .cellSize = cellSize});
199 
200  // Costmap largeCostmap(
201  // largeGrid, Costmap::Parameters{.binaryGrid = false, .cellSize = cellSize}, sceneBounds);
202 
203  // // TODO save each combo individually
204 
205  // static int i = 0;
206  // // - add all costmaps to this one
207  // ranges::for_each(ranges::views::zip(scaledCostmaps, weights),
208  // [&largeCostmap](const auto& p)
209  // {
210  // ARMARX_INFO << "adding to costmap";
211  // const auto& [costmap, weight] = p;
212  // largeCostmap.add(costmap, weight);
213 
214  // armarx::navigation::algorithms::save(
215  // costmap, "/tmp/costmap" + std::to_string(i++));
216  // });
217 
218  // armarx::navigation::algorithms::save(largeCostmap,
219  // "/tmp/large-costmap" + std::to_string(i++));
220 
221  // return largeCostmap;
222  // }
223 
224  void
225  checkSameSize(const std::vector<Costmap>& costmaps)
226  {
227  ARMARX_CHECK_NOT_EMPTY(costmaps);
228 
229  const auto assertSameSize = [&costmaps](const Costmap& costmap)
230  {
231  ARMARX_CHECK_EQUAL(costmap.getGrid().rows(), costmaps.front().getGrid().rows());
232  ARMARX_CHECK_EQUAL(costmap.getGrid().rows(), costmaps.front().getGrid().rows());
233  };
234 
235  ranges::for_each(costmaps, assertSameSize);
236  }
237 
238  Costmap
239  mergeAligned(const std::vector<Costmap>& costmaps, CostmapMergeMode mode)
240  {
241  ARMARX_CHECK_NOT_EMPTY(costmaps);
242  checkSameSize(costmaps);
243 
244 
245  // average mode: fixed weight
246  if (mode == CostmapMergeMode::AVERAGE)
247  {
248  const std::vector<float> weights(costmaps.size(), 1.F / costmaps.size());
249  return mergeAligned(costmaps, weights);
250  }
251 
252  const auto grid = CostmapBuilder::createUniformGrid(costmaps.front().getLocalSceneBounds(),
253  costmaps.front().params());
254 
255  Costmap mergedCostmap(
256  grid, costmaps.front().params(), costmaps.front().getLocalSceneBounds());
257 
258  const auto addMode = [&]() -> Costmap::AddMode
259  {
260  Costmap::AddMode addMode{};
261 
262  switch (mode)
263  {
265  ARMARX_CHECK(false) << "This case should already be handled.";
266  break;
268  addMode = Costmap::AddMode::MIN;
269  break;
271  addMode = Costmap::AddMode::MAX;
272  break;
273  }
274 
275  return addMode;
276  }();
277 
278  ranges::for_each(costmaps,
279  [&mergedCostmap, &addMode](const auto& costmap)
280  { mergedCostmap.add(costmap, addMode); });
281 
282  return mergedCostmap;
283  }
284 
285  Costmap
286  mergeAligned(const std::vector<Costmap>& costmaps, const std::vector<float>& weights)
287  {
288  ARMARX_CHECK_EQUAL(costmaps.size(), weights.size());
289  ARMARX_CHECK_NOT_EMPTY(costmaps);
290  checkSameSize(costmaps);
291 
292 
293  const auto grid = CostmapBuilder::createUniformGrid(costmaps.front().getLocalSceneBounds(),
294  costmaps.front().params());
295 
296  Costmap mergedCostmap(
297  grid, costmaps.front().params(), costmaps.front().getLocalSceneBounds());
298 
299  // foreach pair (costmap, weight): add it to there merged costmap
300  ranges::for_each(ranges::views::zip(costmaps, weights),
301  [&mergedCostmap](const auto& p)
302  {
303  const auto& [costmap, weight] = p;
304  mergedCostmap.add(costmap, weight);
305  });
306 
307  return mergedCostmap;
308  }
309 
310  Costmap
311  scaleCostmap(const Costmap& costmap, float cellSize)
312  {
313  const float scale = costmap.params().cellSize / cellSize;
314  ARMARX_VERBOSE << "Scaling grid by a factor of " << scale;
315 
316  cv::Mat src;
317  cv::eigen2cv(costmap.getGrid(), src);
318 
319  cv::Mat dst;
320  cv::resize(src, dst, cv::Size{0, 0}, scale, scale);
321 
322  Eigen::MatrixXf scaledGrid;
323  cv::cv2eigen(dst, scaledGrid);
324 
325 
326  std::optional<Costmap::Mask> scaledMask;
327  if (costmap.getMask().has_value())
328  {
329  ARMARX_TRACE;
330  ARMARX_VERBOSE << "Scaling mask by a factor of " << scale;
331 
332  // just an int large enough to check if resizing causes
333  // issues on boundary to invalid regions
334  constexpr int someInt = 100;
335 
337  someInt * costmap.getMask().value().cast<std::uint8_t>();
338 
339  cv::Mat maskMat;
340  cv::eigen2cv(mask, maskMat);
341 
342  cv::Mat maskScaled;
343  cv::resize(maskMat, maskScaled, cv::Size{0, 0}, scale, scale);
344 
345  // only those points are valid where the original value '10' is preserved.
346  // all other points are boundary points to the invalid area
347  // cv::threshold(maskMat, maskMat, 10.9, 1, cv::THRESH_BINARY);
348 
349  Eigen::MatrixXi scaledMaskInt; //(scaledGrid.rows(), scaledGrid.cols());
350  cv::cv2eigen(maskScaled, scaledMaskInt);
351 
352  Costmap::Mask m = scaledMaskInt.array() >= someInt;
353 
354  scaledMask = m;
355  }
356 
357  ARMARX_VERBOSE << "Original size (" << costmap.getGrid().rows() << ", "
358  << costmap.getGrid().cols() << ")";
359  ARMARX_VERBOSE << "Resized to (" << scaledGrid.rows() << ", " << scaledGrid.cols() << ")";
360 
361  return {scaledGrid,
362  Costmap::Parameters{.binaryGrid = false, .cellSize = cellSize},
363  costmap.getLocalSceneBounds(),
364  scaledMask};
365  }
366 
367  std::optional<core::Pose2D>
369  {
370  const auto sizeX = costmap.getGrid().cols();
371  const auto sizeY = costmap.getGrid().rows();
372 
373  constexpr std::size_t maxIterations = 1000;
374 
375  // sample a valid pose in the costmap
376 
377  for (std::size_t iteration = 0; iteration < maxIterations; iteration++)
378  {
379  const float iX = VirtualRobot::RandomFloat() * static_cast<float>(sizeX);
380  const float iY = VirtualRobot::RandomFloat() * static_cast<float>(sizeY);
381 
382  algorithms::Costmap::Index idx(iX, iY);
383 
384  if (not costmap.isValid(idx))
385  {
386  continue;
387  }
388 
390  pose.translation() = costmap.toPositionGlobal(idx);
391  return pose;
392  }
393 
394  ARMARX_ERROR << "Failed to sample pose in costmap!";
395  return std::nullopt;
396  }
397 } // namespace armarx::navigation::algorithms
armarx::navigation::algorithms::Costmap::getMask
const std::optional< Mask > & getMask() const noexcept
Definition: Costmap.cpp:392
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::navigation::algorithms::Costmap::Index
Eigen::Array2i Index
Definition: Costmap.h:46
armarx::navigation::algorithms::CostmapMergeMode
CostmapMergeMode
Defines how the.
Definition: util.h:58
armarx::navigation::algorithms::Costmap::AddMode::MAX
@ MAX
util.h
armarx::navigation::algorithms::merge
SceneBounds merge(const std::vector< SceneBounds > &sceneBounds)
Definition: util.cpp:119
CostmapBuilder.h
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
armarx::navigation::algorithms::CostmapMergeMode::AVERAGE
@ AVERAGE
armarx::navigation::algorithms
This file is part of ArmarX.
Definition: aron_conversions.cpp:13
armarx::navigation::algorithms::checkSameSize
void checkSameSize(const std::vector< Costmap > &costmaps)
Definition: util.cpp:225
armarx::navigation::algorithms::Costmap::Grid
Eigen::MatrixXf Grid
Definition: Costmap.h:48
armarx::navigation::algorithms::mergeAligned
Costmap mergeAligned(const std::vector< Costmap > &costmaps, CostmapMergeMode mode)
Definition: util.cpp:239
ARMARX_CHECK_NOT_EMPTY
#define ARMARX_CHECK_NOT_EMPTY(c)
Definition: ExpressionException.h:224
persistence.h
armarx::navigation::algorithms::Costmap::toPositionGlobal
Position toPositionGlobal(const Index &index) const
Definition: Costmap.cpp:71
armarx::navigation::algorithms::CostmapBuilder::createUniformGrid
static Eigen::MatrixXf createUniformGrid(const SceneBounds &sceneBounds, const Costmap::Parameters &parameters)
Definition: CostmapBuilder.cpp:91
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::navigation::algorithms::Costmap::getLocalSceneBounds
const SceneBounds & getLocalSceneBounds() const noexcept
Definition: Costmap.cpp:126
armarx::navigation::core::Pose2D
Eigen::Isometry2f Pose2D
Definition: basic_types.h:34
Costmap.h
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::navigation::algorithms::Costmap::Parameters::binaryGrid
bool binaryGrid
Definition: Costmap.h:24
armarx::navigation::algorithms::Costmap::AddMode::MIN
@ MIN
armarx::navigation::algorithms::Costmap::params
const Parameters & params() const noexcept
Definition: Costmap.cpp:230
armarx::navigation::algorithms::computeSceneBounds
SceneBounds computeSceneBounds(const VirtualRobot::SceneObjectSetPtr &obstacles, const std::vector< VirtualRobot::RobotPtr > &articulatedObjects, const SceneBounds &init, const float margin)
Definition: util.cpp:56
armarx::navigation::algorithms::scaleCostmap
Costmap scaleCostmap(const Costmap &costmap, float cellSize)
Definition: util.cpp:311
armarx::navigation::algorithms::Costmap::getGrid
const Grid & getGrid() const
Definition: Costmap.cpp:236
armarx::conversions::cv2eigen
Eigen::Vector2f cv2eigen(const cv::Point2f &pt)
Definition: opencv_eigen.h:47
armarx::navigation::util::articulatedObjects
objpose::ObjectPoseSeq articulatedObjects(objpose::ObjectPoseSeq objects)
Definition: util.cpp:95
max
T max(T t1, T t2)
Definition: gdiam.h:48
armarx::navigation::algorithms::SceneBounds
Definition: types.h:29
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
ExpressionException.h
armarx::navigation::algorithms::CostmapMergeMode::MAX
@ MAX
armarx::navigation::algorithms::CostmapMergeMode::MIN
@ MIN
armarx::navigation::algorithms::SceneBounds::min
Eigen::Vector2f min
Definition: types.h:31
armarx::navigation::algorithms::Costmap::AddMode
AddMode
Definition: Costmap.h:108
armarx::navigation::algorithms::Costmap::Parameters::cellSize
float cellSize
How big each cell is in the uniform grid.
Definition: Costmap.h:27
armarx::navigation::algorithms::sampleValidPositionInMap
std::optional< core::Pose2D > sampleValidPositionInMap(const algorithms::Costmap &costmap)
Definition: util.cpp:368
armarx::navigation::algorithms::Costmap::Parameters
Definition: Costmap.h:21
Eigen::Matrix
Definition: EigenForwardDeclarations.h:27
armarx::navigation::algorithms::toSceneBounds
SceneBounds toSceneBounds(const VirtualRobot::WorkspaceGrid::Extends &extends)
Definition: util.cpp:112
armarx::navigation::algorithms::toCostmap
Costmap toCostmap(const VirtualRobot::WorkspaceGrid &workspaceGrid)
Definition: util.cpp:140
Logging.h
min
T min(T t1, T t2)
Definition: gdiam.h:42
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::navigation::algorithms::Costmap::isValid
bool isValid(const Index &index) const noexcept
checks whether the cell is masked out
Definition: Costmap.cpp:78
armarx::navigation::algorithms::Costmap::add
bool add(const Costmap &other, float weight=1.0)
Definition: Costmap.cpp:249
types.h
armarx::navigation::algorithms::SceneBounds::max
Eigen::Vector2f max
Definition: types.h:32
armarx::navigation::algorithms::Costmap
Definition: Costmap.h:13
armarx::conversions::eigen2cv
cv::Point2f eigen2cv(const Eigen::Vector2f &pt)
Definition: opencv_eigen.h:30