#include <GridMap.hpp>
Public Types | |
typedef grid_map::DataType | DataType |
typedef grid_map::Matrix | Matrix |
Public Member Functions | |
void | add (const std::string &layer, const double value=NAN) |
void | add (const std::string &layer, const Matrix &data) |
bool | addDataFrom (const GridMap &other, bool extendMap, bool overwriteData, bool copyAllLayers, std::vector< std::string > layers=std::vector< std::string >()) |
float & | at (const std::string &layer, const Index &index) |
float | at (const std::string &layer, const Index &index) const |
float & | atPosition (const std::string &layer, const Position &position) |
float | atPosition (const std::string &layer, const Position &position, InterpolationMethods interpolationMethod=InterpolationMethods::INTER_NEAREST) const |
void | clear (const std::string &layer) |
void | clearAll () |
void | clearBasic () |
void | convertToDefaultStartIndex () |
bool | erase (const std::string &layer) |
bool | exists (const std::string &layer) const |
bool | extendToInclude (const GridMap &other) |
const Matrix & | get (const std::string &layer) const |
Matrix & | get (const std::string &layer) |
const std::vector< std::string > & | getBasicLayers () const |
Position | getClosestPositionInMap (const Position &position) const |
const std::string & | getFrameId () const |
bool | getIndex (const Position &position, Index &index) const |
const std::vector< std::string > & | getLayers () const |
const Length & | getLength () const |
bool | getPosition (const Index &index, Position &position) const |
const Position & | getPosition () const |
bool | getPosition3 (const std::string &layer, const Index &index, Position3 &position) const |
double | getResolution () const |
const Size & | getSize () const |
const Index & | getStartIndex () const |
GridMap | getSubmap (const Position &position, const Length &length, bool &isSuccess) const |
GridMap | getSubmap (const Position &position, const Length &length, Index &indexInSubmap, bool &isSuccess) const |
Time | getTimestamp () const |
GridMap | getTransformedMap (const Eigen::Isometry3d &transform, const std::string &heightLayerName, const std::string &newFrameId, const double sampleRatio=0.0) const |
bool | getVector (const std::string &layerPrefix, const Index &index, Eigen::Vector3d &vector) const |
GridMap (const std::vector< std::string > &layers) | |
GridMap () | |
GridMap (const GridMap &)=default | |
GridMap (GridMap &&)=default | |
bool | hasBasicLayers () const |
bool | hasSameLayers (const grid_map::GridMap &other) const |
bool | isDefaultStartIndex () const |
bool | isInside (const Position &position) const |
bool | isValid (const Index &index) const |
bool | isValid (const Index &index, const std::string &layer) const |
bool | isValid (const Index &index, const std::vector< std::string > &layers) const |
bool | move (const Position &position, std::vector< BufferRegion > &newRegions) |
bool | move (const Position &position) |
GridMap & | operator= (const GridMap &)=default |
GridMap & | operator= (GridMap &&)=default |
const Matrix & | operator[] (const std::string &layer) const |
Matrix & | operator[] (const std::string &layer) |
void | resetTimestamp () |
void | setBasicLayers (const std::vector< std::string > &basicLayers) |
void | setFrameId (const std::string &frameId) |
void | setGeometry (const Length &length, const double resolution, const Position &position=Position::Zero()) |
void | setGeometry (const SubmapGeometry &geometry) |
void | setPosition (const Position &position) |
void | setStartIndex (const Index &startIndex) |
void | setTimestamp (const Time timestamp) |
virtual | ~GridMap ()=default |
Private Member Functions | |
bool | atPositionBicubicConvolutionInterpolated (const std::string &layer, const Position &position, float &value) const |
bool | atPositionBicubicInterpolated (const std::string &layer, const Position &position, float &value) const |
bool | atPositionLinearInterpolated (const std::string &layer, const Position &position, float &value) const |
void | clearCols (unsigned int index, unsigned int nCols) |
void | clearRows (unsigned int index, unsigned int nRows) |
bool | isValid (DataType value) const |
void | resize (const Index &bufferSize) |
Private Attributes | |
std::vector< std::string > | basicLayers_ |
std::unordered_map< std::string, Matrix > | data_ |
Grid map data stored as layers of matrices. More... | |
std::string | frameId_ |
Frame id of the grid map. More... | |
std::vector< std::string > | layers_ |
Names of the data layers. More... | |
Length | length_ |
Side length of the map in x- and y-direction [m]. More... | |
Position | position_ |
Map position in the grid map frame [m]. More... | |
double | resolution_ |
Map resolution in xy plane [m/cell]. More... | |
Size | size_ |
Size of the buffer (rows and cols of the data structure). More... | |
Index | startIndex_ |
Circular buffer start indices. More... | |
Time | timestamp_ |
Timestamp of the grid map (nanoseconds). More... | |
Grid map managing multiple overlaying maps holding float values. Data structure implemented as two-dimensional circular buffer so map can be moved efficiently.
Data is defined with string keys. Examples are:
Definition at line 40 of file GridMap.hpp.
Definition at line 43 of file GridMap.hpp.
Definition at line 44 of file GridMap.hpp.
grid_map::GridMap::GridMap | ( | const std::vector< std::string > & | layers | ) |
Constructor.
layers | a vector of strings containing the definition/description of the data layer. |
Definition at line 28 of file GridMap.cpp.
grid_map::GridMap::GridMap | ( | ) |
Emtpy constructor.
Definition at line 42 of file GridMap.cpp.
|
default |
Default copy assign and copy constructors.
|
default |
|
virtualdefault |
Destructor.
void grid_map::GridMap::add | ( | const std::string & | layer, |
const double | value = NAN |
||
) |
Add a new empty data layer.
layer | the name of the layer. value the value to initialize the cells with. |
Definition at line 82 of file GridMap.cpp.
void grid_map::GridMap::add | ( | const std::string & | layer, |
const Matrix & | data | ||
) |
Add a new data layer (if the layer already exists, overwrite its data, otherwise add layer and data).
layer | the name of the layer. |
data | the data to be added. |
Definition at line 86 of file GridMap.cpp.
bool grid_map::GridMap::addDataFrom | ( | const GridMap & | other, |
bool | extendMap, | ||
bool | overwriteData, | ||
bool | copyAllLayers, | ||
std::vector< std::string > | layers = std::vector<std::string>() |
||
) |
Adds data from an other grid map to this grid map
other | the grid map to take data from. |
extendMap | if true the grid map is resized that the other map fits within. |
overwriteData | if true the new data replaces the old values, else only invalid cells are updated. |
copyAllLayer | if true all layers are used to add data. |
layers | the layers that are copied if not all layers are used. |
Definition at line 514 of file GridMap.cpp.
float & grid_map::GridMap::at | ( | const std::string & | layer, |
const Index & | index | ||
) |
Get cell data for requested index.
layer | the name of the layer to be accessed. |
index | the requested index. |
std::out_of_range | if no map layer with name layer is present. |
Definition at line 212 of file GridMap.cpp.
float grid_map::GridMap::at | ( | const std::string & | layer, |
const Index & | index | ||
) | const |
Get cell data for requested index. Const version form above.
layer | the name of the layer to be accessed. |
index | the requested index. |
std::out_of_range | if no map layer with name layer is present. |
Definition at line 220 of file GridMap.cpp.
float & grid_map::GridMap::atPosition | ( | const std::string & | layer, |
const Position & | position | ||
) |
Get cell data at requested position.
layer | the name of the layer to be accessed. |
position | the requested position. |
std::out_of_range | if no map layer with name layer is present. |
Definition at line 153 of file GridMap.cpp.
float grid_map::GridMap::atPosition | ( | const std::string & | layer, |
const Position & | position, | ||
InterpolationMethods | interpolationMethod = InterpolationMethods::INTER_NEAREST |
||
) | const |
Get cell data at requested position. Const version form above.
layer | the name of the layer to be accessed. |
position | the requested position. |
std::out_of_range | if no map layer with name layer is present. |
std::runtime_error | if the specified interpolation method is not implemented. |
Definition at line 161 of file GridMap.cpp.
|
private |
Get cell data at requested position, cubic convolution interpolated from 4x4 cells. At the edge of the map, the algorithm assumes that height continues with the slope 0. I.e. the border cells just repeat outside of the map Taken from: https://en.wikipedia.org/wiki/Bicubic_interpolation
[in] | layer | the name of the layer to be accessed. |
[in] | position | the requested position. |
[out] | value | the data of the cell. |
Definition at line 856 of file GridMap.cpp.
|
private |
Get cell data at requested position, cubic interpolated on a square. At the edge of the map, the algorithm assumes that height continues with the slope 0. I.e. the border cells just repeat outside of the map Taken from: https://en.wikipedia.org/wiki/Bicubic_interpolation
[in] | layer | the name of the layer to be accessed. |
[in] | position | the requested position. |
[out] | value | the data of the cell. |
Definition at line 872 of file GridMap.cpp.
|
private |
Get cell data at requested position, linearly interpolated from 2x2 cells.
layer | the name of the layer to be accessed. |
position | the requested position. |
value | the data of the cell. |
Definition at line 777 of file GridMap.cpp.
void grid_map::GridMap::clear | ( | const std::string & | layer | ) |
Clears all cells (set to NAN) for a layer.
layer | the layer to be cleared. |
Definition at line 745 of file GridMap.cpp.
void grid_map::GridMap::clearAll | ( | ) |
Clears all cells of all layers. If basic layers are used, clearBasic() is preferred as it is more efficient. Header information (geometry etc.) remains valid.
Definition at line 759 of file GridMap.cpp.
void grid_map::GridMap::clearBasic | ( | ) |
Clears all cells (set to NAN) for all basic layers. Header information (geometry etc.) remains valid.
Definition at line 753 of file GridMap.cpp.
|
private |
Clear a number of columns of the grid map.
index | the left index for the columns to be reset. |
nCols | the number of columns to reset. |
Definition at line 771 of file GridMap.cpp.
|
private |
Clear a number of rows of the grid map.
index | the upper index for the rows to be reset. |
nRows | the number of rows to reset. |
Definition at line 765 of file GridMap.cpp.
void grid_map::GridMap::convertToDefaultStartIndex | ( | ) |
Rearranges data such that the buffer start index is at (0,0).
Definition at line 677 of file GridMap.cpp.
bool grid_map::GridMap::erase | ( | const std::string & | layer | ) |
Removes a layer from the grid map.
layer | the name of the layer to be removed. |
Definition at line 128 of file GridMap.cpp.
bool grid_map::GridMap::exists | ( | const std::string & | layer | ) | const |
Checks if data layer exists.
layer | the name of the layer. |
Definition at line 100 of file GridMap.cpp.
bool grid_map::GridMap::extendToInclude | ( | const GridMap & | other | ) |
Extends the size of the grip map such that the other grid map fits within.
other | the grid map to extend the size to. |
Definition at line 554 of file GridMap.cpp.
const Matrix & grid_map::GridMap::get | ( | const std::string & | layer | ) | const |
Returns the grid map data for a layer as matrix.
layer | the name of the layer to be returned. |
std::out_of_range | if no map layer with name layer is present. |
Definition at line 104 of file GridMap.cpp.
Matrix & grid_map::GridMap::get | ( | const std::string & | layer | ) |
Returns the grid map data for a layer as non-const. Use this method with care!
layer | the name of the layer to be returned. |
std::out_of_range | if no map layer with name layer is present. |
Definition at line 112 of file GridMap.cpp.
const std::vector< std::string > & grid_map::GridMap::getBasicLayers | ( | ) | const |
Gets the names of the basic layers.
Definition at line 69 of file GridMap.cpp.
Calculates the closest point to positionOutMap that is in the grid map. If positionOutMap is already in the grid map, that position is returned.
[in] | position | position that should be approached as close as possible. |
Definition at line 709 of file GridMap.cpp.
const std::string & grid_map::GridMap::getFrameId | ( | ) | const |
Gets the corresponding cell index for a position.
[in] | position | the requested position. |
[out] | index | the corresponding index. |
Definition at line 228 of file GridMap.cpp.
const std::vector< std::string > & grid_map::GridMap::getLayers | ( | ) | const |
Gets the names of the layers.
Definition at line 149 of file GridMap.cpp.
const Length & grid_map::GridMap::getLength | ( | ) | const |
Get the side length of the grid map.
Definition at line 649 of file GridMap.cpp.
Gets the 2d position of cell specified by the index (x, y of cell position) in the grid map frame.
[in] | index | the index of the requested cell. |
[out] | position | the position of the data point in the parent frame. |
Definition at line 232 of file GridMap.cpp.
const Position & grid_map::GridMap::getPosition | ( | ) | const |
Get the 2d position of the grid map in the grid map frame.
Definition at line 653 of file GridMap.cpp.
bool grid_map::GridMap::getPosition3 | ( | const std::string & | layer, |
const Index & | index, | ||
Position3 & | position | ||
) | const |
Gets the 3d position of a data point (x, y of cell position & cell value as z) in the grid map frame. This is useful for data layers such as elevation.
layer | the name of the layer to be accessed. |
index | the index of the requested cell. |
position | the position of the data point in the parent frame. |
Definition at line 260 of file GridMap.cpp.
double grid_map::GridMap::getResolution | ( | ) | const |
Get the resolution of the grid map.
Definition at line 657 of file GridMap.cpp.
const Size & grid_map::GridMap::getSize | ( | ) | const |
Get the grid map size (rows and cols of the data structure).
Definition at line 661 of file GridMap.cpp.
const Index & grid_map::GridMap::getStartIndex | ( | ) | const |
Get the start index of the circular buffer.
Definition at line 669 of file GridMap.cpp.
GridMap grid_map::GridMap::getSubmap | ( | const Position & | position, |
const Length & | length, | ||
bool & | isSuccess | ||
) | const |
Gets a submap from the map. The requested submap is specified with the requested location and length. Note: The returned submap may not have the requested length due to the borders of the map and discretization.
[in] | position | the requested position of the submap (usually the center). |
[in] | length | the requested length of the submap. |
[out] | isSuccess | true if successful, false otherwise. |
Definition at line 282 of file GridMap.cpp.
GridMap grid_map::GridMap::getSubmap | ( | const Position & | position, |
const Length & | length, | ||
Index & | indexInSubmap, | ||
bool & | isSuccess | ||
) | const |
Gets a submap from the map. The requested submap is specified with the requested location and length. Note: The returned submap may not have the requested length due to the borders of the map and discretization.
[in] | position | the requested position of the submap (usually the center). |
[in] | length | the requested length of the submap. |
[out] | indexInSubmap | the index of the requested position in the submap. |
[out] | isSuccess | true if successful, false otherwise. |
Definition at line 287 of file GridMap.cpp.
Time grid_map::GridMap::getTimestamp | ( | ) | const |
Get the timestamp of the grid map.
Definition at line 633 of file GridMap.cpp.
GridMap grid_map::GridMap::getTransformedMap | ( | const Eigen::Isometry3d & | transform, |
const std::string & | heightLayerName, | ||
const std::string & | newFrameId, | ||
const double | sampleRatio = 0.0 |
||
) | const |
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.
[in] | transform | the requested transformation to apply. |
[in] | heightLayerName | the height layer of the map. |
[in] | newFrameId | frame index of the new map. |
[in] | sampleRatio | if zero or negative, no in-painting is used to fill missing points due to sparsity of the map. Otherwise, four points are sampled around each grid cell to make sure that at least one of those points map to a new grid cell. A sampleRatio of 1 corresponds to the the resolution of the grid map. |
std::out_of_range | if no map layer with name heightLayerName is present. |
Definition at line 332 of file GridMap.cpp.
bool grid_map::GridMap::getVector | ( | const std::string & | layerPrefix, |
const Index & | index, | ||
Eigen::Vector3d & | vector | ||
) | const |
Gets the 3d vector of three layers with suffixes 'x', 'y', and 'z'.
layerPrefix | the prefix for the layer to bet get as vector. |
index | the index of the requested cell. |
vector | the vector with the values of the data type. |
Definition at line 272 of file GridMap.cpp.
bool grid_map::GridMap::hasBasicLayers | ( | ) | const |
True if basic layers are defined.
Definition at line 73 of file GridMap.cpp.
bool grid_map::GridMap::hasSameLayers | ( | const grid_map::GridMap & | other | ) | const |
Checks if another grid map contains the same layers as this grid map. The other grid map could contain more layers than the checked ones. Does not check the selection of basic layers.
other | the other grid map. |
Definition at line 77 of file GridMap.cpp.
bool grid_map::GridMap::isDefaultStartIndex | ( | ) | const |
Checks if the buffer is at start index (0,0).
Definition at line 673 of file GridMap.cpp.
bool grid_map::GridMap::isInside | ( | const Position & | position | ) | const |
Check if position is within the map boundaries.
position | the position to be checked. |
Definition at line 236 of file GridMap.cpp.
bool grid_map::GridMap::isValid | ( | const Index & | index | ) | const |
Checks if the index of all layers defined as basic types are valid, i.e. if all basic types are finite. Returns false
if no basic types are defined.
index | the index to check. |
Definition at line 244 of file GridMap.cpp.
bool grid_map::GridMap::isValid | ( | const Index & | index, |
const std::string & | layer | ||
) | const |
Checks if cell at index is a valid (finite) for a certain layer.
index | the index to check. |
layer | the name of the layer to be checked for validity. |
Definition at line 248 of file GridMap.cpp.
bool grid_map::GridMap::isValid | ( | const Index & | index, |
const std::vector< std::string > & | layers | ||
) | const |
Checks if cell at index is a valid (finite) for certain layers.
index | the index to check. |
layers | the layers to be checked for validity. |
Definition at line 252 of file GridMap.cpp.
|
private |
Defines data validation check
value |
Definition at line 240 of file GridMap.cpp.
bool grid_map::GridMap::move | ( | const Position & | position, |
std::vector< BufferRegion > & | newRegions | ||
) |
Relocates the region captured by grid map w.r.t. to the static grid map frame. Use this to move the grid map boundaries without relocating the grid map data. Takes care of all the data handling, such that the grid map data is stationary in the grid map frame.
Data that falls outside the map at its new position is discarded.
Note: Due to the circular buffer structure, neighbouring indices might not fall close in the map frame. This assumption only holds for indices obtained by getUnwrappedIndex().
Note: For a comparison between the setPosition
and the move
method, see the move_demo_node.cpp
file of the grid_map_demos
package.
position | the new location of the grid map in the map frame. |
newRegions | the regions of the newly covered / previously uncovered regions of the buffer. |
Definition at line 442 of file GridMap.cpp.
bool grid_map::GridMap::move | ( | const Position & | position | ) |
Move the grid map w.r.t. to the grid map frame. Use this to move the grid map boundaries without moving the grid map data. Takes care of all the data handling, such that the grid map data is stationary in the grid map frame.
position | the new location of the grid map in the map frame. |
Definition at line 509 of file GridMap.cpp.
const Matrix & grid_map::GridMap::operator[] | ( | const std::string & | layer | ) | const |
Returns the grid map data for a layer as matrix.
layer | the name of the layer to be returned. |
std::out_of_range | if no map layer with name layer is present. |
Definition at line 120 of file GridMap.cpp.
Matrix & grid_map::GridMap::operator[] | ( | const std::string & | layer | ) |
Returns the grid map data for a layer as non-const. Use this method with care!
layer | the name of the layer to be returned. |
std::out_of_range | if no map layer with name layer is present. |
Definition at line 124 of file GridMap.cpp.
void grid_map::GridMap::resetTimestamp | ( | ) |
Resets the timestamp of the grid map (to zero).
Definition at line 637 of file GridMap.cpp.
|
private |
Resize the buffer.
bufferSize | the requested buffer size. |
Definition at line 848 of file GridMap.cpp.
void grid_map::GridMap::setBasicLayers | ( | const std::vector< std::string > & | basicLayers | ) |
Set the basic layers that need to be valid for a cell to be considered as valid. Also, the basic layers are set to NAN when clearing the cells with clearBasic()
. By default the list of basic layers is empty.
basicLayers | the list of layers that are the basic layers of the map. |
Definition at line 65 of file GridMap.cpp.
void grid_map::GridMap::setFrameId | ( | const std::string & | frameId | ) |
Set the frame id of the grid map.
frameId | the frame id to set. |
Definition at line 641 of file GridMap.cpp.
void grid_map::GridMap::setGeometry | ( | const Length & | length, |
const double | resolution, | ||
const Position & | position = Position::Zero() |
||
) |
Set the geometry of the grid map. Clears all the data.
length | the side lengths in x, and y-direction of the grid map [m]. |
resolution | the cell size in [m/cell]. |
position | the 2d position of the grid map in the grid map frame [m]. |
Definition at line 44 of file GridMap.cpp.
void grid_map::GridMap::setGeometry | ( | const SubmapGeometry & | geometry | ) |
Set the geometry of the grid map from submap geometry information.
geometry | the submap geometry information. |
Definition at line 61 of file GridMap.cpp.
void grid_map::GridMap::setPosition | ( | const Position & | position | ) |
Set the position of the grid map. Note: This method does not change the data stored in the grid map and is complementary to the move(...)
method. For a comparison between the setPosition
and the move
method, see the move_demo_node.cpp
file of the grid_map_demos
package.
position | the 2d position of the grid map in the grid map frame [m]. |
Definition at line 438 of file GridMap.cpp.
void grid_map::GridMap::setStartIndex | ( | const Index & | startIndex | ) |
Set the start index of the circular buffer. Use this method with caution!
Definition at line 665 of file GridMap.cpp.
void grid_map::GridMap::setTimestamp | ( | const Time | timestamp | ) |
Set the timestamp of the grid map.
timestamp | the timestamp to set (in nanoseconds). |
Definition at line 629 of file GridMap.cpp.
|
private |
List of layers from data_
that are the basic grid map layers. This means that for a cell to be valid, all basic layers need to be valid. Also, the basic layers are set to NAN when clearing the map with clear()
.
Definition at line 567 of file GridMap.hpp.
|
private |
Grid map data stored as layers of matrices.
Definition at line 559 of file GridMap.hpp.
|
private |
Frame id of the grid map.
Definition at line 553 of file GridMap.hpp.
|
private |
Names of the data layers.
Definition at line 562 of file GridMap.hpp.
|
private |
Side length of the map in x- and y-direction [m].
Definition at line 570 of file GridMap.hpp.
|
private |
Map position in the grid map frame [m].
Definition at line 576 of file GridMap.hpp.
|
private |
Map resolution in xy plane [m/cell].
Definition at line 573 of file GridMap.hpp.
|
private |
Size of the buffer (rows and cols of the data structure).
Definition at line 579 of file GridMap.hpp.
|
private |
Circular buffer start indices.
Definition at line 582 of file GridMap.hpp.
|
private |
Timestamp of the grid map (nanoseconds).
Definition at line 556 of file GridMap.hpp.