GridMapMath.cpp
Go to the documentation of this file.
1 /*
2  * GridMapMath.cpp
3  *
4  * Created on: Dec 2, 2013
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
9 #include "grid_map_core/GridMapMath.hpp"
10 
11 // fabs
12 #include <cmath>
13 
14 // Limits
15 #include <limits>
16 
17 using namespace std;
18 
19 namespace grid_map {
20 
21 namespace internal {
22 
30 inline bool getVectorToOrigin(Vector& vectorToOrigin, const Length& mapLength)
31 {
32  vectorToOrigin = (0.5 * mapLength).matrix();
33  return true;
34 }
35 
44 inline bool getVectorToFirstCell(Vector& vectorToFirstCell,
45  const Length& mapLength, const double& resolution)
46 {
47  Vector vectorToOrigin;
48  getVectorToOrigin(vectorToOrigin, mapLength);
49 
50  // Vector to center of cell.
51  vectorToFirstCell = (vectorToOrigin.array() - 0.5 * resolution).matrix();
52  return true;
53 }
54 
55 inline Eigen::Matrix2i getBufferOrderToMapFrameTransformation()
56 {
57  return -Eigen::Matrix2i::Identity();
58 }
59 
60 inline Eigen::Matrix2i getMapFrameToBufferOrderTransformation()
61 {
62  return getBufferOrderToMapFrameTransformation().transpose();
63 }
64 
65 inline bool checkIfStartIndexAtDefaultPosition(const Index& bufferStartIndex)
66 {
67  return ((bufferStartIndex == 0).all());
68 }
69 
70 inline Vector getIndexVectorFromIndex(
71  const Index& index,
72  const Size& bufferSize,
73  const Index& bufferStartIndex)
74 {
75  Index unwrappedIndex;
76  unwrappedIndex = getIndexFromBufferIndex(index, bufferSize, bufferStartIndex);
77  return (getBufferOrderToMapFrameTransformation() * unwrappedIndex.matrix()).cast<double>();
78 }
79 
80 inline Index getIndexFromIndexVector(
81  const Vector& indexVector,
82  const Size& bufferSize,
83  const Index& bufferStartIndex)
84 {
85  Index index = (getMapFrameToBufferOrderTransformation() * indexVector.cast<int>()).array();
86  return getBufferIndexFromIndex(index, bufferSize, bufferStartIndex);
87 }
88 
89 inline BufferRegion::Quadrant getQuadrant(const Index& index, const Index& bufferStartIndex)
90 {
91  if (index[0] >= bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) return BufferRegion::Quadrant::TopLeft;
92  if (index[0] >= bufferStartIndex[0] && index[1] < bufferStartIndex[1]) return BufferRegion::Quadrant::TopRight;
93  if (index[0] < bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) return BufferRegion::Quadrant::BottomLeft;
94  if (index[0] < bufferStartIndex[0] && index[1] < bufferStartIndex[1]) return BufferRegion::Quadrant::BottomRight;
95  return BufferRegion::Quadrant::Undefined;
96 }
97 
98 } // namespace
99 
100 using namespace internal;
101 
102 bool getPositionFromIndex(Position& position,
103  const Index& index,
104  const Length& mapLength,
105  const Position& mapPosition,
106  const double& resolution,
107  const Size& bufferSize,
108  const Index& bufferStartIndex)
109 {
110  if (!checkIfIndexInRange(index, bufferSize)) return false;
111  Vector offset;
112  getVectorToFirstCell(offset, mapLength, resolution);
113  position = mapPosition + offset + resolution * getIndexVectorFromIndex(index, bufferSize, bufferStartIndex);
114  return true;
115 }
116 
117 bool getIndexFromPosition(Index& index,
118  const Position& position,
119  const Length& mapLength,
120  const Position& mapPosition,
121  const double& resolution,
122  const Size& bufferSize,
123  const Index& bufferStartIndex)
124 {
125  Vector offset;
126  getVectorToOrigin(offset, mapLength);
127  Vector indexVector = ((position - offset - mapPosition).array() / resolution).matrix();
128  index = getIndexFromIndexVector(indexVector, bufferSize, bufferStartIndex);
129  if (!checkIfPositionWithinMap(position, mapLength, mapPosition)) return false;
130  return true;
131 }
132 
133 bool checkIfPositionWithinMap(const Position& position,
134  const Length& mapLength,
135  const Position& mapPosition)
136 {
137  Vector offset;
138  getVectorToOrigin(offset, mapLength);
139  Position positionTransformed = getMapFrameToBufferOrderTransformation().cast<double>() * (position - mapPosition - offset);
140 
141  if (positionTransformed.x() >= 0.0 && positionTransformed.y() >= 0.0
142  && positionTransformed.x() < mapLength(0) && positionTransformed.y() < mapLength(1)) {
143  return true;
144  }
145  return false;
146 }
147 
148 void getPositionOfDataStructureOrigin(const Position& position,
149  const Length& mapLength,
150  Position& positionOfOrigin)
151 {
152  Vector vectorToOrigin;
153  getVectorToOrigin(vectorToOrigin, mapLength);
154  positionOfOrigin = position + vectorToOrigin;
155 }
156 
157 bool getIndexShiftFromPositionShift(Index& indexShift,
158  const Vector& positionShift,
159  const double& resolution)
160 {
161  Vector indexShiftVectorTemp = (positionShift.array() / resolution).matrix();
162  Eigen::Vector2i indexShiftVector;
163 
164  for (int i = 0; i < indexShiftVector.size(); i++) {
165  indexShiftVector[i] = static_cast<int>(indexShiftVectorTemp[i] + 0.5 * (indexShiftVectorTemp[i] > 0 ? 1 : -1));
166  }
167 
168  indexShift = (getMapFrameToBufferOrderTransformation() * indexShiftVector).array();
169  return true;
170 }
171 
172 bool getPositionShiftFromIndexShift(Vector& positionShift,
173  const Index& indexShift,
174  const double& resolution)
175 {
176  positionShift = (getBufferOrderToMapFrameTransformation() * indexShift.matrix()).cast<double>() * resolution;
177  return true;
178 }
179 
180 bool checkIfIndexInRange(const Index& index, const Size& bufferSize)
181 {
182  if (index[0] >= 0 && index[1] >= 0 && index[0] < bufferSize[0] && index[1] < bufferSize[1])
183  {
184  return true;
185  }
186  return false;
187 }
188 
189 void boundIndexToRange(Index& index, const Size& bufferSize)
190 {
191  for (int i = 0; i < index.size(); i++) {
192  boundIndexToRange(index[i], bufferSize[i]);
193  }
194 }
195 
196 void boundIndexToRange(int& index, const int& bufferSize)
197 {
198  if (index < 0) index = 0;
199  else if (index >= bufferSize) index = bufferSize - 1;
200 }
201 
202 void wrapIndexToRange(Index& index, const Size& bufferSize)
203 {
204  for (int i = 0; i < index.size(); i++) {
205  wrapIndexToRange(index[i], bufferSize[i]);
206  }
207 }
208 
209 void wrapIndexToRange(int& index, const int& bufferSize)
210 {
211  if (index < 0) index += ((-index / bufferSize) + 1) * bufferSize;
212  index = index % bufferSize;
213 }
214 
215 void boundPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition)
216 {
217  Vector vectorToOrigin;
218  getVectorToOrigin(vectorToOrigin, mapLength);
219  Position positionShifted = position - mapPosition + vectorToOrigin;
220 
221  // We have to make sure to stay inside the map.
222  for (int i = 0; i < positionShifted.size(); i++) {
223 
224  double epsilon = 10.0 * numeric_limits<double>::epsilon(); // TODO Why is the factor 10 necessary.
225  if (std::fabs(position(i)) > 1.0) epsilon *= std::fabs(position(i));
226 
227  if (positionShifted(i) <= 0) {
228  positionShifted(i) = epsilon;
229  continue;
230  }
231  if (positionShifted(i) >= mapLength(i)) {
232  positionShifted(i) = mapLength(i) - epsilon;
233  continue;
234  }
235  }
236 
237  position = positionShifted + mapPosition - vectorToOrigin;
238 }
239 
240 const Eigen::Matrix2i getBufferOrderToMapFrameAlignment()
241 {
242  return getBufferOrderToMapFrameTransformation().array().abs().matrix();
243 }
244 
245 bool getSubmapInformation(Index& submapTopLeftIndex,
246  Size& submapBufferSize,
247  Position& submapPosition,
248  Length& submapLength,
249  Index& requestedIndexInSubmap,
250  const Position& requestedSubmapPosition,
251  const Length& requestedSubmapLength,
252  const Length& mapLength,
253  const Position& mapPosition,
254  const double& resolution,
255  const Size& bufferSize,
256  const Index& bufferStartIndex)
257 {
258  // (Top left / bottom right corresponds to the position in the matrix, not the map frame)
259  Eigen::Matrix2d transform = getMapFrameToBufferOrderTransformation().cast<double>();
260 
261  // Corners of submap.
262  Position topLeftPosition = requestedSubmapPosition - transform * 0.5 * requestedSubmapLength.matrix();
263  boundPositionToRange(topLeftPosition, mapLength, mapPosition);
264  if(!getIndexFromPosition(submapTopLeftIndex, topLeftPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false;
265  Index topLeftIndex;
266  topLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex);
267 
268  Position bottomRightPosition = requestedSubmapPosition + transform * 0.5 * requestedSubmapLength.matrix();
269  boundPositionToRange(bottomRightPosition, mapLength, mapPosition);
270  Index bottomRightIndex;
271  if(!getIndexFromPosition(bottomRightIndex, bottomRightPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false;
272  bottomRightIndex = getIndexFromBufferIndex(bottomRightIndex, bufferSize, bufferStartIndex);
273 
274  // Get the position of the top left corner of the generated submap.
275  Position topLeftCorner;
276  if(!getPositionFromIndex(topLeftCorner, submapTopLeftIndex, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false;
277  topLeftCorner -= transform * Position::Constant(0.5 * resolution);
278 
279  // Size of submap.
280  submapBufferSize = bottomRightIndex - topLeftIndex + Index::Ones();
281 
282  // Length of the submap.
283  submapLength = submapBufferSize.cast<double>() * resolution;
284 
285  // Position of submap.
286  Vector vectorToSubmapOrigin;
287  getVectorToOrigin(vectorToSubmapOrigin, submapLength);
288  submapPosition = topLeftCorner - vectorToSubmapOrigin;
289 
290  // Get the index of the cell which corresponds the requested
291  // position of the submap.
292  if(!getIndexFromPosition(requestedIndexInSubmap, requestedSubmapPosition, submapLength, submapPosition, resolution, submapBufferSize)) return false;
293 
294  return true;
295 }
296 
297 Size getSubmapSizeFromCornerIndeces(const Index& topLeftIndex, const Index& bottomRightIndex,
298  const Size& bufferSize, const Index& bufferStartIndex)
299 {
300  const Index unwrappedTopLeftIndex = getIndexFromBufferIndex(topLeftIndex, bufferSize, bufferStartIndex);
301  const Index unwrappedBottomRightIndex = getIndexFromBufferIndex(bottomRightIndex, bufferSize, bufferStartIndex);
302  return Size(unwrappedBottomRightIndex - unwrappedTopLeftIndex + Size::Ones());
303 }
304 
305 bool getBufferRegionsForSubmap(std::vector<BufferRegion>& submapBufferRegions,
306  const Index& submapIndex,
307  const Size& submapBufferSize,
308  const Size& bufferSize,
309  const Index& bufferStartIndex)
310 {
311  if ((getIndexFromBufferIndex(submapIndex, bufferSize, bufferStartIndex) + submapBufferSize > bufferSize).any()) return false;
312 
313  submapBufferRegions.clear();
314 
315  Index bottomRightIndex = submapIndex + submapBufferSize - Index::Ones();
316  wrapIndexToRange(bottomRightIndex, bufferSize);
317 
318  BufferRegion::Quadrant quadrantOfTopLeft = getQuadrant(submapIndex, bufferStartIndex);
319  BufferRegion::Quadrant quadrantOfBottomRight = getQuadrant(bottomRightIndex, bufferStartIndex);
320 
321  if (quadrantOfTopLeft == BufferRegion::Quadrant::TopLeft) {
322 
323  if (quadrantOfBottomRight == BufferRegion::Quadrant::TopLeft) {
324  submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopLeft));
325  return true;
326  }
327 
328  if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) {
329  Size topLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1));
330  submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
331 
332  Index topRightIndex(submapIndex(0), 0);
333  Size topRightSize(submapBufferSize(0), submapBufferSize(1) - topLeftSize(1));
334  submapBufferRegions.push_back(BufferRegion(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight));
335  return true;
336  }
337 
338  if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) {
339  Size topLeftSize(bufferSize(0) - submapIndex(0), submapBufferSize(1));
340  submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
341 
342  Index bottomLeftIndex(0, submapIndex(1));
343  Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), submapBufferSize(1));
344  submapBufferRegions.push_back(BufferRegion(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft));
345  return true;
346  }
347 
348  if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
349  Size topLeftSize(bufferSize(0) - submapIndex(0), bufferSize(1) - submapIndex(1));
350  submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
351 
352  Index topRightIndex(submapIndex(0), 0);
353  Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1) - topLeftSize(1));
354  submapBufferRegions.push_back(BufferRegion(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight));
355 
356  Index bottomLeftIndex(0, submapIndex(1));
357  Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), bufferSize(1) - submapIndex(1));
358  submapBufferRegions.push_back(BufferRegion(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft));
359 
360  Index bottomRightIndex = Index::Zero();
361  Size bottomRightSize(bottomLeftSize(0), topRightSize(1));
362  submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight));
363  return true;
364  }
365 
366  } else if (quadrantOfTopLeft == BufferRegion::Quadrant::TopRight) {
367 
368  if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) {
369  submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopRight));
370  return true;
371  }
372 
373  if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
374 
375  Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1));
376  submapBufferRegions.push_back(BufferRegion(submapIndex, topRightSize, BufferRegion::Quadrant::TopRight));
377 
378  Index bottomRightIndex(0, submapIndex(1));
379  Size bottomRightSize(submapBufferSize(0) - topRightSize(0), submapBufferSize(1));
380  submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight));
381  return true;
382  }
383 
384  } else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomLeft) {
385 
386  if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) {
387  submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomLeft));
388  return true;
389  }
390 
391  if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
392  Size bottomLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1));
393  submapBufferRegions.push_back(BufferRegion(submapIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft));
394 
395  Index bottomRightIndex(submapIndex(0), 0);
396  Size bottomRightSize(submapBufferSize(0), submapBufferSize(1) - bottomLeftSize(1));
397  submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight));
398  return true;
399  }
400 
401  } else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomRight) {
402 
403  if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
404  submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomRight));
405  return true;
406  }
407 
408  }
409 
410  return false;
411 }
412 
413 bool incrementIndex(Index& index, const Size& bufferSize, const Index& bufferStartIndex)
414 {
415  Index unwrappedIndex = getIndexFromBufferIndex(index, bufferSize, bufferStartIndex);
416 
417  // Increment index.
418  if (unwrappedIndex(1) + 1 < bufferSize(1)) {
419  // Same row.
420  unwrappedIndex[1]++;
421  } else {
422  // Next row.
423  unwrappedIndex[0]++;
424  unwrappedIndex[1] = 0;
425  }
426 
427  // End of iterations reached.
428  if (!checkIfIndexInRange(unwrappedIndex, bufferSize)) return false;
429 
430  // Return true iterated index.
431  index = getBufferIndexFromIndex(unwrappedIndex, bufferSize, bufferStartIndex);
432  return true;
433 }
434 
435 bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& submapTopLeftIndex,
436  const Size& submapBufferSize, const Size& bufferSize,
437  const Index& bufferStartIndex)
438 {
439  // Copy the data first, only copy it back if everything is within range.
440  Index tempIndex = index;
441  Index tempSubmapIndex = submapIndex;
442 
443  // Increment submap index.
444  if (tempSubmapIndex[1] + 1 < submapBufferSize[1]) {
445  // Same row.
446  tempSubmapIndex[1]++;
447  } else {
448  // Next row.
449  tempSubmapIndex[0]++;
450  tempSubmapIndex[1] = 0;
451  }
452 
453  // End of iterations reached.
454  if (!checkIfIndexInRange(tempSubmapIndex, submapBufferSize)) return false;
455 
456  // Get corresponding index in map.
457  Index unwrappedSubmapTopLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex);
458  tempIndex = getBufferIndexFromIndex(unwrappedSubmapTopLeftIndex + tempSubmapIndex, bufferSize, bufferStartIndex);
459 
460  // Copy data back.
461  index = tempIndex;
462  submapIndex = tempSubmapIndex;
463  return true;
464 }
465 
466 Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize, const Index& bufferStartIndex)
467 {
468  if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) return bufferIndex;
469 
470  Index index = bufferIndex - bufferStartIndex;
471  wrapIndexToRange(index, bufferSize);
472  return index;
473 }
474 
475 Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize, const Index& bufferStartIndex)
476 {
477  if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) return index;
478 
479  Index bufferIndex = index + bufferStartIndex;
480  wrapIndexToRange(bufferIndex, bufferSize);
481  return bufferIndex;
482 }
483 
484 size_t getLinearIndexFromIndex(const Index& index, const Size& bufferSize, const bool rowMajor)
485 {
486  if (!rowMajor) return index(1) * bufferSize(0) + index(0);
487  return index(0) * bufferSize(1) + index(1);
488 }
489 
490 Index getIndexFromLinearIndex(const size_t linearIndex, const Size& bufferSize, const bool rowMajor)
491 {
492  if (!rowMajor) return Index((int)linearIndex % bufferSize(0), (int)linearIndex / bufferSize(0));
493  return Index((int)linearIndex / bufferSize(1), (int)linearIndex % bufferSize(1));
494 }
495 
496 void getIndicesForRegion(const Index& regionIndex, const Size& regionSize,
497  std::vector<Index> indices)
498 {
499 // for (int i = line.index_; col < line.endIndex(); col++) {
500 // for (int i = 0; i < getSize()(0); i++) {
501 //
502 // }
503 // }
504 }
505 
506 void getIndicesForRegions(const std::vector<Index>& regionIndeces, const Size& regionSizes,
507  std::vector<Index> indices)
508 {
509 }
510 
511 bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3i& colorVector)
512 {
513  colorVector(0) = (colorValue >> 16) & 0x0000ff;
514  colorVector(1) = (colorValue >> 8) & 0x0000ff;
515  colorVector(2) = colorValue & 0x0000ff;
516  return true;
517 }
518 
519 bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3f& colorVector)
520 {
521  Eigen::Vector3i tempColorVector;
522  colorValueToVector(colorValue, tempColorVector);
523  colorVector = ((tempColorVector.cast<float>()).array() / 255.0).matrix();
524  return true;
525 }
526 
527 bool colorValueToVector(const float& colorValue, Eigen::Vector3f& colorVector)
528 {
529  // cppcheck-suppress invalidPointerCast
530  const unsigned long tempColorValue = *reinterpret_cast<const unsigned long*>(&colorValue);
531  colorValueToVector(tempColorValue, colorVector);
532  return true;
533 }
534 
535 bool colorVectorToValue(const Eigen::Vector3i& colorVector, unsigned long& colorValue)
536 {
537  colorValue = ((int)colorVector(0)) << 16 | ((int)colorVector(1)) << 8 | ((int)colorVector(2));
538  return true;
539 }
540 
541 void colorVectorToValue(const Eigen::Vector3i& colorVector, float& colorValue)
542 {
543  unsigned long color = (colorVector(0) << 16) + (colorVector(1) << 8) + colorVector(2);
544  // cppcheck-suppress invalidPointerCast
545  colorValue = *reinterpret_cast<float*>(&color);
546 }
547 
548 void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue)
549 {
550  Eigen::Vector3i tempColorVector = (colorVector * 255.0).cast<int>();
551  colorVectorToValue(tempColorVector, colorValue);
552 }
553 
554 } // namespace
555 
grid_map::incrementIndexForSubmap
bool incrementIndexForSubmap(Index &submapIndex, Index &index, const Index &submapTopLeftIndex, const Size &submapBufferSize, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Definition: GridMapMath.cpp:435
grid_map::Index
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
grid_map::Vector
Eigen::Vector2d Vector
Definition: TypeDefs.hpp:19
grid_map::wrapIndexToRange
void wrapIndexToRange(Index &index, const Size &bufferSize)
Definition: GridMapMath.cpp:202
grid_map::getBufferRegionsForSubmap
bool getBufferRegionsForSubmap(std::vector< BufferRegion > &submapBufferRegions, const Index &submapIndex, const Size &submapBufferSize, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Definition: GridMapMath.cpp:305
grid_map::getIndexFromBufferIndex
Index getIndexFromBufferIndex(const Index &bufferIndex, const Size &bufferSize, const Index &bufferStartIndex)
Definition: GridMapMath.cpp:466
grid_map::Size
Eigen::Array2i Size
Definition: TypeDefs.hpp:23
grid_map::getSubmapInformation
bool getSubmapInformation(Index &submapTopLeftIndex, Size &submapBufferSize, Position &submapPosition, Length &submapLength, Index &requestedIndexInSubmap, const Position &requestedSubmapPosition, const Length &requestedSubmapLength, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Definition: GridMapMath.cpp:245
grid_map::getIndicesForRegions
void getIndicesForRegions(const std::vector< Index > &regionIndeces, const Size &regionSizes, std::vector< Index > indices)
Definition: GridMapMath.cpp:506
grid_map::BufferRegion::Quadrant
Quadrant
Definition: BufferRegion.hpp:26
grid_map::internal::getIndexFromIndexVector
Index getIndexFromIndexVector(const Vector &indexVector, const Size &bufferSize, const Index &bufferStartIndex)
Definition: GridMapMath.cpp:80
grid_map::getIndexFromPosition
bool getIndexFromPosition(Index &index, const Position &position, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Definition: GridMapMath.cpp:117
grid_map
Definition: BufferRegion.hpp:13
grid_map::internal::getBufferOrderToMapFrameTransformation
Eigen::Matrix2i getBufferOrderToMapFrameTransformation()
Definition: GridMapMath.cpp:55
grid_map::checkIfPositionWithinMap
bool checkIfPositionWithinMap(const Position &position, const Length &mapLength, const Position &mapPosition)
Definition: GridMapMath.cpp:133
grid_map::getPositionShiftFromIndexShift
bool getPositionShiftFromIndexShift(Vector &positionShift, const Index &indexShift, const double &resolution)
Definition: GridMapMath.cpp:172
grid_map::Position
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
grid_map::colorVectorToValue
bool colorVectorToValue(const Eigen::Vector3i &colorVector, unsigned long &colorValue)
Definition: GridMapMath.cpp:535
grid_map::incrementIndex
bool incrementIndex(Index &index, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Definition: GridMapMath.cpp:413
grid_map::getSubmapSizeFromCornerIndeces
Size getSubmapSizeFromCornerIndeces(const Index &topLeftIndex, const Index &bottomRightIndex, const Size &bufferSize, const Index &bufferStartIndex)
Definition: GridMapMath.cpp:297
grid_map::getBufferOrderToMapFrameAlignment
const Eigen::Matrix2i getBufferOrderToMapFrameAlignment()
Definition: GridMapMath.cpp:240
grid_map::internal::getVectorToOrigin
bool getVectorToOrigin(Vector &vectorToOrigin, const Length &mapLength)
Definition: GridMapMath.cpp:30
grid_map::getPositionOfDataStructureOrigin
void getPositionOfDataStructureOrigin(const Position &position, const Length &mapLength, Position &positionOfOrigin)
Definition: GridMapMath.cpp:148
grid_map::getIndexFromLinearIndex
Index getIndexFromLinearIndex(const size_t linearIndex, const Size &bufferSize, const bool rowMajor=false)
Definition: GridMapMath.cpp:490
grid_map::internal::getQuadrant
BufferRegion::Quadrant getQuadrant(const Index &index, const Index &bufferStartIndex)
Definition: GridMapMath.cpp:89
grid_map::boundIndexToRange
void boundIndexToRange(Index &index, const Size &bufferSize)
Definition: GridMapMath.cpp:189
grid_map::checkIfIndexInRange
bool checkIfIndexInRange(const Index &index, const Size &bufferSize)
Definition: GridMapMath.cpp:180
grid_map::internal::getIndexVectorFromIndex
Vector getIndexVectorFromIndex(const Index &index, const Size &bufferSize, const Index &bufferStartIndex)
Definition: GridMapMath.cpp:70
grid_map::getLinearIndexFromIndex
size_t getLinearIndexFromIndex(const Index &index, const Size &bufferSize, const bool rowMajor=false)
Definition: GridMapMath.cpp:484
grid_map::getPositionFromIndex
bool getPositionFromIndex(Position &position, const Index &index, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Definition: GridMapMath.cpp:102
grid_map::getIndicesForRegion
void getIndicesForRegion(const Index &regionIndex, const Size &regionSize, std::vector< Index > indices)
Definition: GridMapMath.cpp:496
grid_map::getBufferIndexFromIndex
Index getBufferIndexFromIndex(const Index &index, const Size &bufferSize, const Index &bufferStartIndex)
Definition: GridMapMath.cpp:475
grid_map::internal::getVectorToFirstCell
bool getVectorToFirstCell(Vector &vectorToFirstCell, const Length &mapLength, const double &resolution)
Definition: GridMapMath.cpp:44
grid_map::boundPositionToRange
void boundPositionToRange(Position &position, const Length &mapLength, const Position &mapPosition)
Definition: GridMapMath.cpp:215
grid_map::colorValueToVector
bool colorValueToVector(const unsigned long &colorValue, Eigen::Vector3i &colorVector)
Definition: GridMapMath.cpp:511
grid_map::getIndexShiftFromPositionShift
bool getIndexShiftFromPositionShift(Index &indexShift, const Vector &positionShift, const double &resolution)
Definition: GridMapMath.cpp:157
grid_map::internal::getMapFrameToBufferOrderTransformation
Eigen::Matrix2i getMapFrameToBufferOrderTransformation()
Definition: GridMapMath.cpp:60
grid_map::BufferRegion
Definition: BufferRegion.hpp:19
grid_map::internal::checkIfStartIndexAtDefaultPosition
bool checkIfStartIndexAtDefaultPosition(const Index &bufferStartIndex)
Definition: GridMapMath.cpp:65
grid_map::Length
Eigen::Array2d Length
Definition: TypeDefs.hpp:24

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