GridMapMath.hpp
Go to the documentation of this file.
00001 /*
00002  * GridMapMath.hpp
00003  *
00004  *  Created on: Dec 2, 2013
00005  *      Author: Péter Fankhauser
00006  *       Institute: ETH Zurich, ANYbotics
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 } // namespace


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jul 9 2019 05:06:13