Feature/grow - Merged master into #174

grid_map_core/include/grid_map_core/GridMap.hpp

@@ -83,6 +83,26 @@ class GridMap
    */
   void setGeometry(const SubmapGeometry& geometry);
 
+  /*!
+   * Specification for which direction(s) to grow the gridmap in.
+   */
+  enum Direction
+  {
+    CENTERED, // Grow the gridmap in all directions evenly from the center.
+    NE,       // Grow in the +X and +Y (Quadrant 1).
+    NW,       // Grow in the -X and +Y (Quadrant 2).
+    SW,       // Grow in the -X and -Y (Quadrant 3).
+    SE        // Grow in the +X and -Y (Quadrant 4).
+  };
+
+  /*!
+   * Increase the size of the grid map while retaining old data.
+   * @param length the new side lengths in x, and y-direction of the grid map [m].
+   * @param direction the direction to grow in (default = SE).
+   * @param value the value new elements should be initialized with.
+   */
+  void grow(const Length& length, const Direction direction=SE, float value=0.0);
+
   /*!
    * Add a new empty data layer.
    * @param layer the name of the layer.
@@ -551,6 +571,14 @@ class GridMap
    */
   void resize(const Index& bufferSize);
 
+  /*!
+   * Resize the buffer without deleting data.
+   * @param size the requested buffer size.
+   * @param direction the direction to grow in (default = SE).
+   * @param value the value new elements should be initialized with.
+   */
+  void conservativeResize(const Index& size, const Direction direction=SE, float value=0.0);
+
   //! Frame id of the grid map.
   std::string frameId_;
 

grid_map_core/src/GridMap.cpp

@@ -55,7 +55,9 @@ void GridMap::setGeometry(const Length& length, const double resolution, const P
 
   resolution_ = resolution;
   length_ = (size_.cast<double>() * resolution_).matrix();
+
   position_ = position;
+
   startIndex_.setZero();
 
   return;
@@ -65,6 +67,49 @@ void GridMap::setGeometry(const SubmapGeometry& geometry) {
   setGeometry(geometry.getLength(), geometry.getResolution(), geometry.getPosition());
 }
 
+void GridMap::grow(const Length& length, const Direction direction, float value)
+{
+  if (length(0) < length_(0) || length(1) < length_(1))
+  {
+    return;
+  }
+
+  Length delta(length(0) - length_(0), length(1) - length_(1));
+
+  Size size;
+  size(0) = static_cast<int>(round(length(0) / resolution_));
+  size(1) = static_cast<int>(round(length(1) / resolution_));
+  conservativeResize(size, direction, value);
+  length_ = length;
+
+  // Update position
+  switch (direction)
+  {
+    case CENTERED:
+      break;
+
+    case NE:
+      position_(0) += delta(0)/2;
+      position_(1) -= delta(1)/2;
+      break;
+
+    case NW:
+      position_(0) += delta(0)/2;
+      position_(1) += delta(1)/2;
+      break;
+
+    case SW:
+      position_(0) -= delta(0)/2;
+      position_(1) += delta(1)/2;
+      break;
+
+    default:
+      position_(0) -= delta(0)/2;
+      position_(1) -= delta(1)/2;
+      break;
+  }
+}
+
 void GridMap::setBasicLayers(const std::vector<std::string>& basicLayers) {
   basicLayers_ = basicLayers;
 }
@@ -843,6 +888,62 @@ void GridMap::resize(const Index& size) {
   }
 }
 
+void GridMap::conservativeResize(const Index& size, const Direction direction, float value)
+{
+  int row_delta = size(0) - size_(0);
+  int col_delta = size(1) - size_(1);
+
+  Eigen::MatrixXf like = Eigen::MatrixXf::Constant(size(0), size(1), value);
+  for (auto& data : data_)
+  {
+    data.second.conservativeResizeLike(like);
+
+    // Swap rows and columns as needed
+    switch (direction)
+    {
+      case CENTERED:
+        for (int i = size_(0)-1; i >= 0; i--)
+        {
+          data.second.row(i).swap(data.second.row(i+row_delta/2));
+        }
+        for (int i = size_(1)-1; i >= 0; i--)
+        {
+          data.second.col(i).swap(data.second.col(i+col_delta/2));
+        }
+        break;
+
+      case NE:
+        for (int i = size_(0)-1; i >= 0; i--)
+        {
+          data.second.row(i).swap(data.second.row(i+row_delta));
+        }
+        break;
+
+      case NW:
+        for (int i = size_(0)-1; i >= 0; i--)
+        {
+          data.second.row(i).swap(data.second.row(i+row_delta));
+        }
+        for (int i = size_(1)-1; i >= 0; i--)
+        {
+          data.second.col(i).swap(data.second.col(i+col_delta));
+        }
+        break;
+
+      case SW:
+        for (int i = size_(1)-1; i >= 0; i--)
+        {
+          data.second.col(i).swap(data.second.col(i+col_delta));
+        }
+        break;
+
+      default:
+        break;
+    }
+  }
+
+  size_ = size;
+} /* namespace */
 
 bool GridMap::atPositionBicubicConvolutionInterpolated(const std::string& layer, const Position& position,
                                             float& value) const

grid_map_core/test/GridMapTest.cpp

@@ -84,6 +84,46 @@ TEST(GridMap, Move)
   EXPECT_EQ(2, regions[1].getSize()[1]);
 }
 
+TEST(GridMap, Grow)
+{
+  GridMap map;
+  map.setGeometry(Length(5.0, 5.0), 0.5, Position(0.0, 0.0));
+  map.add("layer", 0.0);
+
+  int original_rows = map.getSize()(0);
+  int original_cols = map.getSize()(1);
+
+  // Fill in map with data
+  for(int i = 0; i < original_rows; i++)
+  {
+    for(int j = 0; j < original_cols; j++)
+    {
+      map["layer"](i, j) = i*10 + j;
+    }
+  }
+
+  map.grow(Length(10.0, 10.0));
+
+  int rows = map.getSize()(0);
+  int cols = map.getSize()(1);
+
+  // Check original data
+  for(int i = 0; i < rows; i++)
+  {
+    for(int j = 0; j < cols; j++)
+    {
+      if (i < original_rows && j < original_cols)
+      {
+        EXPECT_DOUBLE_EQ(map["layer"](i, j), i*10 + j);
+      }
+      else
+      {
+        EXPECT_DOUBLE_EQ(map["layer"](i, j), 0.0);
+      }
+    }
+  }
+}
+
 TEST(GridMap, Transform)
 {
   // Initial map.