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


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