GridMapBase.h

Go to the documentation of this file.

//=================================================================================================
// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Simulation, Systems Optimization and Robotics
//       group, TU Darmstadt nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================

#ifndef __GridMapBase_h_
#define __GridMapBase_h_

#include <Eigen/Geometry>
#include <Eigen/LU>

#include "MapDimensionProperties.h"

namespace hectorslam {

template<typename ConcreteCellType>
class GridMapBase
{

public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  bool hasGridValue(int x, int y) const
  {
    return (x >= 0) && (y >= 0) && (x < this->getSizeX()) && (y < this->getSizeY());
  }

  const Eigen::Vector2i& getMapDimensions() const { return mapDimensionProperties.getMapDimensions(); };
  int getSizeX() const { return mapDimensionProperties.getSizeX(); };
  int getSizeY() const { return mapDimensionProperties.getSizeY(); };

  bool pointOutOfMapBounds(const Eigen::Vector2f& pointMapCoords) const
  {
    return mapDimensionProperties.pointOutOfMapBounds(pointMapCoords);
  }

  virtual void reset()
  {
    this->clear();
  }

  void clear()
  {
    int size = this->getSizeX() * this->getSizeY();

    for (int i = 0; i < size; ++i) {
      this->mapArray[i].resetGridCell();
    }

    //this->mapArray[0].set(1.0f);
    //this->mapArray[size-1].set(1.0f);
  }


  const MapDimensionProperties& getMapDimProperties() const { return mapDimensionProperties; };

  GridMapBase(float mapResolution, const Eigen::Vector2i& size, const Eigen::Vector2f& offset)
    : mapArray(0)
    , lastUpdateIndex(-1)
  {
    Eigen::Vector2i newMapDimensions (size);

    this->setMapGridSize(newMapDimensions);
    sizeX = size[0];

    setMapTransformation(offset, mapResolution);

    this->clear();
  }

  virtual ~GridMapBase()
  {
    deleteArray();
  }

  void allocateArray(const Eigen::Vector2i& newMapDims)
  {
    int sizeX = newMapDims.x();
    int sizeY = newMapDims.y();

    mapArray = new ConcreteCellType [sizeX*sizeY];

    mapDimensionProperties.setMapCellDims(newMapDims);
  }

  void deleteArray()
  {
    if (mapArray != 0){

      delete[] mapArray;

      mapArray = 0;
      mapDimensionProperties.setMapCellDims(Eigen::Vector2i(-1,-1));
    }
  }

  ConcreteCellType& getCell(int x, int y)
  {
    return mapArray[y * sizeX + x];
  }

  const ConcreteCellType& getCell(int x, int y) const
  {
    return mapArray[y * sizeX + x];
  }

  ConcreteCellType& getCell(int index)
  {
    return mapArray[index];
  }

  const ConcreteCellType& getCell(int index) const
  {
    return mapArray[index];
  }

  void setMapGridSize(const Eigen::Vector2i& newMapDims)
  {
    if (newMapDims != mapDimensionProperties.getMapDimensions() ){
     deleteArray();
     allocateArray(newMapDims);
     this->reset();
    }
  }

  GridMapBase(const GridMapBase& other)
  {
    allocateArray(other.getMapDimensions());
    *this = other;
  }

  GridMapBase& operator=(const GridMapBase& other)
  {
    if ( !(this->mapDimensionProperties == other.mapDimensionProperties)){
      this->setMapGridSize(other.mapDimensionProperties.getMapDimensions());
    }

    this->mapDimensionProperties = other.mapDimensionProperties;

    this->worldTmap = other.worldTmap;
    this->mapTworld = other.mapTworld;
    this->worldTmap3D = other.worldTmap3D;

    this->scaleToMap = other.scaleToMap;

    //@todo potential resize
    int sizeX = this->getSizeX();
    int sizeY = this->getSizeY();

    size_t concreteCellSize = sizeof(ConcreteCellType);

    memcpy(this->mapArray, other.mapArray, sizeX*sizeY*concreteCellSize);

    return *this;
  }

  inline Eigen::Vector2f getWorldCoords(const Eigen::Vector2f& mapCoords) const
  {
    return worldTmap * mapCoords;
  }

  inline Eigen::Vector2f getMapCoords(const Eigen::Vector2f& worldCoords) const
  {
    return mapTworld * worldCoords;
  }

  inline Eigen::Vector3f getWorldCoordsPose(const Eigen::Vector3f& mapPose) const
  {
    Eigen::Vector2f worldCoords (worldTmap * mapPose.head<2>());
    return Eigen::Vector3f(worldCoords[0], worldCoords[1], mapPose[2]);
  }

  inline Eigen::Vector3f getMapCoordsPose(const Eigen::Vector3f& worldPose) const
  {
    Eigen::Vector2f mapCoords (mapTworld * worldPose.head<2>());
    return Eigen::Vector3f(mapCoords[0], mapCoords[1], worldPose[2]);
  }

  void setDimensionProperties(const Eigen::Vector2f& topLeftOffsetIn, const Eigen::Vector2i& mapDimensionsIn, float cellLengthIn)
  {
    setDimensionProperties(MapDimensionProperties(topLeftOffsetIn,mapDimensionsIn,cellLengthIn));
  }

  void setDimensionProperties(const MapDimensionProperties& newMapDimProps)
  {
    //Grid map cell number has changed
    if (!newMapDimProps.hasEqualDimensionProperties(this->mapDimensionProperties)){
      this->setMapGridSize(newMapDimProps.getMapDimensions());
    }

    //Grid map transformation/cell size has changed
    if(!newMapDimProps.hasEqualTransformationProperties(this->mapDimensionProperties)){
      this->setMapTransformation(newMapDimProps.getTopLeftOffset(), newMapDimProps.getCellLength());
    }
  }

  void setMapTransformation(const Eigen::Vector2f& topLeftOffset, float cellLength)
  {
    mapDimensionProperties.setCellLength(cellLength);
    mapDimensionProperties.setTopLeftOffset(topLeftOffset);

    scaleToMap = 1.0f / cellLength;

    mapTworld = Eigen::AlignedScaling2f(scaleToMap, scaleToMap) * Eigen::Translation2f(topLeftOffset[0], topLeftOffset[1]);

    worldTmap3D = Eigen::AlignedScaling3f(scaleToMap, scaleToMap, 1.0f) * Eigen::Translation3f(topLeftOffset[0], topLeftOffset[1], 0);

    //std::cout << worldTmap3D.matrix() << std::endl;
    worldTmap3D = worldTmap3D.inverse();

    worldTmap = mapTworld.inverse();
  }


  float getScaleToMap() const
  {
    return scaleToMap;
  }

  float getCellLength() const
  {
    return mapDimensionProperties.getCellLength();
  }

  const Eigen::Affine2f& getWorldTmap() const
  {
    return worldTmap;
  }

  const Eigen::Affine3f& getWorldTmap3D() const
  {
    return worldTmap3D;
  }

  const Eigen::Affine2f& getMapTworld() const
  {
    return mapTworld;
  }

  void setUpdated() { lastUpdateIndex++; };
  int getUpdateIndex() const { return lastUpdateIndex; };

  bool getMapExtends(int& xMax, int& yMax, int& xMin, int& yMin) const
  {
    int lowerStart = -1;
    int upperStart = 10000;

    int xMaxTemp = lowerStart;
    int yMaxTemp = lowerStart;
    int xMinTemp = upperStart;
    int yMinTemp = upperStart;

    int sizeX = this->getSizeX();
    int sizeY = this->getSizeY();

    for (int x = 0; x < sizeX; ++x) {
      for (int y = 0; y < sizeY; ++y) {
        if (this->mapArray[x][y].getValue() != 0.0f) {

          if (x > xMaxTemp) {
            xMaxTemp = x;
          }

          if (x < xMinTemp) {
            xMinTemp = x;
          }

          if (y > yMaxTemp) {
            yMaxTemp = y;
          }

          if (y < yMinTemp) {
            yMinTemp = y;
          }
        }
      }
    }

    if ((xMaxTemp != lowerStart) &&
        (yMaxTemp != lowerStart) &&
        (xMinTemp != upperStart) &&
        (yMinTemp != upperStart)) {

      xMax = xMaxTemp;
      yMax = yMaxTemp;
      xMin = xMinTemp;
      yMin = yMinTemp;
      return true;
    } else {
      return false;
    }
  }

protected:

  ConcreteCellType *mapArray;    

  float scaleToMap;              

  Eigen::Affine2f worldTmap;     
  Eigen::Affine3f worldTmap3D;   
  Eigen::Affine2f mapTworld;     

  MapDimensionProperties mapDimensionProperties;
  int sizeX;

private:
  int lastUpdateIndex;
};

}

#endif