GridMap.cpp
Go to the documentation of this file.
1 /*
2  * GridMap.cpp
3  *
4  * Created on: Jul 14, 2014
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
13 
14 #include <Eigen/Dense>
15 
16 #include <iostream>
17 #include <cassert>
18 #include <math.h>
19 #include <algorithm>
20 #include <stdexcept>
21 
22 using namespace std;
23 using namespace grid_map;
24 
25 namespace grid_map {
26 
27 GridMap::GridMap(const std::vector<std::string>& layers)
28 {
29  position_.setZero();
30  length_.setZero();
31  resolution_ = 0.0;
32  size_.setZero();
33  startIndex_.setZero();
34  timestamp_ = 0;
35  layers_ = layers;
36 
37  for (auto& layer : layers_) {
38  data_.insert(std::pair<std::string, Matrix>(layer, Matrix()));
39  }
40 }
41 
42 GridMap::GridMap() :
43  GridMap(std::vector<std::string>())
44 {
45 }
46 
48 {
49 }
50 
51 void GridMap::setGeometry(const Length& length, const double resolution,
52  const Position& position)
53 {
54  assert(length(0) > 0.0);
55  assert(length(1) > 0.0);
56  assert(resolution > 0.0);
57 
58  Size size;
59  size(0) = static_cast<int>(round(length(0) / resolution)); // There is no round() function in Eigen.
60  size(1) = static_cast<int>(round(length(1) / resolution));
61  resize(size);
62  clearAll();
63 
64  resolution_ = resolution;
65  length_ = (size_.cast<double>() * resolution_).matrix();
66  position_ = position;
67  startIndex_.setZero();
68 
69  return;
70 }
71 
72 void GridMap::setGeometry(const SubmapGeometry& geometry)
73 {
74  setGeometry(geometry.getLength(), geometry.getResolution(), geometry.getPosition());
75 }
76 
77 void GridMap::setBasicLayers(const std::vector<std::string>& basicLayers)
78 {
79  basicLayers_ = basicLayers;
80 }
81 
82 const std::vector<std::string>& GridMap::getBasicLayers() const
83 {
84  return basicLayers_;
85 }
86 
88 {
89  return basicLayers_.size() > 0;
90 }
91 
92 bool GridMap::hasSameLayers(const GridMap& other) const
93 {
94  for (const auto& layer : layers_) {
95  if (!other.exists(layer)) return false;
96  }
97  return true;
98 }
99 
100 void GridMap::add(const std::string& layer, const double value)
101 {
102  add(layer, Matrix::Constant(size_(0), size_(1), value));
103 }
104 
105 void GridMap::add(const std::string& layer, const Matrix& data)
106 {
107  assert(size_(0) == data.rows());
108  assert(size_(1) == data.cols());
109 
110  if (exists(layer)) {
111  // Type exists already, overwrite its data.
112  data_.at(layer) = data;
113  } else {
114  // Type does not exist yet, add type and data.
115  data_.insert(std::pair<std::string, Matrix>(layer, data));
116  layers_.push_back(layer);
117  }
118 }
119 
120 bool GridMap::exists(const std::string& layer) const
121 {
122  return !(data_.find(layer) == data_.end());
123 }
124 
125 const Matrix& GridMap::get(const std::string& layer) const
126 {
127  try {
128  return data_.at(layer);
129  } catch (const std::out_of_range& exception) {
130  throw std::out_of_range("GridMap::get(...) : No map layer '" + layer + "' available.");
131  }
132 }
133 
134 Matrix& GridMap::get(const std::string& layer)
135 {
136  try {
137  return data_.at(layer);
138  } catch (const std::out_of_range& exception) {
139  throw std::out_of_range("GridMap::get(...) : No map layer of type '" + layer + "' available.");
140  }
141 }
142 
143 const Matrix& GridMap::operator [](const std::string& layer) const
144 {
145  return get(layer);
146 }
147 
148 Matrix& GridMap::operator [](const std::string& layer)
149 {
150  return get(layer);
151 }
152 
153 bool GridMap::erase(const std::string& layer)
154 {
155  const auto dataIterator = data_.find(layer);
156  if (dataIterator == data_.end()) return false;
157  data_.erase(dataIterator);
158 
159  const auto layerIterator = std::find(layers_.begin(), layers_.end(), layer);
160  if (layerIterator == layers_.end()) return false;
161  layers_.erase(layerIterator);
162 
163  const auto basicLayerIterator = std::find(basicLayers_.begin(), basicLayers_.end(), layer);
164  if (basicLayerIterator != basicLayers_.end()) basicLayers_.erase(basicLayerIterator);
165 
166  return true;
167 }
168 
169 const std::vector<std::string>& GridMap::getLayers() const
170 {
171  return layers_;
172 }
173 
174 float& GridMap::atPosition(const std::string& layer, const Position& position)
175 {
176  Index index;
177  if (getIndex(position, index)) {
178  return at(layer, index);
179  }
180  throw std::out_of_range("GridMap::atPosition(...) : Position is out of range.");
181 }
182 
183 float GridMap::atPosition(const std::string& layer, const Position& position, InterpolationMethods interpolationMethod) const
184 {
185  switch (interpolationMethod) {
187  {
188  float value;
189  if (atPositionLinearInterpolated(layer, position, value))
190  return value;
191  else
192  interpolationMethod = InterpolationMethods::INTER_NEAREST;
193  }
195  {
196  Index index;
197  if (getIndex(position, index)) {
198  return at(layer, index);
199  }
200  else
201  throw std::out_of_range("GridMap::atPosition(...) : Position is out of range.");
202  break;
203  }
204  default:
205  throw std::runtime_error("GridMap::atPosition(...) : Specified interpolation method not implemented.");
206  }
207 }
208 
209 float& GridMap::at(const std::string& layer, const Index& index)
210 {
211  try {
212  return data_.at(layer)(index(0), index(1));
213  } catch (const std::out_of_range& exception) {
214  throw std::out_of_range("GridMap::at(...) : No map layer '" + layer + "' available.");
215  }
216 }
217 
218 float GridMap::at(const std::string& layer, const Index& index) const
219 {
220  try {
221  return data_.at(layer)(index(0), index(1));
222  } catch (const std::out_of_range& exception) {
223  throw std::out_of_range("GridMap::at(...) : No map layer '" + layer + "' available.");
224  }
225 }
226 
227 bool GridMap::getIndex(const Position& position, Index& index) const
228 {
229  return getIndexFromPosition(index, position, length_, position_, resolution_, size_, startIndex_);
230 }
231 
232 bool GridMap::getPosition(const Index& index, Position& position) const
233 {
234  return getPositionFromIndex(position, index, length_, position_, resolution_, size_, startIndex_);
235 }
236 
237 bool GridMap::isInside(const Position& position) const
238 {
239  return checkIfPositionWithinMap(position, length_, position_);
240 }
241 
242 bool GridMap::isValid(const Index& index) const
243 {
244  return isValid(index, basicLayers_);
245 }
246 
247 bool GridMap::isValid(const Index& index, const std::string& layer) const
248 {
249  if (!isfinite(at(layer, index))) return false;
250  return true;
251 }
252 
253 bool GridMap::isValid(const Index& index, const std::vector<std::string>& layers) const
254 {
255  if (layers.empty()) return false;
256  for (auto& layer : layers) {
257  if (!isfinite(at(layer, index))) return false;
258  }
259  return true;
260 }
261 
262 bool GridMap::getPosition3(const std::string& layer, const Index& index,
263  Position3& position) const
264 {
265  if (!isValid(index, layer)) return false;
266  Position position2d;
267  getPosition(index, position2d);
268  position.head(2) = position2d;
269  position.z() = at(layer, index);
270  return true;
271 }
272 
273 bool GridMap::getVector(const std::string& layerPrefix, const Index& index,
274  Eigen::Vector3d& vector) const
275 {
276  std::vector<std::string> layers;
277  layers.push_back(layerPrefix + "x");
278  layers.push_back(layerPrefix + "y");
279  layers.push_back(layerPrefix + "z");
280  if (!isValid(index, layers)) return false;
281  for (size_t i = 0; i < 3; ++i) {
282  vector(i) = at(layers[i], index);
283  }
284  return true;
285 }
286 
287 GridMap GridMap::getSubmap(const Position& position, const Length& length,
288  bool& isSuccess) const
289 {
290  Index index;
291  return getSubmap(position, length, index, isSuccess);
292 }
293 
294 GridMap GridMap::getSubmap(const Position& position, const Length& length,
295  Index& indexInSubmap, bool& isSuccess) const
296 {
297  // Submap the generate.
298  GridMap submap(layers_);
300  submap.setTimestamp(timestamp_);
301  submap.setFrameId(frameId_);
302 
303  // Get submap geometric information.
304  SubmapGeometry submapInformation(*this, position, length, isSuccess);
305  if (isSuccess == false) return GridMap(layers_);
306  submap.setGeometry(submapInformation);
307  submap.startIndex_.setZero(); // Because of the way we copy the data below.
308 
309  // Copy data.
310  std::vector<BufferRegion> bufferRegions;
311 
312  if (!getBufferRegionsForSubmap(bufferRegions, submapInformation.getStartIndex(),
313  submap.getSize(), size_, startIndex_)) {
314  cout << "Cannot access submap of this size." << endl;
315  isSuccess = false;
316  return GridMap(layers_);
317  }
318 
319  for (const auto& data : data_) {
320  for (const auto& bufferRegion : bufferRegions) {
321  Index index = bufferRegion.getStartIndex();
322  Size size = bufferRegion.getSize();
323 
324  if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopLeft) {
325  submap.data_[data.first].topLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
326  } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopRight) {
327  submap.data_[data.first].topRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
328  } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomLeft) {
329  submap.data_[data.first].bottomLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
330  } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomRight) {
331  submap.data_[data.first].bottomRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
332  }
333 
334  }
335  }
336 
337  isSuccess = true;
338  return submap;
339 }
340 
341 GridMap GridMap::getTransformedMap(const Eigen::Isometry3d& transform, const std::string& heightLayerName,
342  const std::string& newFrameId,
343  const double sampleRatio) const
344 {
345  // Check if height layer is valid.
346  if (!exists(heightLayerName)) {
347  throw std::out_of_range("GridMap::getTransformedMap(...) : No map layer '" + heightLayerName + "' available.");
348  }
349 
350  // Initialization.
351  std::vector<Position3> positionSamples;
352  Position3 center;
353  Index newIndex;
354 
355  const double sampleLength = resolution_*sampleRatio;
356 
357  // Find edges in new coordinate frame.
358  const double halfLengthX = length_.x()*0.5;
359  const double halfLengthY = length_.y()*0.5;
360  const Position3 topLeftCorner (position_.x() + halfLengthX, position_.y() + halfLengthY, 0.0);
361  const Position3 topRightCorner (position_.x() + halfLengthX, position_.y() - halfLengthY, 0.0);
362  const Position3 bottomLeftCorner (position_.x() - halfLengthX, position_.y() + halfLengthY, 0.0);
363  const Position3 bottomRightCorner (position_.x() - halfLengthX, position_.y() - halfLengthY, 0.0);
364 
365  std::vector<Position3> newEdges;
366  newEdges.reserve(4);
367  newEdges.push_back(transform * topLeftCorner);
368  newEdges.push_back(transform * topRightCorner);
369  newEdges.push_back(transform * bottomLeftCorner);
370  newEdges.push_back(transform * bottomRightCorner);
371 
372 
373  // Find new grid center.
374  Position3 newCenter = Position3::Zero();
375  for (const auto& newEdge : newEdges) { newCenter += newEdge; }
376  newCenter *= 0.25;
377 
378  // Find new grid length.
379  Length maxLengthFromCenter = Length(0.0, 0.0);
380  for (const auto& newEdge : newEdges) {
381  Position3 positionCenterToEdge = newEdge-newCenter;
382  maxLengthFromCenter.x() = std::fmax(std::fabs(positionCenterToEdge.x()), maxLengthFromCenter.x());
383  maxLengthFromCenter.y() = std::fmax(std::fabs(positionCenterToEdge.y()), maxLengthFromCenter.y());
384  }
385  Length newLength = 2.0*maxLengthFromCenter;
386 
387  // Create new grid map.
388  GridMap newMap(layers_);
390  newMap.setTimestamp(timestamp_);
391  newMap.setFrameId(newFrameId);
392  newMap.setGeometry(newLength, resolution_, Position(newCenter.x(), newCenter.y()));
393  newMap.startIndex_.setZero();
394 
395  for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
396  // Get position at current index.
397  if(!getPosition3(heightLayerName, *iterator, center)) { continue; }
398 
399  // Sample four points around the center cell.
400  positionSamples.clear();
401 
402  if (sampleRatio>0.0) {
403  positionSamples.reserve(5);
404  positionSamples.push_back(center);
405  positionSamples.push_back(Position3(center.x() - sampleLength,
406  center.y(),
407  center.z()));
408  positionSamples.push_back(Position3(center.x() + sampleLength,
409  center.y(),
410  center.z()));
411  positionSamples.push_back(Position3(center.x(),
412  center.y() - sampleLength,
413  center.z()));
414  positionSamples.push_back(Position3(center.x(),
415  center.y() + sampleLength,
416  center.z()));
417  } else {
418  positionSamples.push_back(center);
419  }
420 
421 
422  // Transform the sampled points and register to the new map.
423  for (const auto& position : positionSamples){
424  const Position3 transformedPosition = transform * position;
425 
426  // Get new index.
427  if(!newMap.getIndex(Position(transformedPosition.x(), transformedPosition.y()), newIndex)) { continue; }
428 
429  // Check if we have already assigned a value (preferably larger height values -> inpainting).
430  const auto newExistingValue = newMap.at(heightLayerName, newIndex);
431  if(!std::isnan(newExistingValue) && newExistingValue > transformedPosition.z()){
432  continue;
433  }
434 
435  // Copy the layers.
436  for(const auto& layer: layers_){
437  const auto currentValueInOldGrid = at(layer, *iterator);
438  auto& newValue = newMap.at(layer, newIndex);
439  if(layer == heightLayerName) { newValue = transformedPosition.z(); } // adjust height
440  else { newValue = currentValueInOldGrid; } // re-assign
441  }
442  }
443  }
444 
445  return newMap;
446 }
447 
448 void GridMap::setPosition(const Position& position)
449 {
450  position_ = position;
451 }
452 
453 bool GridMap::move(const Position& position, std::vector<BufferRegion>& newRegions)
454 {
455  Index indexShift;
456  Position positionShift = position - position_;
457  getIndexShiftFromPositionShift(indexShift, positionShift, resolution_);
458  Position alignedPositionShift;
459  getPositionShiftFromIndexShift(alignedPositionShift, indexShift, resolution_);
460 
461  // Delete fields that fall out of map (and become empty cells).
462  for (int i = 0; i < indexShift.size(); i++) {
463  if (indexShift(i) != 0) {
464  if (abs(indexShift(i)) >= getSize()(i)) {
465  // Entire map is dropped.
466  clearAll();
467  newRegions.push_back(BufferRegion(Index(0, 0), getSize(), BufferRegion::Quadrant::Undefined));
468  } else {
469  // Drop cells out of map.
470  int sign = (indexShift(i) > 0 ? 1 : -1);
471  int startIndex = startIndex_(i) - (sign < 0 ? 1 : 0);
472  int endIndex = startIndex - sign + indexShift(i);
473  int nCells = abs(indexShift(i));
474  int index = (sign > 0 ? startIndex : endIndex);
475  wrapIndexToRange(index, getSize()(i));
476 
477  if (index + nCells <= getSize()(i)) {
478  // One region to drop.
479  if (i == 0) {
480  clearRows(index, nCells);
481  newRegions.push_back(BufferRegion(Index(index, 0), Size(nCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
482  } else if (i == 1) {
483  clearCols(index, nCells);
484  newRegions.push_back(BufferRegion(Index(0, index), Size(getSize()(0), nCells), BufferRegion::Quadrant::Undefined));
485  }
486  } else {
487  // Two regions to drop.
488  int firstIndex = index;
489  int firstNCells = getSize()(i) - firstIndex;
490  if (i == 0) {
491  clearRows(firstIndex, firstNCells);
492  newRegions.push_back(BufferRegion(Index(firstIndex, 0), Size(firstNCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
493  } else if (i == 1) {
494  clearCols(firstIndex, firstNCells);
495  newRegions.push_back(BufferRegion(Index(0, firstIndex), Size(getSize()(0), firstNCells), BufferRegion::Quadrant::Undefined));
496  }
497 
498  int secondIndex = 0;
499  int secondNCells = nCells - firstNCells;
500  if (i == 0) {
501  clearRows(secondIndex, secondNCells);
502  newRegions.push_back(BufferRegion(Index(secondIndex, 0), Size(secondNCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
503  } else if (i == 1) {
504  clearCols(secondIndex, secondNCells);
505  newRegions.push_back(BufferRegion(Index(0, secondIndex), Size(getSize()(0), secondNCells), BufferRegion::Quadrant::Undefined));
506  }
507  }
508  }
509  }
510  }
511 
512  // Update information.
513  startIndex_ += indexShift;
515  position_ += alignedPositionShift;
516 
517  // Check if map has been moved at all.
518  return (indexShift.any() != 0);
519 }
520 
521 bool GridMap::move(const Position& position)
522 {
523  std::vector<BufferRegion> newRegions;
524  return move(position, newRegions);
525 }
526 
527 bool GridMap::addDataFrom(const GridMap& other, bool extendMap, bool overwriteData,
528  bool copyAllLayers, std::vector<std::string> layers)
529 {
530  // Set the layers to copy.
531  if (copyAllLayers) layers = other.getLayers();
532 
533  // Resize map.
534  if (extendMap) extendToInclude(other);
535 
536  // Check if all layers to copy exist and add missing layers.
537  for (const auto& layer : layers) {
538  if (std::find(layers_.begin(), layers_.end(), layer) == layers_.end()) {
539  add(layer);
540  }
541  }
542  // Copy data.
543  for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
544  if (isValid(*iterator) && !overwriteData) continue;
545  Position position;
546  getPosition(*iterator, position);
547  Index index;
548  if (!other.isInside(position)) continue;
549  other.getIndex(position, index);
550  for (const auto& layer : layers) {
551  if (!other.isValid(index, layer)) continue;
552  at(layer, *iterator) = other.at(layer, index);
553  }
554  }
555 
556  return true;
557 }
558 
560 {
561  // Get dimension of maps.
562  Position topLeftCorner(position_.x() + length_.x() / 2.0, position_.y() + length_.y() / 2.0);
563  Position bottomRightCorner(position_.x() - length_.x() / 2.0, position_.y() - length_.y() / 2.0);
564  Position topLeftCornerOther(other.getPosition().x() + other.getLength().x() / 2.0, other.getPosition().y() + other.getLength().y() / 2.0);
565  Position bottomRightCornerOther(other.getPosition().x() - other.getLength().x() / 2.0, other.getPosition().y() - other.getLength().y() / 2.0);
566  // Check if map needs to be resized.
567  bool resizeMap = false;
568  Position extendedMapPosition = position_;
569  Length extendedMapLength = length_;
570  if (topLeftCornerOther.x() > topLeftCorner.x()) {
571  extendedMapPosition.x() += (topLeftCornerOther.x() - topLeftCorner.x()) / 2.0;
572  extendedMapLength.x() += topLeftCornerOther.x() - topLeftCorner.x();
573  resizeMap = true;
574  }
575  if (topLeftCornerOther.y() > topLeftCorner.y()) {
576  extendedMapPosition.y() += (topLeftCornerOther.y() - topLeftCorner.y()) / 2.0;
577  extendedMapLength.y() += topLeftCornerOther.y() - topLeftCorner.y();
578  resizeMap = true;
579  }
580  if (bottomRightCornerOther.x() < bottomRightCorner.x()) {
581  extendedMapPosition.x() -= (bottomRightCorner.x() - bottomRightCornerOther.x()) / 2.0;
582  extendedMapLength.x() += bottomRightCorner.x() - bottomRightCornerOther.x();
583  resizeMap = true;
584  }
585  if (bottomRightCornerOther.y() < bottomRightCorner.y()) {
586  extendedMapPosition.y() -= (bottomRightCorner.y() - bottomRightCornerOther.y()) / 2.0;
587  extendedMapLength.y() += bottomRightCorner.y() - bottomRightCornerOther.y();
588  resizeMap = true;
589  }
590  // Resize map and copy data to new map.
591  if (resizeMap) {
592  GridMap mapCopy = *this;
593  setGeometry(extendedMapLength, resolution_, extendedMapPosition);
594  // Align new map with old one.
595  Vector shift = position_ - mapCopy.getPosition();
596  shift.x() = std::fmod(shift.x(), resolution_);
597  shift.y() = std::fmod(shift.y(), resolution_);
598  if (std::abs(shift.x()) < resolution_ / 2.0) {
599  position_.x() -= shift.x();
600  } else {
601  position_.x() += resolution_ - shift.x();
602  }
603  if (size_.x() % 2 != mapCopy.getSize().x() % 2) {
604  position_.x() += -std::copysign(resolution_ / 2.0, shift.x());
605  }
606  if (std::abs(shift.y()) < resolution_ / 2.0) {
607  position_.y() -= shift.y();
608  } else {
609  position_.y() += resolution_ - shift.y();
610  }
611  if (size_.y() % 2 != mapCopy.getSize().y() % 2) {
612  position_.y() += -std::copysign(resolution_ / 2.0, shift.y());
613  }
614  // Copy data.
615  for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
616  if (isValid(*iterator)) continue;
617  Position position;
618  getPosition(*iterator, position);
619  Index index;
620  if (!mapCopy.isInside(position)) continue;
621  mapCopy.getIndex(position, index);
622  for (const auto& layer : layers_) {
623  at(layer, *iterator) = mapCopy.at(layer, index);
624  }
625  }
626  }
627  return true;
628 }
629 
630 void GridMap::setTimestamp(const Time timestamp)
631 {
632  timestamp_ = timestamp;
633 }
634 
636 {
637  return timestamp_;
638 }
639 
641 {
642  timestamp_ = 0.0;
643 }
644 
645 void GridMap::setFrameId(const std::string& frameId)
646 {
647  frameId_ = frameId;
648 }
649 
650 const std::string& GridMap::getFrameId() const
651 {
652  return frameId_;
653 }
654 
656 {
657  return length_;
658 }
659 
661 {
662  return position_;
663 }
664 
666 {
667  return resolution_;
668 }
669 
670 const Size& GridMap::getSize() const
671 {
672  return size_;
673 }
674 
675 void GridMap::setStartIndex(const Index& startIndex) {
676  startIndex_ = startIndex;
677 }
678 
680 {
681  return startIndex_;
682 }
683 
685 {
686  return (startIndex_ == 0).all();
687 }
688 
690 {
691  if (isDefaultStartIndex()) return;
692 
693  std::vector<BufferRegion> bufferRegions;
694  if (!getBufferRegionsForSubmap(bufferRegions, startIndex_, size_, size_, startIndex_)) {
695  throw std::out_of_range("Cannot access submap of this size.");
696  }
697 
698  for (auto& data : data_) {
699  auto tempData(data.second);
700  for (const auto& bufferRegion : bufferRegions) {
701  Index index = bufferRegion.getStartIndex();
702  Size size = bufferRegion.getSize();
703 
704  if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopLeft) {
705  tempData.topLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
706  } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopRight) {
707  tempData.topRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
708  } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomLeft) {
709  tempData.bottomLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
710  } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomRight) {
711  tempData.bottomRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
712  }
713  }
714  data.second = tempData;
715  }
716 
717  startIndex_.setZero();
718 }
719 
720 void GridMap::clear(const std::string& layer)
721 {
722  try {
723  data_.at(layer).setConstant(NAN);
724  } catch (const std::out_of_range& exception) {
725  throw std::out_of_range("GridMap::clear(...) : No map layer '" + layer + "' available.");
726  }
727 }
728 
730 {
731  for (auto& layer : basicLayers_) {
732  clear(layer);
733  }
734 }
735 
737 {
738  for (auto& data : data_) {
739  data.second.setConstant(NAN);
740  }
741 }
742 
743 void GridMap::clearRows(unsigned int index, unsigned int nRows)
744 {
745  std::vector<std::string> layersToClear;
746  if (basicLayers_.size() > 0) layersToClear = basicLayers_;
747  else layersToClear = layers_;
748  for (auto& layer : layersToClear) {
749  data_.at(layer).block(index, 0, nRows, getSize()(1)).setConstant(NAN);
750  }
751 }
752 
753 void GridMap::clearCols(unsigned int index, unsigned int nCols)
754 {
755  std::vector<std::string> layersToClear;
756  if (basicLayers_.size() > 0) layersToClear = basicLayers_;
757  else layersToClear = layers_;
758  for (auto& layer : layersToClear) {
759  data_.at(layer).block(0, index, getSize()(0), nCols).setConstant(NAN);
760  }
761 }
762 
763 bool GridMap::atPositionLinearInterpolated(const std::string& layer, const Position& position,
764  float& value) const
765 {
766  Position point;
767  Index indices[4];
768  bool idxTempDir;
769  size_t idxShift[4];
770 
771  getIndex(position, indices[0]);
772  getPosition(indices[0], point);
773 
774  if (position.x() >= point.x()) {
775  indices[1] = indices[0] + Index(-1, 0); // Second point is above first point.
776  idxTempDir = true;
777  } else {
778  indices[1] = indices[0] + Index(+1, 0);
779  idxTempDir = false;
780  }
781  if (position.y() >= point.y()) {
782  indices[2] = indices[0] + Index(0, -1); // Third point is right of first point.
783  if(idxTempDir){ idxShift[0]=0; idxShift[1]=1; idxShift[2]=2; idxShift[3]=3; }
784  else { idxShift[0]=1; idxShift[1]=0; idxShift[2]=3; idxShift[3]=2; }
785 
786 
787  } else {
788  indices[2] = indices[0] + Index(0, +1);
789  if(idxTempDir){ idxShift[0]=2; idxShift[1]=3; idxShift[2]=0; idxShift[3]=1; }
790  else { idxShift[0]=3; idxShift[1]=2; idxShift[2]=1; idxShift[3]=0; }
791  }
792  indices[3].x() = indices[1].x();
793  indices[3].y() = indices[2].y();
794 
795  const Size& mapSize = getSize();
796  const size_t bufferSize = mapSize(0) * mapSize(1);
797  const size_t startIndexLin = getLinearIndexFromIndex(startIndex_, mapSize);
798  const size_t endIndexLin = startIndexLin + bufferSize;
799  const auto& layerMat = operator[](layer);
800  float f[4];
801 
802  for (size_t i = 0; i < 4; ++i) {
803  const size_t indexLin = getLinearIndexFromIndex(indices[idxShift[i]], mapSize);
804  if ((indexLin < startIndexLin) || (indexLin > endIndexLin)) return false;
805  f[i] = layerMat(indexLin);
806  }
807 
808  getPosition(indices[idxShift[0]], point);
809  const Position positionRed = ( position - point ) / resolution_;
810  const Position positionRedFlip = Position(1.,1.) - positionRed;
811 
812  value = f[0] * positionRedFlip.x() * positionRedFlip.y() +
813  f[1] * positionRed.x() * positionRedFlip.y() +
814  f[2] * positionRedFlip.x() * positionRed.y() +
815  f[3] * positionRed.x() * positionRed.y();
816  return true;
817 }
818 
819 void GridMap::resize(const Index& size)
820 {
821  size_ = size;
822  for (auto& data : data_) {
823  data.second.resize(size_(0), size_(1));
824  }
825 }
826 
827 } /* namespace */
828 
const Length & getLength() const
Definition: GridMap.cpp:655
double getResolution() const
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
Definition: GridMap.cpp:51
Time getTimestamp() const
Definition: GridMap.cpp:635
Size size_
Size of the buffer (rows and cols of the data structure).
Definition: GridMap.hpp:537
void setPosition(const Position &position)
Definition: GridMap.cpp:448
float & atPosition(const std::string &layer, const Position &position)
Definition: GridMap.cpp:174
Eigen::Vector2d Vector
Definition: TypeDefs.hpp:19
const Index & getStartIndex() const
Definition: GridMap.cpp:679
void clear(const std::string &layer)
Definition: GridMap.cpp:720
bool getPosition(const Index &index, Position &position) const
Definition: GridMap.cpp:232
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)
Definition: GridMap.cpp:77
const Position & getPosition() const
Definition: GridMap.cpp:660
Eigen::Array2i Size
Definition: TypeDefs.hpp:23
const std::string & getFrameId() const
Definition: GridMap.cpp:650
double resolution_
Map resolution in xy plane [m/cell].
Definition: GridMap.hpp:531
GridMap getTransformedMap(const Eigen::Isometry3d &transform, const std::string &heightLayerName, const std::string &newFrameId, const double sampleRatio=0.0) const
Definition: GridMap.cpp:341
std::vector< std::string > basicLayers_
Definition: GridMap.hpp:525
std::unordered_map< std::string, Matrix > data_
Grid map data stored as layers of matrices.
Definition: GridMap.hpp:517
Eigen::MatrixXf Matrix
Definition: TypeDefs.hpp:16
bool getVector(const std::string &layerPrefix, const Index &index, Eigen::Vector3d &vector) const
Definition: GridMap.cpp:273
std::string frameId_
Frame id of the grid map.
Definition: GridMap.hpp:511
const Matrix & operator[](const std::string &layer) const
Definition: GridMap.cpp:143
grid_map::Matrix Matrix
Definition: GridMap.hpp:45
bool move(const Position &position, std::vector< BufferRegion > &newRegions)
Definition: GridMap.cpp:453
void convertToDefaultStartIndex()
Definition: GridMap.cpp:689
InterpolationMethods
Definition: TypeDefs.hpp:27
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 >())
Definition: GridMap.cpp:527
GridMap getSubmap(const Position &position, const Length &length, bool &isSuccess) const
Definition: GridMap.cpp:287
bool isValid(const Index &index) const
Definition: GridMap.cpp:242
const Length & getLength() const
Eigen::Vector3d Position3
Definition: TypeDefs.hpp:20
bool exists(const std::string &layer) const
Definition: GridMap.cpp:120
bool extendToInclude(const GridMap &other)
Definition: GridMap.cpp:559
bool checkIfPositionWithinMap(const Position &position, const Length &mapLength, const Position &mapPosition)
double getResolution() const
Definition: GridMap.cpp:665
const std::vector< std::string > & getLayers() const
Definition: GridMap.cpp:169
bool getPositionShiftFromIndexShift(Vector &positionShift, const Index &indexShift, const double &resolution)
std::vector< std::string > layers_
Names of the data layers.
Definition: GridMap.hpp:520
bool isInside(const Position &position) const
Definition: GridMap.cpp:237
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
bool isDefaultStartIndex() const
Definition: GridMap.cpp:684
const Matrix & get(const std::string &layer) const
Definition: GridMap.cpp:125
void resetTimestamp()
Definition: GridMap.cpp:640
const Position & getPosition() const
void setStartIndex(const Index &startIndex)
Definition: GridMap.cpp:675
uint64_t Time
Definition: TypeDefs.hpp:25
virtual ~GridMap()
Definition: GridMap.cpp:47
Position position_
Map position in the grid map frame [m].
Definition: GridMap.hpp:534
float & at(const std::string &layer, const Index &index)
Definition: GridMap.cpp:209
bool hasSameLayers(const grid_map::GridMap &other) const
Definition: GridMap.cpp:92
Time timestamp_
Timestamp of the grid map (nanoseconds).
Definition: GridMap.hpp:514
bool hasBasicLayers() const
Definition: GridMap.cpp:87
Index startIndex_
Circular buffer start indeces.
Definition: GridMap.hpp:540
void add(const std::string &layer, const double value=NAN)
Definition: GridMap.cpp:100
size_t getLinearIndexFromIndex(const Index &index, const Size &bufferSize, const bool rowMajor=false)
bool atPositionLinearInterpolated(const std::string &layer, const Position &position, float &value) const
Definition: GridMap.cpp:763
bool getPositionFromIndex(Position &position, const Index &index, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
void clearCols(unsigned int index, unsigned int nCols)
Definition: GridMap.cpp:753
void clearRows(unsigned int index, unsigned int nRows)
Definition: GridMap.cpp:743
Length length_
Side length of the map in x- and y-direction [m].
Definition: GridMap.hpp:528
bool getIndexShiftFromPositionShift(Index &indexShift, const Vector &positionShift, const double &resolution)
void setFrameId(const std::string &frameId)
Definition: GridMap.cpp:645
const Index & getStartIndex() const
void setTimestamp(const Time timestamp)
Definition: GridMap.cpp:630
bool erase(const std::string &layer)
Definition: GridMap.cpp:153
bool getIndex(const Position &position, Index &index) const
Definition: GridMap.cpp:227
bool getPosition3(const std::string &layer, const Index &index, Position3 &position) const
Definition: GridMap.cpp:262
Eigen::Array2d Length
Definition: TypeDefs.hpp:24
void resize(const Index &bufferSize)
Definition: GridMap.cpp:819
const std::vector< std::string > & getBasicLayers() const
Definition: GridMap.cpp:82
const Size & getSize() const
Definition: GridMap.cpp:670


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jun 25 2019 20:02:08