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


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Sat Nov 28 2020 03:16:50