GridMapMath.hpp
Go to the documentation of this file.
1 /*
2  * GridMapMath.hpp
3  *
4  * Created on: Dec 2, 2013
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
9 #pragma once
10 
13 
14 #include <Eigen/Core>
15 #include <vector>
16 #include <map>
17 
18 namespace grid_map {
19 
20 union Color
21 {
22  unsigned long longColor_;
23  float floatColor_;
24 };
25 
37 bool getPositionFromIndex(Position& position,
38  const Index& index,
39  const Length& mapLength,
40  const Position& mapPosition,
41  const double& resolution,
42  const Size& bufferSize,
43  const Index& bufferStartIndex = Index::Zero());
44 
56 bool getIndexFromPosition(Index& index,
57  const Position& position,
58  const Length& mapLength,
59  const Position& mapPosition,
60  const double& resolution,
61  const Size& bufferSize,
62  const Index& bufferStartIndex = Index::Zero());
63 
71 bool checkIfPositionWithinMap(const Position& position,
72  const Length& mapLength,
73  const Position& mapPosition);
74 
81 void getPositionOfDataStructureOrigin(const Position& position,
82  const Length& mapLength,
83  Position& positionOfOrigin);
84 
94 bool getIndexShiftFromPositionShift(Index& indexShift,
95  const Vector& positionShift,
96  const double& resolution);
97 
107 bool getPositionShiftFromIndexShift(Vector& positionShift,
108  const Index& indexShift,
109  const double& resolution);
110 
117 bool checkIfIndexInRange(const Index& index, const Size& bufferSize);
118 
126 void boundIndexToRange(Index& index, const Size& bufferSize);
127 
134 void boundIndexToRange(int& index, const int& bufferSize);
135 
143 void wrapIndexToRange(Index& index, const Size& bufferSize);
144 
151 void wrapIndexToRange(int& index, int bufferSize);
152 
160 void boundPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition);
161 
167 Eigen::Matrix2i getBufferOrderToMapFrameAlignment();
168 
188 bool getSubmapInformation(Index& submapTopLeftIndex,
189  Size& submapBufferSize,
190  Position& submapPosition,
191  Length& submapLength,
192  Index& requestedIndexInSubmap,
193  const Position& requestedSubmapPosition,
194  const Length& requestedSubmapLength,
195  const Length& mapLength,
196  const Position& mapPosition,
197  const double& resolution,
198  const Size& bufferSize,
199  const Index& bufferStartIndex = Index::Zero());
200 
207 Size getSubmapSizeFromCornerIndices(const Index& topLeftIndex, const Index& bottomRightIndex,
208  const Size& bufferSize, const Index& bufferStartIndex);
209 
220 bool getBufferRegionsForSubmap(std::vector<BufferRegion>& submapBufferRegions,
221  const Index& submapIndex,
222  const Size& submapBufferSize,
223  const Size& bufferSize,
224  const Index& bufferStartIndex = Index::Zero());
225 
235 bool incrementIndex(Index& index, const Size& bufferSize,
236  const Index& bufferStartIndex = Index::Zero());
237 
254 bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& submapTopLeftIndex,
255  const Size& submapBufferSize, const Size& bufferSize,
256  const Index& bufferStartIndex = Index::Zero());
257 
266 Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize,
267  const Index& bufferStartIndex);
268 
276 Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize, const Index& bufferStartIndex);
277 
287 size_t getLinearIndexFromIndex(const Index& index, const Size& bufferSize, bool rowMajor = false);
288 
296 Index getIndexFromLinearIndex(size_t linearIndex, const Size& bufferSize, bool rowMajor = false);
297 
304 bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3i& colorVector);
305 
312 bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3f& colorVector);
313 
320 bool colorValueToVector(const float& colorValue, Eigen::Vector3f& colorVector);
321 
328 bool colorVectorToValue(const Eigen::Vector3i& colorVector, unsigned long& colorValue);
329 
335 void colorVectorToValue(const Eigen::Vector3i& colorVector, float& colorValue);
336 
342 void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue);
343 
344 } // namespace grid_map
bool incrementIndexForSubmap(Index &submapIndex, Index &index, const Index &submapTopLeftIndex, const Size &submapBufferSize, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
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)
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())
size_t getLinearIndexFromIndex(const Index &index, const Size &bufferSize, bool rowMajor=false)
bool getIndexFromPosition(Index &index, const Position &position, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Size getSubmapSizeFromCornerIndices(const Index &topLeftIndex, const Index &bottomRightIndex, const Size &bufferSize, const Index &bufferStartIndex)
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())
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
Eigen::Array2i Size
Definition: TypeDefs.hpp:23
unsigned long longColor_
Definition: GridMapMath.hpp:22
void getPositionOfDataStructureOrigin(const Position &position, const Length &mapLength, Position &positionOfOrigin)
Eigen::Array2d Length
Definition: TypeDefs.hpp:24
void boundIndexToRange(Index &index, const Size &bufferSize)
bool checkIfIndexInRange(const Index &index, const Size &bufferSize)
bool getPositionFromIndex(Position &position, const Index &index, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Index getBufferIndexFromIndex(const Index &index, const Size &bufferSize, const Index &bufferStartIndex)
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 getBufferOrderToMapFrameAlignment()
Index getIndexFromLinearIndex(size_t linearIndex, const Size &bufferSize, bool rowMajor=false)
Eigen::Vector2d Vector
Definition: TypeDefs.hpp:19


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Wed Jul 5 2023 02:23:35