Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/grow - Merged master into #174 #330

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 28 additions & 0 deletions grid_map_core/include/grid_map_core/GridMap.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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_;

Expand Down
101 changes: 101 additions & 0 deletions grid_map_core/src/GridMap.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I had to replace this line by this one to avoid alignment issues:

Suggested change
length_ = length;
length_ = (size_.cast<double>() * resolution_).matrix();

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Taken from here:

length_ = (size_.cast<double>() * resolution_).matrix();


// 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;
}
Expand Down Expand Up @@ -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
Expand Down
40 changes: 40 additions & 0 deletions grid_map_core/test/GridMapTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down