00001
00002
00003
00004
00005
00006
00007
00008
00009 #pragma once
00010
00011 #include "grid_map_core/TypeDefs.hpp"
00012 #include "grid_map_core/BufferRegion.hpp"
00013
00014 #include <Eigen/Core>
00015 #include <vector>
00016 #include <map>
00017
00018 namespace grid_map {
00019
00031 bool getPositionFromIndex(Position& position,
00032 const Index& index,
00033 const Length& mapLength,
00034 const Position& mapPosition,
00035 const double& resolution,
00036 const Size& bufferSize,
00037 const Index& bufferStartIndex = Index::Zero());
00038
00050 bool getIndexFromPosition(Index& index,
00051 const Position& position,
00052 const Length& mapLength,
00053 const Position& mapPosition,
00054 const double& resolution,
00055 const Size& bufferSize,
00056 const Index& bufferStartIndex = Index::Zero());
00057
00065 bool checkIfPositionWithinMap(const Position& position,
00066 const Length& mapLength,
00067 const Position& mapPosition);
00068
00075 void getPositionOfDataStructureOrigin(const Position& position,
00076 const Length& mapLength,
00077 Position& positionOfOrigin);
00078
00088 bool getIndexShiftFromPositionShift(Index& indexShift,
00089 const Vector& positionShift,
00090 const double& resolution);
00091
00101 bool getPositionShiftFromIndexShift(Vector& positionShift,
00102 const Index& indexShift,
00103 const double& resolution);
00104
00111 bool checkIfIndexInRange(const Index& index, const Size& bufferSize);
00112
00120 void boundIndexToRange(Index& index, const Size& bufferSize);
00121
00128 void boundIndexToRange(int& index, const int& bufferSize);
00129
00137 void wrapIndexToRange(Index& index, const Size& bufferSize);
00138
00145 void wrapIndexToRange(int& index, const int& bufferSize);
00146
00154 void boundPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition);
00155
00161 const Eigen::Matrix2i getBufferOrderToMapFrameAlignment();
00162
00182 bool getSubmapInformation(Index& submapTopLeftIndex,
00183 Size& submapBufferSize,
00184 Position& submapPosition,
00185 Length& submapLength,
00186 Index& requestedIndexInSubmap,
00187 const Position& requestedSubmapPosition,
00188 const Length& requestedSubmapLength,
00189 const Length& mapLength,
00190 const Position& mapPosition,
00191 const double& resolution,
00192 const Size& bufferSize,
00193 const Index& bufferStartIndex = Index::Zero());
00194
00201 Size getSubmapSizeFromCornerIndeces(const Index& topLeftIndex, const Index& bottomRightIndex,
00202 const Size& bufferSize, const Index& bufferStartIndex);
00203
00214 bool getBufferRegionsForSubmap(std::vector<BufferRegion>& submapBufferRegions,
00215 const Index& submapIndex,
00216 const Size& submapBufferSize,
00217 const Size& bufferSize,
00218 const Index& bufferStartIndex = Index::Zero());
00219
00229 bool incrementIndex(Index& index, const Size& bufferSize,
00230 const Index& bufferStartIndex = Index::Zero());
00231
00248 bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& submapTopLeftIndex,
00249 const Size& submapBufferSize, const Size& bufferSize,
00250 const Index& bufferStartIndex = Index::Zero());
00251
00260 Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize,
00261 const Index& bufferStartIndex);
00262
00270 Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize, const Index& bufferStartIndex);
00271
00281 size_t getLinearIndexFromIndex(const Index& index, const Size& bufferSize, const bool rowMajor = false);
00282
00290 Index getIndexFromLinearIndex(const size_t linearIndex, const Size& bufferSize, const bool rowMajor = false);
00291
00298 void getIndicesForRegion(const Index& regionIndex, const Size& regionSize,
00299 std::vector<Index> indices);
00300
00308 void getIndicesForRegions(const std::vector<Index>& regionIndeces, const Size& regionSizes,
00309 std::vector<Index> indices);
00310
00317 bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3i& colorVector);
00318
00325 bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3f& colorVector);
00326
00333 bool colorValueToVector(const float& colorValue, Eigen::Vector3f& colorVector);
00334
00341 bool colorVectorToValue(const Eigen::Vector3i& colorVector, unsigned long& colorValue);
00342
00348 void colorVectorToValue(const Eigen::Vector3i& colorVector, float& colorValue);
00349
00355 void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue);
00356
00357 }