#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 indeces. 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 44 of file GridMap.hpp.
Definition at line 45 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 29 of file GridMap.cpp.
grid_map::GridMap::GridMap | ( | ) |
Emtpy constructor.
Definition at line 43 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 89 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 93 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 523 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 217 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 225 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 160 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 168 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 847 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 863 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 770 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 738 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 752 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 746 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 764 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 758 of file GridMap.cpp.
void grid_map::GridMap::convertToDefaultStartIndex | ( | ) |
Rearranges data such that the buffer start index is at (0,0).
Definition at line 672 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 135 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 107 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 553 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 111 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 119 of file GridMap.cpp.
const std::vector< std::string > & grid_map::GridMap::getBasicLayers | ( | ) | const |
Gets the names of the basic layers.
Definition at line 72 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 702 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 233 of file GridMap.cpp.
const std::vector< std::string > & grid_map::GridMap::getLayers | ( | ) | const |
Gets the names of the layers.
Definition at line 156 of file GridMap.cpp.
const Length & grid_map::GridMap::getLength | ( | ) | const |
Get the side length of the grid map.
Definition at line 644 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 237 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 648 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 269 of file GridMap.cpp.
double grid_map::GridMap::getResolution | ( | ) | const |
Get the resolution of the grid map.
Definition at line 652 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 656 of file GridMap.cpp.
const Index & grid_map::GridMap::getStartIndex | ( | ) | const |
Get the start index of the circular buffer.
Definition at line 664 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 291 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 296 of file GridMap.cpp.
Time grid_map::GridMap::getTimestamp | ( | ) | const |
Get the timestamp of the grid map.
Definition at line 628 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 341 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 281 of file GridMap.cpp.
bool grid_map::GridMap::hasBasicLayers | ( | ) | const |
True if basic layers are defined.
Definition at line 76 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 80 of file GridMap.cpp.
bool grid_map::GridMap::isDefaultStartIndex | ( | ) | const |
Checks if the buffer is at start index (0,0).
Definition at line 668 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 241 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 249 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 253 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 257 of file GridMap.cpp.
|
private |
Defines data validation check
value |
Definition at line 245 of file GridMap.cpp.
bool grid_map::GridMap::move | ( | const Position & | position, |
std::vector< BufferRegion > & | newRegions | ||
) |
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. 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 451 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 518 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 127 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 131 of file GridMap.cpp.
void grid_map::GridMap::resetTimestamp | ( | ) |
Resets the timestamp of the grid map (to zero).
Definition at line 632 of file GridMap.cpp.
|
private |
Resize the buffer.
bufferSize | the requested buffer size. |
Definition at line 839 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 68 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 636 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 45 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 64 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 447 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 660 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 624 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 569 of file GridMap.hpp.
|
private |
Grid map data stored as layers of matrices.
Definition at line 561 of file GridMap.hpp.
|
private |
Frame id of the grid map.
Definition at line 555 of file GridMap.hpp.
|
private |
Names of the data layers.
Definition at line 564 of file GridMap.hpp.
|
private |
Side length of the map in x- and y-direction [m].
Definition at line 572 of file GridMap.hpp.
|
private |
Map position in the grid map frame [m].
Definition at line 578 of file GridMap.hpp.
|
private |
Map resolution in xy plane [m/cell].
Definition at line 575 of file GridMap.hpp.
|
private |
Size of the buffer (rows and cols of the data structure).
Definition at line 581 of file GridMap.hpp.
|
private |
Circular buffer start indeces.
Definition at line 584 of file GridMap.hpp.
|
private |
Timestamp of the grid map (nanoseconds).
Definition at line 558 of file GridMap.hpp.