38 data_.insert(std::pair<std::string, Matrix>(layer,
Matrix()));
45 assert(length(0) > 0.0);
46 assert(length(1) > 0.0);
47 assert(resolution > 0.0);
50 size(0) =
static_cast<int>(round(length(0) / resolution));
51 size(1) =
static_cast<int>(round(length(1) / resolution));
79 [&](
const std::string& layer){
return other.
exists(layer);});
87 assert(
size_(0) == data.rows());
88 assert(
size_(1) == data.cols());
92 data_.at(layer) = data;
95 data_.insert(std::pair<std::string, Matrix>(layer, data));
106 return data_.at(layer);
107 }
catch (
const std::out_of_range& exception) {
108 throw std::out_of_range(
"GridMap::get(...) : No map layer '" + layer +
"' available.");
114 return data_.at(layer);
115 }
catch (
const std::out_of_range& exception) {
116 throw std::out_of_range(
"GridMap::get(...) : No map layer of type '" + layer +
"' available.");
129 const auto dataIterator =
data_.find(layer);
130 if (dataIterator ==
data_.end()) {
133 data_.erase(dataIterator);
135 const auto layerIterator = std::find(
layers_.begin(),
layers_.end(), layer);
136 if (layerIterator ==
layers_.end()) {
156 return at(layer, index);
158 throw std::out_of_range(
"GridMap::atPosition(...) : Position is out of range.");
163 bool skipNextSwitchCase =
false;
164 switch (interpolationMethod) {
171 skipNextSwitchCase =
true;
176 if (!skipNextSwitchCase) {
199 return at(layer, index);
201 throw std::out_of_range(
"GridMap::atPosition(...) : Position is out of range.");
206 throw std::runtime_error(
207 "GridMap::atPosition(...) : Specified " 208 "interpolation method not implemented.");
214 return data_.at(layer)(index(0), index(1));
215 }
catch (
const std::out_of_range& exception) {
216 throw std::out_of_range(
"GridMap::at(...) : No map layer '" + layer +
"' available.");
222 return data_.at(layer)(index(0), index(1));
223 }
catch (
const std::out_of_range& exception) {
224 throw std::out_of_range(
"GridMap::at(...) : No map layer '" + layer +
"' available.");
241 return isfinite(value);
253 if (layers.empty()) {
256 return std::all_of(layers.begin(), layers.end(),
257 [&](
const std::string& layer){
return isValid(index, layer);});
261 const auto value =
at(layer, index);
267 position.head(2) = position2d;
268 position.z() = value;
273 Eigen::Vector3d temp{
at(layerPrefix +
"x", index),
at(layerPrefix +
"y", index),
at(layerPrefix +
"z", index)};
284 return getSubmap(position, length, index, isSuccess);
295 SubmapGeometry submapInformation(*
this, position, length, isSuccess);
303 std::vector<BufferRegion> bufferRegions;
306 cout <<
"Cannot access submap of this size." << endl;
311 for (
const auto& data :
data_) {
312 for (
const auto& bufferRegion : bufferRegions) {
313 Index index = bufferRegion.getStartIndex();
314 Size size = bufferRegion.getSize();
317 submap.
data_[data.first].topLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
319 submap.
data_[data.first].topRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
321 submap.
data_[data.first].bottomLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
323 submap.
data_[data.first].bottomRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
333 const double sampleRatio)
const {
335 if (!
exists(heightLayerName)) {
336 throw std::out_of_range(
"GridMap::getTransformedMap(...) : No map layer '" + heightLayerName +
"' available.");
340 std::vector<Position3> positionSamples;
344 const double sampleLength =
resolution_ * sampleRatio;
347 const double halfLengthX =
length_.x() * 0.5;
348 const double halfLengthY =
length_.y() * 0.5;
354 std::vector<Position3> newEdges;
356 newEdges.push_back(transform * topLeftCorner);
357 newEdges.push_back(transform * topRightCorner);
358 newEdges.push_back(transform * bottomLeftCorner);
359 newEdges.push_back(transform * bottomRightCorner);
363 for (
const auto& newEdge : newEdges) {
364 newCenter += newEdge;
370 for (
const auto& newEdge : newEdges) {
371 Position3 positionCenterToEdge = newEdge - newCenter;
372 maxLengthFromCenter.x() = std::fmax(std::fabs(positionCenterToEdge.x()), maxLengthFromCenter.x());
373 maxLengthFromCenter.y() = std::fmax(std::fabs(positionCenterToEdge.y()), maxLengthFromCenter.y());
375 Length newLength = 2.0 * maxLengthFromCenter;
387 if (!
getPosition3(heightLayerName, *iterator, center)) {
392 positionSamples.clear();
394 if (sampleRatio > 0.0) {
395 positionSamples.reserve(5);
396 positionSamples.push_back(center);
397 positionSamples.emplace_back(center.x() - sampleLength, center.y(), center.z());
398 positionSamples.emplace_back(center.x() + sampleLength, center.y(), center.z());
399 positionSamples.emplace_back(center.x(), center.y() - sampleLength, center.z());
400 positionSamples.emplace_back(center.x(), center.y() + sampleLength, center.z());
402 positionSamples.push_back(center);
406 for (
const auto& position : positionSamples) {
407 const Position3 transformedPosition = transform * position;
410 if (!newMap.
getIndex(
Position(transformedPosition.x(), transformedPosition.y()), newIndex)) {
416 const auto newExistingValue = newMap.
at(heightLayerName, newIndex);
417 if (!std::isnan(newExistingValue) && newExistingValue > transformedPosition.z()) {
422 for (
const auto& layer :
layers_) {
423 const auto currentValueInOldGrid =
at(layer, *iterator);
424 auto& newValue = newMap.
at(layer, newIndex);
425 if (layer == heightLayerName) {
426 newValue = transformedPosition.z();
429 newValue = currentValueInOldGrid;
450 for (
int i = 0; i < indexShift.size(); i++) {
451 if (indexShift(i) != 0) {
452 if (abs(indexShift(i)) >=
getSize()(i)) {
458 int sign = (indexShift(i) > 0 ? 1 : -1);
459 int startIndex =
startIndex_(i) - (sign < 0 ? 1 : 0);
460 int endIndex = startIndex - sign + indexShift(i);
461 int nCells = abs(indexShift(i));
462 int index = (sign > 0 ? startIndex : endIndex);
465 if (index + nCells <=
getSize()(i)) {
476 int firstIndex = index;
477 int firstNCells =
getSize()(i) - firstIndex;
487 int secondNCells = nCells - firstNCells;
503 position_ += alignedPositionShift;
506 return indexShift.any();
510 std::vector<BufferRegion> newRegions;
511 return move(position, newRegions);
526 for (
const auto& layer : layers) {
533 if (
isValid(*iterator) && !overwriteData) {
543 for (
const auto& layer : layers) {
544 if (!other.
isValid(index, layer)) {
547 at(layer, *iterator) = other.
at(layer, index);
562 bool resizeMap =
false;
565 if (topLeftCornerOther.x() > topLeftCorner.x()) {
566 extendedMapPosition.x() += (topLeftCornerOther.x() - topLeftCorner.x()) / 2.0;
567 extendedMapLength.x() += topLeftCornerOther.x() - topLeftCorner.x();
570 if (topLeftCornerOther.y() > topLeftCorner.y()) {
571 extendedMapPosition.y() += (topLeftCornerOther.y() - topLeftCorner.y()) / 2.0;
572 extendedMapLength.y() += topLeftCornerOther.y() - topLeftCorner.y();
575 if (bottomRightCornerOther.x() < bottomRightCorner.x()) {
576 extendedMapPosition.x() -= (bottomRightCorner.x() - bottomRightCornerOther.x()) / 2.0;
577 extendedMapLength.x() += bottomRightCorner.x() - bottomRightCornerOther.x();
580 if (bottomRightCornerOther.y() < bottomRightCorner.y()) {
581 extendedMapPosition.y() -= (bottomRightCorner.y() - bottomRightCornerOther.y()) / 2.0;
582 extendedMapLength.y() += bottomRightCorner.y() - bottomRightCornerOther.y();
621 for (
const auto& layer :
layers_) {
622 at(layer, *iterator) = mapCopy.
at(layer, index);
682 std::vector<BufferRegion> bufferRegions;
684 throw std::out_of_range(
"Cannot access submap of this size.");
687 for (
auto& data :
data_) {
688 auto tempData(data.second);
689 for (
const auto& bufferRegion : bufferRegions) {
690 Index index = bufferRegion.getStartIndex();
691 Size size = bufferRegion.getSize();
694 tempData.topLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
696 tempData.topRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
698 tempData.bottomLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
700 tempData.bottomRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
703 data.second = tempData;
721 const double halfLengthX =
length_.x() * 0.5;
722 const double halfLengthY =
length_.y() * 0.5;
729 const double maxX = topRightCorner.x();
730 const double minX = bottomRightCorner.x();
731 const double maxY = bottomLeftCorner.y();
732 const double minY = bottomRightCorner.y();
736 positionInMap.x() = std::fmin(positionInMap.x(), maxX - std::numeric_limits<double>::epsilon());
737 positionInMap.y() = std::fmin(positionInMap.y(), maxY - std::numeric_limits<double>::epsilon());
739 positionInMap.x() = std::fmax(positionInMap.x(), minX + std::numeric_limits<double>::epsilon());
740 positionInMap.y() = std::fmax(positionInMap.y(), minY + std::numeric_limits<double>::epsilon());
742 return positionInMap;
747 data_.at(layer).setConstant(NAN);
748 }
catch (
const std::out_of_range& exception) {
749 throw std::out_of_range(
"GridMap::clear(...) : No map layer '" + layer +
"' available.");
760 for (
auto& data :
data_) {
761 data.second.setConstant(NAN);
767 data_.at(layer).block(index, 0, nRows,
getSize()(1)).setConstant(NAN);
773 data_.at(layer).block(0, index,
getSize()(0), nCols).setConstant(NAN);
786 if (position.x() >= point.x()) {
787 indices[1] = indices[0] +
Index(-1, 0);
790 indices[1] = indices[0] +
Index(+1, 0);
793 if (position.y() >= point.y()) {
794 indices[2] = indices[0] +
Index(0, -1);
808 indices[2] = indices[0] +
Index(0, +1);
821 indices[3].x() = indices[1].x();
822 indices[3].y() = indices[2].y();
825 const size_t bufferSize = mapSize(0) * mapSize(1);
827 const size_t endIndexLin = startIndexLin + bufferSize;
831 for (
size_t i = 0; i < 4; ++i) {
833 if ((indexLin < startIndexLin) || (indexLin > endIndexLin)) {
836 f[i] = layerMat(indexLin);
843 value = f[0] * positionRedFlip.x() * positionRedFlip.y() + f[1] * positionRed.x() * positionRedFlip.y() +
844 f[2] * positionRedFlip.x() * positionRed.y() + f[3] * positionRed.x() * positionRed.y();
850 for (
auto& data :
data_) {
859 double interpolatedValue = 0.0;
864 if (!std::isfinite(interpolatedValue)) {
867 value = interpolatedValue;
875 double interpolatedValue = 0.0;
880 if (!std::isfinite(interpolatedValue)) {
883 value = interpolatedValue;
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
GridMap getTransformedMap(const Eigen::Isometry3d &transform, const std::string &heightLayerName, const std::string &newFrameId, const double sampleRatio=0.0) const
Size size_
Size of the buffer (rows and cols of the data structure).
bool hasSameLayers(const grid_map::GridMap &other) const
bool isDefaultStartIndex() const
void setPosition(const Position &position)
float & atPosition(const std::string &layer, const Position &position)
void clear(const std::string &layer)
void wrapIndexToRange(Index &index, const Size &bufferSize)
bool getBufferRegionsForSubmap(std::vector< BufferRegion > &submapBufferRegions, const Index &submapIndex, const Size &submapBufferSize, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
void setBasicLayers(const std::vector< std::string > &basicLayers)
const Index & getStartIndex() const
double resolution_
Map resolution in xy plane [m/cell].
const Matrix & get(const std::string &layer) const
std::vector< std::string > basicLayers_
const std::vector< std::string > & getBasicLayers() const
std::unordered_map< std::string, Matrix > data_
Grid map data stored as layers of matrices.
const Size & getSize() const
grid_map::DataType DataType
std::string frameId_
Frame id of the grid map.
const Position & getPosition() const
bool getVector(const std::string &layerPrefix, const Index &index, Eigen::Vector3d &vector) const
const std::vector< std::string > & getLayers() 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
size_t getLinearIndexFromIndex(const Index &index, const Size &bufferSize, bool rowMajor=false)
bool atPositionBicubicConvolutionInterpolated(const std::string &layer, const Position &position, float &value) const
bool move(const Position &position, std::vector< BufferRegion > &newRegions)
void convertToDefaultStartIndex()
double getResolution() const
bool isInside(const Position &position) const
bool hasBasicLayers() const
bool getIndexFromPosition(Index &index, const Position &position, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
bool addDataFrom(const GridMap &other, bool extendMap, bool overwriteData, bool copyAllLayers, std::vector< std::string > layers=std::vector< std::string >())
bool extendToInclude(const GridMap &other)
bool checkIfPositionWithinMap(const Position &position, const Length &mapLength, const Position &mapPosition)
bool getPositionShiftFromIndexShift(Vector &positionShift, const Index &indexShift, const double &resolution)
std::vector< std::string > layers_
Names of the data layers.
bool isValid(const Index &index) const
void setStartIndex(const Index &startIndex)
GridMap getSubmap(const Position &position, const Length &length, bool &isSuccess) const
double getResolution() const
const Position & getPosition() const
Position position_
Map position in the grid map frame [m].
const Index & getStartIndex() const
Position getClosestPositionInMap(const Position &position) const
float & at(const std::string &layer, const Index &index)
Time timestamp_
Timestamp of the grid map (nanoseconds).
const std::string & getFrameId() const
Index startIndex_
Circular buffer start indices.
void add(const std::string &layer, const double value=NAN)
Time getTimestamp() const
Eigen::Vector3d Position3
bool exists(const std::string &layer) const
bool evaluateBicubicConvolutionInterpolation(const GridMap &gridMap, const std::string &layer, const Position &queriedPosition, double *interpolatedValue)
bool getPositionFromIndex(Position &position, const Index &index, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
const Length & getLength() const
bool getPosition3(const std::string &layer, const Index &index, Position3 &position) const
void clearCols(unsigned int index, unsigned int nCols)
void clearRows(unsigned int index, unsigned int nRows)
Length length_
Side length of the map in x- and y-direction [m].
bool getIndexShiftFromPositionShift(Index &indexShift, const Vector &positionShift, const double &resolution)
void setFrameId(const std::string &frameId)
bool getIndex(const Position &position, Index &index) const
void setTimestamp(const Time timestamp)
bool erase(const std::string &layer)
bool getPosition(const Index &index, Position &position) const
bool evaluateBicubicInterpolation(const GridMap &gridMap, const std::string &layer, const Position &queriedPosition, double *interpolatedValue)
const Matrix & operator[](const std::string &layer) const
void resize(const Index &bufferSize)
const Length & getLength() const