diff --git a/grid_map_core/include/grid_map_core/GridMap.hpp b/grid_map_core/include/grid_map_core/GridMap.hpp index 850109515..263c2d26f 100644 --- a/grid_map_core/include/grid_map_core/GridMap.hpp +++ b/grid_map_core/include/grid_map_core/GridMap.hpp @@ -82,6 +82,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. @@ -489,6 +509,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_; diff --git a/grid_map_core/src/GridMap.cpp b/grid_map_core/src/GridMap.cpp index a5f4e3594..c27979af8 100644 --- a/grid_map_core/src/GridMap.cpp +++ b/grid_map_core/src/GridMap.cpp @@ -63,7 +63,9 @@ void GridMap::setGeometry(const Length& length, const double resolution, resolution_ = resolution; length_ = (size_.cast() * resolution_).matrix(); + position_ = position; + startIndex_.setZero(); return; @@ -74,6 +76,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(round(length(0) / resolution_)); + size(1) = static_cast(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& basicLayers) { basicLayers_ = basicLayers; @@ -717,5 +762,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 */ diff --git a/grid_map_core/test/GridMapTest.cpp b/grid_map_core/test/GridMapTest.cpp index f6c0449bd..0deec6e00 100644 --- a/grid_map_core/test/GridMapTest.cpp +++ b/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(AddDataFrom, ExtendMapAligned) { GridMap map1, map2;