29 #ifndef __GridMapBase_h_ 30 #define __GridMapBase_h_ 32 #include <Eigen/Geometry> 43 template<
typename ConcreteCellType>
49 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57 return (x >= 0) && (y >= 0) && (x < this->
getSizeX()) && (y < this->
getSizeY());
81 for (
int i = 0; i < size; ++i) {
95 GridMapBase(
float mapResolution,
const Eigen::Vector2i& size,
const Eigen::Vector2f& offset)
99 Eigen::Vector2i newMapDimensions (size);
122 int sizeX = newMapDims.x();
123 int sizeY = newMapDims.y();
125 mapArray =
new ConcreteCellType [sizeX*sizeY];
146 const ConcreteCellType&
getCell(
int x,
int y)
const 156 const ConcreteCellType&
getCell(
int index)
const 200 size_t concreteCellSize =
sizeof(ConcreteCellType);
218 inline Eigen::Vector2f
getMapCoords(
const Eigen::Vector2f& worldCoords)
const 228 Eigen::Vector2f worldCoords (
worldTmap * mapPose.head<2>());
229 return Eigen::Vector3f(worldCoords[0], worldCoords[1], mapPose[2]);
237 Eigen::Vector2f mapCoords (
mapTworld * worldPose.head<2>());
238 return Eigen::Vector3f(mapCoords[0], mapCoords[1], worldPose[2]);
241 void setDimensionProperties(
const Eigen::Vector2f& topLeftOffsetIn,
const Eigen::Vector2i& mapDimensionsIn,
float cellLengthIn)
337 int upperStart = 10000;
339 int xMaxTemp = lowerStart;
340 int yMaxTemp = lowerStart;
341 int xMinTemp = upperStart;
342 int yMinTemp = upperStart;
348 for (
int y = 0;
y < sizeY; ++
y) {
370 if ((xMaxTemp != lowerStart) &&
371 (yMaxTemp != lowerStart) &&
372 (xMinTemp != upperStart) &&
373 (yMinTemp != upperStart)) {
const Eigen::Vector2i & getMapDimensions() const
void setMapCellDims(const Eigen::Vector2i &newDims)
const Eigen::Affine3f & getWorldTmap3D() const
Eigen::Affine2f worldTmap
Homogenous 2D transform from map to world coordinates.
float getScaleToMap() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW bool hasGridValue(int x, int y) const
TFSIMD_FORCE_INLINE const tfScalar & y() const
const ConcreteCellType & getCell(int index) const
void allocateArray(const Eigen::Vector2i &newMapDims)
const MapDimensionProperties & getMapDimProperties() const
void setDimensionProperties(const MapDimensionProperties &newMapDimProps)
GridMapBase(const GridMapBase &other)
bool hasEqualDimensionProperties(const MapDimensionProperties &other) const
int getUpdateIndex() const
ConcreteCellType & getCell(int x, int y)
ConcreteCellType * mapArray
Map representation used with plain pointer array.
GridMapBase & operator=(const GridMapBase &other)
const ConcreteCellType & getCell(int x, int y) const
const Eigen::Vector2i & getMapDimensions() const
Eigen::Affine2f mapTworld
Homogenous 2D transform from world to map coordinates.
TFSIMD_FORCE_INLINE const tfScalar & x() const
bool pointOutOfMapBounds(const Eigen::Vector2f &coords) const
bool getMapExtends(int &xMax, int &yMax, int &xMin, int &yMin) const
Eigen::Vector2f getWorldCoords(const Eigen::Vector2f &mapCoords) const
float getCellLength() const
const Eigen::Vector2f & getTopLeftOffset() const
void setDimensionProperties(const Eigen::Vector2f &topLeftOffsetIn, const Eigen::Vector2i &mapDimensionsIn, float cellLengthIn)
void setMapTransformation(const Eigen::Vector2f &topLeftOffset, float cellLength)
const Eigen::Affine2f & getWorldTmap() const
void setTopLeftOffset(const Eigen::Vector2f &topLeftOffsetIn)
Eigen::Vector3f getMapCoordsPose(const Eigen::Vector3f &worldPose) const
GridMapBase(float mapResolution, const Eigen::Vector2i &size, const Eigen::Vector2f &offset)
float getCellLength() const
const Eigen::Affine2f & getMapTworld() const
bool hasEqualTransformationProperties(const MapDimensionProperties &other) const
ConcreteCellType & getCell(int index)
void setMapGridSize(const Eigen::Vector2i &newMapDims)
Eigen::Vector3f getWorldCoordsPose(const Eigen::Vector3f &mapPose) const
Eigen::Vector2f getMapCoords(const Eigen::Vector2f &worldCoords) const
Eigen::Affine3f worldTmap3D
Homogenous 3D transform from map to world coordinates.
MapDimensionProperties mapDimensionProperties
bool pointOutOfMapBounds(const Eigen::Vector2f &pointMapCoords) const
float scaleToMap
Scaling factor from world to map.
void setCellLength(float cl)