text: //=================================================================================================
  // 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>
  
  
  /** @class MapDimensionProperties
   *  @brief
   *  @member topLeftOffset   di tu zuo shang jia wei zhi
   *  @member mapDimensions
   *  @member mapLimitsf
   *  @member cellLength
   * */
  
  class MapDimensionProperties
  {
  public:
      MapDimensionProperties()
              : topLeftOffset(-1.0f, -1.0f), mapDimensions(-1, -1), cellLength(-1.0f)
      {
      }
  
      MapDimensionProperties(const Eigen::Vector2f &topLeftOffsetIn, const Eigen::Vector2i &mapDimensionsIn, float cellLengthIn)
              : topLeftOffset(topLeftOffsetIn), mapDimensions(mapDimensionsIn), cellLength(cellLengthIn)
      {
          mapLimitsf = (mapDimensionsIn.cast<float>()).array() - 1.0f;
      }
  
      bool operator==(const MapDimensionProperties &other) const
      {
          return (topLeftOffset == other.topLeftOffset) && (mapDimensions == other.mapDimensions) && (cellLength == other.cellLength);
      }
  
      bool hasEqualDimensionProperties(const MapDimensionProperties &other) const
      {
          return (mapDimensions == other.mapDimensions);
      }
  
      bool hasEqualTransformationProperties(const MapDimensionProperties &other) const
      {
          return (topLeftOffset == other.topLeftOffset) && (cellLength == other.cellLength);
      }
  
      bool pointOutOfMapBounds(const Eigen::Vector2f &coords) const
      {
          return ((coords[0] < 0.0f) || (coords[0] > mapLimitsf[0]) || (coords[1] < 0.0f) || (coords[1] > mapLimitsf[1]));
      }
  
      void setMapCellDims(const Eigen::Vector2i &newDims)
      {
          mapDimensions = newDims;
          mapLimitsf = (newDims.cast<float>()).array() - 2.0f;
      }
  
      void setTopLeftOffset(const Eigen::Vector2f &topLeftOffsetIn)
      {
          topLeftOffset = topLeftOffsetIn;
      }
  
      void setSizeX(int sX) { mapDimensions[0] = sX; };
      void setSizeY(int sY) { mapDimensions[1] = sY; };
      void setCellLength(float cl) { cellLength = cl; };
  
      const Eigen::Vector2f &getTopLeftOffset() const { return topLeftOffset; };
      const Eigen::Vector2i &getMapDimensions() const { return mapDimensions; };
      int getSizeX() const { return mapDimensions[0]; };
      int getSizeY() const { return mapDimensions[1]; };
      float getCellLength() const { return cellLength; };
  
  protected:
      Eigen::Vector2f topLeftOffset;
      Eigen::Vector2i mapDimensions;
      Eigen::Vector2f mapLimitsf;
      float cellLength;
  
  };// class MapDimensionProperties
  
  namespace hectorslam
  {
  
  /**
   * GridMapBase provides basic grid map functionality (creates grid , provides transformation from/to world coordinates).
   * It serves as the base class for different map representations that may extend it's functionality.
   */
  template <typename ConcreteCellType>
  class GridMapBase
  {
  
  public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  
      /**
     * Constructor, creates grid representation and transformations.
     */
      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();
      }
  
      /**
     * Destructor
     */
      virtual ~GridMapBase()
      {
          deleteArray();
      }
  
      /**
     * Indicates if given x and y are within map bounds
     * @return True if coordinates are within map bounds
     */
      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();
      }
  
      /**
     * Resets the grid cell values by using the resetGridCell() function.
     */
      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; };
  
      /**
     * Allocates memory for the two dimensional pointer array for map representation.
     */
      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();
          }
      }
  
      /**
     * Copy Constructor, only needed if pointer members are present.
     */
      GridMapBase(const GridMapBase &other)
      {
          allocateArray(other.getMapDimensions());
          *this = other;
      }
  
      /**
     * Assignment operator, only needed if pointer members are present.
     */
      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;
      }
  
      /**
     * Returns the world coordinates for the given map coords.
     */
      inline Eigen::Vector2f getWorldCoords(const Eigen::Vector2f &mapCoords) const
      {
          return worldTmap * mapCoords;
      }
  
      /**
     * Returns the map coordinates for the given world coords.
     */
      inline Eigen::Vector2f getMapCoords(const Eigen::Vector2f &worldCoords) const
      {
          return mapTworld * worldCoords;
      }
  
      /**
     * Returns the world pose for the given map pose.
     */
      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]);
      }
  
      /**
     * Returns the map pose for the given world pose.
     */
      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());
          }
      }
  
      /**
     * Set the map transformations
     * @param xWorld The origin of the map coordinate system on the x axis in world coordinates
     * @param yWorld The origin of the map coordinate system on the y axis in world coordinates
     * @param The cell length of the grid map
     */
      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();
      }
  
      /**
     * Returns the scale factor for one unit in world coords to one unit in map coords.
     * @return The scale factor
     */
      float getScaleToMap() const
      {
          return scaleToMap;
      }
  
      /**
     * Returns the cell edge length of grid cells in millimeters.
     * @return the cell edge length in millimeters.
     */
      float getCellLength() const
      {
          return mapDimensionProperties.getCellLength();
      }
  
      /**
     * Returns a reference to the homogenous 2D transform from map to world coordinates.
     * @return The homogenous 2D transform.
     */
      const Eigen::Affine2f &getWorldTmap() const
      {
          return worldTmap;
      }
  
      /**
     * Returns a reference to the homogenous 3D transform from map to world coordinates.
     * @return The homogenous 3D transform.
     */
      const Eigen::Affine3f &getWorldTmap3D() const
      {
          return worldTmap3D;
      }
  
      /**
     * Returns a reference to the homogenous 2D transform from world to map coordinates.
     * @return The homogenous 2D transform.
     */
      const Eigen::Affine2f &getMapTworld() const
      {
          return mapTworld;
      }
  
      void setUpdated() { lastUpdateIndex++; };
      int getUpdateIndex() const { return lastUpdateIndex; };
  
      /**
      * Returns the rectangle ([xMin,yMin],[xMax,xMax]) containing non-default cell values
      */
      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; ///< Map representation used with plain pointer array.
  
      float scaleToMap; ///< Scaling factor from world to map.
  
      Eigen::Affine2f worldTmap;   ///< Homogenous 2D transform from map to world coordinates.
      Eigen::Affine3f worldTmap3D; ///< Homogenous 3D transform from map to world coordinates.
      Eigen::Affine2f mapTworld;   ///< Homogenous 2D transform from world to map coordinates.
  
      MapDimensionProperties mapDimensionProperties;
      int sizeX;
  
  private:
      int lastUpdateIndex;
  
  };// class GridMapBase
  
  }// namespace hectorslam
  
  #endif// GRIDMAPBASE_H_
  