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.

Sign up for GitHub

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

Jump to bottom

Feature - Grow! #174

Closed
wants to merge 2 commits into from
Closed

Feature - Grow! #174

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
077c143
Change grid size without deleting data
Jun 5, 2018
0f45e12
Add ability to grow in different directions
Jun 28, 2018
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
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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_;

Expand Down
102 changes: 102 additions & 0 deletions grid_map_core/src/GridMap.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,9 @@ void GridMap::setGeometry(const Length& length, const double resolution,

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

position_ = position;

startIndex_.setZero();

return;
Expand All @@ -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<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;
Expand Down Expand Up @@ -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);
Copy link

Choose a reason for hiding this comment

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

Plain curiosity,
why are you using conservativeResizeLike rather than conservativeResize ??
The function conservativeResize allocates a temporary matrix under the hood, so I would imagine that conservativeResizeLike does the same. Thus isn't the temporary matrix like a waste of memory ?

Copy link
Author

Choose a reason for hiding this comment

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

conservativeResize doesn't let you initialize the values of new elements, conservativeResizeLike does. This is important when you want to be able to guarantee that every new grid cell is initalized to 0.0, for instance.

In principle I could use conservativeResize, loop through all new cells and manually initialize them, but I suspect that would be less efficient.

Copy link

@artivis artivis Aug 1, 2018 •
edited
Loading

Choose a reason for hiding this comment

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

Ok I see your point.
Still I think a plain conservativeResize followed by manually setting the uninitialized values would be better as you don't really need allocating matrix like, especially as the matrix grows large.
Rather than looping over uninitialized values you can use the block functions, something along the lines :

data.second.conservativeResize(size(0), size(1));
// Assuming that resizing to larger dimension appends 
// rows at the bottom and cols at the right side.
// Set value for appended bottom rows
data.second.block(size_(0), 0, row_delta, size(1)).setConstant(value); 
// Set value for appended top-left block (remaining unintialized)
data.second.topRightCorner(size(0), col_delta).setConstant(value);

I haven't checked that the indices are actually correct ^^, but that's the general idea.

Edit: you could also use the block scheme to swap the whole original matrix over the new layout.

Copy link

Choose a reason for hiding this comment

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

Is this feature working? What is it stopping from going to production?

Copy link
Author

Choose a reason for hiding this comment

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

Just waiting for an approving review. We have been using this feature extensively in production at our company.

Copy link

Choose a reason for hiding this comment

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

this is very nice feature and exactly what I needed! A merge would be good.

Copy link

Choose a reason for hiding this comment

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

+1 for useful feature and would be good to have merged

Copy link
Collaborator

Choose a reason for hiding this comment

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

Yes, will import it.


// 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 */

40 changes: 40 additions & 0 deletions grid_map_core/test/GridMapTest.cpp
View file
Open in desktop
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(AddDataFrom, ExtendMapAligned)
{
GridMap map1, map2;
Expand Down