Merging in upstream https://github.com/ANYbotics/grid_map.git master

Author: tomlankhorst

README.md

@@ -15,14 +15,14 @@ Features:
 * **Visualizations:** The *grid_map_rviz_plugin* renders grid maps as 3d surface plots (height maps) in [RViz]. Additionally, the *grid_map_visualization* package helps to visualize grid maps as point clouds, occupancy grids, grid cells etc.
 * **Filters:** The *grid_map_filters* provides are range of filters to process grid maps as a sequence of filters. Parsing of mathematical expressions allows to flexibly setup powerful computations such as thresholding, normal vectors, smoothening, variance, inpainting, and matrix kernel convolutions.
 
-The grid map package has been tested with [ROS] Indigo, Jade (under Ubuntu 14.04), Kinetic (under Ubuntu 16.04), and Melodic (under Ubuntu 18.04). This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed.
+This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed.
 
 The source code is released under a [BSD 3-Clause license](LICENSE).
 
 **Author: Péter Fankhauser<br />
 Affiliation: [ANYbotics](https://www.anybotics.com/)<br />
 Maintainer: Péter Fankhauser, pfankhauser@anybotics.com<br />**
-With contributions by: Tanja Baumann, Jeff Delmerico, Remo Diethelm, Perry Franklin, Dominic Jud, Ralph Kaestner, Philipp Krüsi, Alex Millane, Daniel Stonier, Elena Stumm, Martin Wermelinger, Christos Zalidis, Edo Jelavic, Ruben Grandia, Simone Arreghini
+With contributions by: Tanja Baumann, Jeff Delmerico, Remo Diethelm, Perry Franklin, Dominic Jud, Ralph Kaestner, Philipp Krüsi, Alex Millane, Daniel Stonier, Elena Stumm, Martin Wermelinger, Christos Zalidis, Edo Jelavic, Ruben Grandia, Simone Arreghini, Magnus Gärtner
 
 This projected was initially developed at ETH Zurich (Autonomous Systems Lab & Robotic Systems Lab).
 
@@ -152,6 +152,10 @@ The *grid_map_demos* package contains several demonstration nodes. Use this code
         roslaunch grid_map_demos image_to_gridmap_demo.launch
 
     ![Image to grid map demo result](grid_map_demos/doc/image_to_grid_map_demo_result.png)
+    
+* *[grid_map_to_image_demo](grid_map_demos/src/GridmapToImageDemo.cpp)* demonstrates how to save a grid map layer to an image. Start the demonstration with
+
+        rosrun grid_map_demos grid_map_to_image_demo _grid_map_topic:=/grid_map _file:=/home/$USER/Desktop/grid_map_image.png
 
 * *[opencv_demo](grid_map_demos/src/opencv_demo_node.cpp)* demonstrates map manipulations with help of [OpenCV] functions. Start the demonstration with
 
@@ -391,7 +395,20 @@ Several basic filters are provided in the *grid_map_filters* package:
           input_layer: input
           output_layer: output
           radius: 0.06 # in m.
+* **`gridMapFilters/MedianFillFilter`**
+
+    Compute for each _NaN_ cell of a layer the median (of finites) inside a patch with radius. 
+    Optionally, apply median calculations for values that are already finite, the patch radius for these points is given by existing_value_radius. 
 
+        name: median
+        type: gridMapFilters/MedianFillFilter
+        params:
+          input_layer: input
+          output_layer: output
+          fill_hole_radius: 0.11 # in m. 
+          filter_existing_values: false # Default is false. If enabled it also does a median computation for existing values. 
+          existing_value_radius: 0.2 # in m. Note that this option only has an effect if filter_existing_values is set true. 
+    
 * **`gridMapFilters/NormalVectorsFilter`**
 
     Compute the normal vectors of a layer in a map.

grid_map_core/src/GridMapMath.cpp

@@ -219,9 +219,22 @@ void wrapIndexToRange(Index& index, const Size& bufferSize)
 
 void wrapIndexToRange(int& index, int bufferSize)
 {
-  index = index % bufferSize;
-  if (index < 0) {
-    index += bufferSize;
+  // Try shortcuts before resorting to the expensive modulo operation.
+  if (index < bufferSize){
+    if(index >= 0){ // within the wanted range
+      return;
+    } else if(index >= -bufferSize){ // Index is below range, but not more than one span of the range.
+      index +=bufferSize;
+      return;
+    }else{ // Index is largely below range.
+      index = index % bufferSize;
+      index += bufferSize;
+    }
+  }else if(index < bufferSize*2){ // Index is above range, but not more than one span of the range.
+    index -= bufferSize;
+    return;
+  } else{ // Index is largely above range.
+    index = index % bufferSize;
   }
 }
 

grid_map_core/test/SubmapIteratorTest.cpp

@@ -6,8 +6,8 @@
  *	 Institute: ETH Zurich, ANYbotics
  */
 
-#include "grid_map_core/iterators/SubmapIterator.hpp"
 #include "grid_map_core/GridMap.hpp"
+#include "grid_map_core/iterators/SubmapIterator.hpp"
 
 // Eigen
 #include <Eigen/Core>
@@ -25,8 +25,7 @@ using namespace std;
 using namespace Eigen;
 using namespace grid_map;
 
-TEST(SubmapIterator, Simple)
-{
+TEST(SubmapIterator, Simple) {
   Eigen::Array2i submapTopLeftIndex(3, 1);
   Eigen::Array2i submapBufferSize(3, 2);
   Eigen::Array2i index;
@@ -35,7 +34,7 @@ TEST(SubmapIterator, Simple)
   vector<string> types;
   types.push_back("type");
   GridMap map(types);
-  map.setGeometry(Array2d(8.1, 5.1), 1.0, Vector2d(0.0, 0.0)); // bufferSize(8, 5)
+  map.setGeometry(Array2d(8.1, 5.1), 1.0, Vector2d(0.0, 0.0));  // bufferSize(8, 5)
 
   SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize);
 
@@ -88,8 +87,7 @@ TEST(SubmapIterator, Simple)
   EXPECT_EQ(1, iterator.getSubmapIndex()(1));
 }
 
-TEST(SubmapIterator, CircularBuffer)
-{
+TEST(SubmapIterator, CircularBuffer) {
   Eigen::Array2i submapTopLeftIndex(6, 3);
   Eigen::Array2i submapBufferSize(2, 4);
   Eigen::Array2i index;
@@ -98,8 +96,8 @@ TEST(SubmapIterator, CircularBuffer)
   vector<string> types;
   types.push_back("type");
   GridMap map(types);
-  map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
-  map.move(Position(-3.0, -2.0)); // bufferStartIndex(3, 2)
+  map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0));  // bufferSize(8, 5)
+  map.move(Position(-3.0, -2.0));                              // bufferStartIndex(3, 2)
 
   SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize);
 
@@ -165,3 +163,104 @@ TEST(SubmapIterator, CircularBuffer)
   EXPECT_EQ(1, iterator.getSubmapIndex()(0));
   EXPECT_EQ(3, iterator.getSubmapIndex()(1));
 }
+
+/**
+ * The submap should contain the same elements as before even after moving the underlying map.
+ *
+ *                                                          +----------------------------+
+ *                                                          |                            |
+ *                                                          |                            |
+ *              +----------------------------+              |                            |
+ *              |0  0  0  0  0  0  0  0  0  0|              |      0  0  0  0  0  0  0  0|
+ *              |     +----+                 |              |           +----+           |
+ * Submap       |1  1 |1  1| 1  1  1  1  1  1|              |      1  1 |1  1| 1  1  1  1|
+ *           +------> |    |                 |              |           |    |           |
+ *              |2  2 |2  2| 2  2  2  2  2  2|              |      2  2 |2  2| 2  2  2  2|
+ *              |     +----+                 |              |           +----+           |
+ *              |3  3  3  3  3  3  3  3  3  3|   Move       |      3  3  3  3  3  3  3  3|
+ *              |                            |              |                            |
+ *              |4  4  4  4  4  4  4  4  4  4| +--------->  |      4  4  4  4  4  4  4  4|
+ *              |                            |              |                            |
+ *              |5  5  5  5  5  5  5  5  5  5|              |      5  5  5  5  5  5  5  5|
+ *              |                            |              |                            |
+ *              |6  6  6  6  6  6  6  6  6  6|              |      6  6  6  6  6  6  6  6|
+ *              |                            |              |                            |
+ *              |7  7  7  7  7  7  7  7  7  7|              |      7  7  7  7  7  7  7  7|
+ *              |                            |              +----------------------------+
+ *              |8  8  8  8  8  8  8  8  8  8|
+ *              |                            |
+ *              |9  9  9  9  9  9  9  9  9  9|
+ *              +----------------------------+
+ */
+TEST(SubmapIterator, InterleavedExecutionWithMove) {
+  grid_map::Index submapTopLeftIndex(3, 1);
+  grid_map::Size submapSize(2, 2);
+
+  GridMap map({"layer"});
+
+  map.setGeometry(Length(10, 10), 1.0, Position(0.0, 0.0));  // bufferSize(8, 5)
+
+  auto& layer = map.get("layer");
+
+  // Initialize the layer as sketched.
+  for (size_t colIndex = 0; colIndex < layer.cols(); colIndex++) {
+    layer.col(colIndex).setConstant(colIndex);
+  }
+
+  std::cout << "(4,7) contains " << map.at("layer", {4, 7}) << std::endl;
+  // Instantiate the submap iterator as sketched.
+  SubmapIterator iterator(map, submapTopLeftIndex, submapSize);
+
+  // check that the submap iterator returns {1,1,2,2}
+  auto checkCorrectValues = [](std::array<double, 4> given) {
+    int countOnes = 0, countTwos = 0;
+    for (auto& value : given) {
+      if (std::abs(value - 1.0) < 1e-6) {
+        countOnes++;
+      } else if (std::abs(value - 2.0) < 1e-6) {
+        countTwos++;
+      } else {
+        FAIL() << "Submap iterator returned unexpected value.";
+      }
+    }
+    EXPECT_EQ(countOnes, 2);
+    EXPECT_EQ(countTwos, 2);
+  };
+
+  std::array<double, 4> returnedSequence;
+  returnedSequence.fill(0);
+
+  for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
+    returnedSequence.at(submapIndex) = map.at("layer", *iterator);
+    ++iterator;
+  }
+
+  checkCorrectValues(returnedSequence);
+
+  // Reset the iterator and now check that it still returns the same sequence when we move the map interleaved with iterating.
+  iterator = SubmapIterator(map, submapTopLeftIndex, submapSize);
+  returnedSequence.fill(0);
+  for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
+    if (submapIndex == 2) {
+      // Now move the map as depicted.
+      map.move(Position(2.0, 2.0));
+    }
+    returnedSequence.at(submapIndex) = map.at("layer", *iterator);
+    ++iterator;
+  }
+  checkCorrectValues(returnedSequence);
+
+  // TODO (mwulf, mgaertner): This behavior is not yet implemented:
+  //
+  //  // Reset the iterator and now check that the iterator throws? if the submap moved out of range.
+  //  iterator = SubmapIterator(map, submapTopLeftIndex, submapSize);
+  //
+  //  EXPECT_ANY_THROW(for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
+  //    if (submapIndex == 2) {
+  //      // Now move the map so that the submap gets out of range.
+  //      map.move(Position(20.0, 20.0));
+  //    }
+  //    returnedSequence.at(submapIndex) = map.at("layer", *iterator);
+  //    ++iterator;
+  //  });
+}

grid_map_cv/CHANGELOG.rst

@@ -1,6 +1,9 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package grid_map_cv
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.6.3 (2020-09-30)
+------------------
+* Added getTransformedMap.
 
 1.6.2 (2019-10-14)
 ------------------

grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp

@@ -2,7 +2,7 @@
  * GridMapCvProcessing.hpp
  *
  *  Created on: Apr 15, 2016
- *      Author: Péter Fankhauser
+ *      Author: Péter Fankhauser, Magnus Gärtner
  *	 Institute: ETH Zurich, ANYbotics
  */
 
@@ -18,8 +18,7 @@ namespace grid_map {
 /*!
  * Processing of grid maps with OpenCV methods.
  */
-class GridMapCvProcessing
-{
+class GridMapCvProcessing {
  public:
   /*!
    * Default constructor.
@@ -39,11 +38,28 @@ class GridMapCvProcessing
    * @param[in](optional) interpolationAlgorithm the interpolation method.
    * @return true if successful, false otherwise.
    */
-  static bool changeResolution(const GridMap& gridMapSource,
-                               GridMap& gridMapResult,
-                               const double resolution,
+  static bool changeResolution(const GridMap& gridMapSource, GridMap& gridMapResult, const double resolution,
                                const int interpolationAlgorithm = cv::INTER_CUBIC);
 
+  /*!
+   * Apply isometric transformation (rotation + offset) to grid map and returns the transformed map.
+   * Note: The returned map may not have the same length since it's geometric description contains
+   * the original map.
+   * Also note that the map treats the height layer differently from other layers. I.e, the translation in z-direction is applied only to
+   * the height layer. For layers other than the height layer that contain geometric information this function might compute unexpected
+   * results. E.g transforming the layers normals_x/y/z will only represent the same values at the transformed location but will not
+   * transform the normal vectors into the new coordinate system.
+   *
+   * @param[consume] gridMapSource The map to transform.
+   * @param[in] transform the requested transformation to apply.
+   * @param[in] heightLayerName the height layer of the map.
+   * @param[in] newFrameId frame index of the new map.
+   * @return transformed map.
+   * @throw std::out_of_range if no map layer with name `heightLayerName` is present.
+   * @throw std::invalid_argument if the transform is not approximately z-aligned.
+   */
+  static GridMap getTransformedMap(GridMap&& gridMapSource, const Eigen::Isometry3d& transform, const std::string& heightLayerName,
+                                   const std::string& newFrameId);
 };
 
-} /* namespace */
+}  // namespace grid_map

grid_map_cv/package.xml

@@ -1,14 +1,15 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>grid_map_cv</name>
-  <version>1.6.2</version>
+  <version>1.6.3</version>
   <description>Conversions between grid maps and OpenCV images.</description>
   <maintainer email="mwulf@anybotics.com">Maximilian Wulf</maintainer>
   <maintainer email="ynava@anybotics.com">Yoshua Nava</maintainer>
   <license>BSD</license>
   <url type="website">http://github.com/anybotics/grid_map</url>
   <url type="bugtracker">http://github.com/anybotics/grid_map/issues</url>
   <author email="pfankhauser@anybotics.com">Péter Fankhauser</author>
+  <author email="mgaertner@anybotics.com">Magnus Gärtner</author>
   <buildtool_depend>catkin</buildtool_depend>
   <depend>grid_map_core</depend>
   <depend>cv_bridge</depend>