first commit

.gitignore

@@ -0,0 +1,4 @@
+# Bỏ qua thư mục build/
+build/
+
+CODE_REVIEW.md

CHANGELOG.rst

@@ -0,0 +1,124 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package grid_map_core
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.7.0 (2022-03-17)
+------------------
+
+1.6.4 (2020-12-04)
+------------------
+
+1.6.2 (2019-10-14)
+------------------
+* Implements a grid map transformation from one map frame to another map frame given the transform between the frames.
+  Authors:
+  Co-authored-by: fabianje <fabian.jenelten@mavt.ethz.ch>
+* Contributors: fabianje
+
+1.6.1 (2019-02-27)
+------------------
+* Updated host changes.
+* Updated author e-mail address.
+* Contributors: Peter Fankhauser, Péter Fankhauser
+
+1.6.0 (2017-11-24)
+------------------
+* Added new sliding window iterator.
+* Added new `thickenLine()`, triangulation, and bounding box method to polygon.
+* Added unit tests for LineIterator with using move function.
+* Fixed cpp-check warnings and errors.
+* Fixed line iterator for moved maps (`#119 <https://github.com/anybotics/grid_map/issues/119>`_).
+* Fixed error in SpiralIterator when center is outside the map (`#114 <https://github.com/anybotics/grid_map/issues/114>`_).
+* Contributors: Péter Fankhauser, 2scholz, Remo Diethelm, Takahiro Miki, Tanja Baumann
+
+1.5.2 (2017-07-25)
+------------------
+
+1.5.1 (2017-07-25)
+------------------
+
+1.5.0 (2017-07-18)
+------------------
+* Added new function for polygon triangulation.
+* Added Eigen macro for bit-alignment (`#88 <https://github.com/anybotics/grid_map/issues/88>`_).
+* Added default copy constructor and assign operator methods after the rule of five.
+* Fixing return value in `getQuadrant` member function.
+* Fixing buffer handling bug for circular and ellipse iterators.
+* Capture case when both circles are the same in `convexHullOfTwoCircles`.
+* Fixing build error on ROS Kinetic.
+* Contributors: Peter Fankhauser, Sascha, Thomas Emter, Martin Wermelinger
+
+1.4.2 (2017-01-24)
+------------------
+* Added linear interpolation method for data access.
+* Increased efficiency for linear interpolation method.
+* Addressing C++ compiler warnings.
+* Contributors: Dominic Jud, Peter Fankhauser, Horatiu George Todoran
+
+1.4.1 (2016-10-23)
+------------------
+* Improved line iterator with start and end positions.
+* Added method to retrieve submap size for iterators.
+* Improved transformation of images to color grid map layers.
+* Fixing issues with order of include with Eigen (`#67 <https://github.com/anybotics/grid_map/issues/67>`_).
+* Contributors: Peter Fankhauser, Dominic Jud
+
+1.4.0 (2016-08-22)
+------------------
+* Added convenience function to convert a grid map to form with circular buffer at (0,0).
+* Contributors: Peter Fankhauser
+
+1.3.3 (2016-05-10)
+------------------
+* Release for ROS Kinetic.
+* Contributors: Peter Fankhauser
+
+1.3.2 (2016-05-10)
+------------------
+
+1.3.1 (2016-05-10)
+------------------
+* Cleanup up Eigen types as preparation for ROS Kinetic release.
+* Contributors: Peter Fankhauser
+
+1.3.0 (2016-04-26)
+------------------
+* Made the `isInside` checks `const`.
+* Fixes polygon iterator bug when using moved maps.
+* Added unit test for polygon iterator on a moved map.
+* Added comment about size of the returning submap.
+* Reduced test build warning.
+* Contributors: Peter Fankhauser, Martin Wermelinger, Marcus Liebhardt
+
+1.2.0 (2016-03-03)
+------------------
+* Improved efficiency for the Grid Map iterator (speed increase of 10x for large maps) (`#45 <https://github.com/anybotics/grid_map/issues/45>`_).
+* New iterator_benchmark demo to exemplify the usage of the iterators and their computational performance (`#45 <https://github.com/anybotics/grid_map/issues/45>`_).
+* Added new method to set the position of a grid map (`#42 <https://github.com/anybotics/grid_map/pull/42>`_).
+* Added new move_demo to illustrate the difference between the `move` and `setPosition` method.
+* Fixed behavior of checkIfPositionWithinMap() in edge cases (`#41 <https://github.com/anybotics/grid_map/issues/41>`_).
+* Updated documentation for spiral and ellipse iterator, and iterator performance.
+* const correctness for grid's getSubmap.
+* Cleanup of arguments and return types.
+* Contributors: Péter Fankhauser, Christos Zalidis, Daniel Stonier
+
+1.1.3 (2016-01-11)
+------------------
+
+1.1.2 (2016-01-11)
+------------------
+* Should fix errors on build server regarding Eigen3 and visualization_msgs dependencies.
+
+1.1.1 (2016-01-11)
+------------------
+* Changes to CMakeLists.txt to enable compatibility with Ubuntu Saucy.
+
+1.1.0 (2016-01-08)
+-------------------
+* added installation instructions in CMakeLists
+* new ellipse iterator tool
+* general improvements and bugfixes
+
+1.0.0 (2015-11-20)
+-------------------
+* release for Springer ROS Book Chapter

CMakeLists.txt

@@ -0,0 +1,187 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(grid_map_core VERSION 1.0.0 LANGUAGES CXX)
+
+# ========================================================
+# Detect Catkin or Standalone
+# ========================================================
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building grid_map_core with Catkin")
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building grid_map_core with Standalone CMake")
+endif()
+
+# ========================================================
+# C++ Standard
+# ========================================================
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+# ========================================================
+# Compiler warnings
+# ========================================================
+add_compile_options(-Wall -Wextra -Wpedantic)
+
+# ========================================================
+# Eigen
+# ========================================================
+find_package(Eigen3 QUIET)
+if(NOT EIGEN3_FOUND)
+  find_package(PkgConfig REQUIRED)
+  pkg_check_modules(EIGEN3 REQUIRED eigen3)
+  set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS})
+endif()
+
+# ========================================================
+# Extras
+# ========================================================
+include(cmake/${PROJECT_NAME}-extras.cmake)
+
+# ========================================================
+# Dependencies
+# ========================================================
+if(BUILDING_WITH_CATKIN)
+
+  find_package(catkin REQUIRED)
+
+  catkin_package(
+    INCLUDE_DIRS
+      include
+      ${EIGEN3_INCLUDE_DIR}
+    LIBRARIES
+      ${PROJECT_NAME}
+    CATKIN_DEPENDS
+    DEPENDS
+    CFG_EXTRAS
+      ${PROJECT_NAME}-extras.cmake
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIR}
+  )
+
+else()
+
+  # Standalone build settings
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # RPATH (quan trọng cho plugin / shared lib)
+  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
+  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
+
+endif()
+
+# ========================================================
+# Library
+# ========================================================
+add_library(${PROJECT_NAME} SHARED
+  src/GridMap.cpp
+  src/GridMapMath.cpp
+  src/SubmapGeometry.cpp
+  src/BufferRegion.cpp
+  src/Polygon.cpp
+  src/CubicInterpolation.cpp
+  src/iterators/GridMapIterator.cpp
+  src/iterators/SubmapIterator.cpp
+  src/iterators/CircleIterator.cpp
+  src/iterators/EllipseIterator.cpp
+  src/iterators/SpiralIterator.cpp
+  src/iterators/PolygonIterator.cpp
+  src/iterators/LineIterator.cpp
+  src/iterators/SlidingWindowIterator.cpp
+)
+
+# ========================================================
+# Includes
+# ========================================================
+target_include_directories(${PROJECT_NAME}
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+    ${EIGEN3_INCLUDE_DIR}
+)
+
+# ========================================================
+# Link libraries
+# ========================================================
+if(BUILDING_WITH_CATKIN)
+
+  add_dependencies(${PROJECT_NAME}
+    ${${PROJECT_NAME}_EXPORTED_TARGETS}
+    ${catkin_EXPORTED_TARGETS}
+  )
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC ${catkin_LIBRARIES}
+  )
+
+else()
+
+  set_target_properties(${PROJECT_NAME} PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+    BUILD_RPATH "${CMAKE_BINARY_DIR}"
+    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+  )
+
+endif()
+
+# ========================================================
+# Install
+# ========================================================
+if(BUILDING_WITH_CATKIN)
+
+  install(TARGETS ${PROJECT_NAME}
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.hpp"
+  )
+
+  install(DIRECTORY doc
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  )
+
+else()
+
+  install(TARGETS ${PROJECT_NAME}
+    EXPORT ${PROJECT_NAME}-targets
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+  )
+
+  install(EXPORT ${PROJECT_NAME}-targets
+    FILE ${PROJECT_NAME}-targets.cmake
+    NAMESPACE ${PROJECT_NAME}::
+    DESTINATION lib/cmake/${PROJECT_NAME}
+  )
+
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.hpp"
+  )
+
+  install(DIRECTORY doc
+    DESTINATION share/${PROJECT_NAME}
+  )
+
+  message(STATUS "=================================")
+  message(STATUS "Project: ${PROJECT_NAME}")
+  message(STATUS "Version: ${PROJECT_VERSION}")
+  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+  message(STATUS "Library: ${PROJECT_NAME}")
+  message(STATUS "Eigen include: ${EIGEN3_INCLUDE_DIR}")
+  message(STATUS "=================================")
+
+endif()

cmake/grid_map_core-extras.cmake

@@ -0,0 +1,17 @@
+set(EIGEN_FUNCTORS_PLUGIN_PATH "grid_map_core/eigen_plugins/FunctorsPlugin.hpp")
+if (EIGEN_FUNCTORS_PLUGIN)
+  if (NOT EIGEN_FUNCTORS_PLUGIN STREQUAL EIGEN_FUNCTORS_PLUGIN_PATH)
+    MESSAGE(FATAL_ERROR "EIGEN_FUNCTORS_PLUGIN already defined!")
+  endif ()
+else (EIGEN_FUNCTORS_PLUGIN)
+  add_definitions(-DEIGEN_FUNCTORS_PLUGIN=\"${EIGEN_FUNCTORS_PLUGIN_PATH}\")
+endif (EIGEN_FUNCTORS_PLUGIN)
+
+set(EIGEN_DENSEBASE_PLUGIN_PATH "grid_map_core/eigen_plugins/DenseBasePlugin.hpp")
+if (EIGEN_DENSEBASE_PLUGIN)
+  if (NOT EIGEN_DENSEBASE_PLUGIN STREQUAL EIGEN_DENSEBASE_PLUGIN_PATH)
+    MESSAGE(FATAL_ERROR "EIGEN_DENSEBASE_PLUGIN already defined!")
+  endif ()
+else (EIGEN_DENSEBASE_PLUGIN)
+    add_definitions(-DEIGEN_DENSEBASE_PLUGIN=\"${EIGEN_DENSEBASE_PLUGIN_PATH}\")
+endif (EIGEN_DENSEBASE_PLUGIN)

include/grid_map_core/BufferRegion.hpp

@@ -0,0 +1,63 @@
+/*
+ * BufferRegion.hpp
+ *
+ *  Created on: Aug 19, 2015
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include "grid_map_core/TypeDefs.hpp"
+
+namespace grid_map {
+
+/*!
+ * This class holds information about a rectangular region
+ * of cells of the circular buffer.
+ */
+class BufferRegion
+{
+ public:
+
+  /*!
+   * The definition of the buffer region positions.
+   */
+  enum class Quadrant
+  {
+    Undefined,
+    TopLeft,
+    TopRight,
+    BottomLeft,
+    BottomRight
+  };
+
+  constexpr static unsigned int nQuadrants = 4;
+
+  BufferRegion();
+  BufferRegion(Index startIndex, Size size, BufferRegion::Quadrant quadrant);
+  virtual ~BufferRegion() = default;
+
+  const Index& getStartIndex() const;
+  void setStartIndex(const Index& startIndex);
+  const Size& getSize() const;
+  void setSize(const Size& size);
+  BufferRegion::Quadrant getQuadrant() const;
+  void setQuadrant(BufferRegion::Quadrant type);
+
+ private:
+
+  //! Start index (typically top-left) of the buffer region.
+  Index startIndex_;
+
+  //! Size of the buffer region.
+  Size size_;
+
+  //! Quadrant type of the buffer region.
+  Quadrant quadrant_;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+} /* namespace grid_map */

include/grid_map_core/CubicInterpolation.hpp

@@ -0,0 +1,345 @@
+/*
+ * CubicInterpolation.hpp
+ *
+ *  Created on: Jan 21, 2020
+ *      Author: Edo Jelavic
+ *      Institute: ETH Zurich, Robotic Systems Lab
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+#include <vector>
+#include <map>
+#include "grid_map_core/TypeDefs.hpp"
+
+/*
+ * For difference between bicubic convolution interpolation (piecewise cubic)
+ * and bicubic interpolation see:
+ *
+ * https://en.wikipedia.org/wiki/Bicubic_interpolation
+ *
+ * R. Keys (1981). "Cubic convolution interpolation for digital image processing".
+ * IEEE Transactions on Acoustics, Speech, and Signal Processing. 29 (6): 1153–1160.
+ *
+*   https://web.archive.org/web/20051024202307/
+*   http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm
+ */
+
+namespace grid_map {
+
+class GridMap;
+
+/*
+ * Data structure (matrix) that contains data
+ * necessary for interpolation. These are either 16
+ * function values in the case of bicubic convolution interpolation
+ * or function values and their derivatives for the case
+ * of standard bicubic interpolation.
+ */
+using FunctionValueMatrix = Eigen::Matrix4d;
+
+/*!
+ * Takes the id requested, performs checks and returns
+ * an id that it is within the specified bounds.
+ * @param[in] idReq - input index .
+ * @param[in] nElem - number of elements in the container
+ * @return index that is within [0, nElem-1].
+ */
+unsigned int bindIndexToRange(unsigned int idReq, unsigned int nElem);
+
+
+/*!
+ * Extract the value of the specific layer at the
+ * row and column requested. If row and column are out
+ * of bounds they will be bound to range.
+ * @param[in] layerMat - matrix of the layer from where
+ *                       the data is extracted
+ * @param[in] rowReq - row requested
+ * @param[in] colReq - column requested
+ * @return - value of the layer at rowReq and colReq
+ */
+double getLayerValue(const Matrix &layerMat, int rowReq, int colReq);
+
+namespace bicubic_conv {
+
+/*!
+ * Matrix for cubic interpolation via convolution. Taken from:
+ * https://en.wikipedia.org/wiki/Bicubic_interpolation
+ */
+static const Eigen::Matrix4d cubicInterpolationConvolutionMatrix {
+    (Eigen::Matrix4d() << 0.0,  2.0,  0.0,  0.0,
+                         -1.0,  0.0,  1.0,  0.0,
+                          2.0, -5.0,  4.0, -1.0,
+                         -1.0,  3.0, -3.0,  1.0).finished() };
+
+/*
+ * Index of the middle knot for bicubic interpolation. This is
+ * the function value with subscripts (0,0), i.e. f00 in
+ * https://en.wikipedia.org/wiki/Bicubic_interpolation
+ * In the grid map it corresponds to the grid map point closest to the
+ * queried point (in terms of Euclidean distance). Queried point has
+ * coordinates (x,y) for at which the interpolation is requested.
+ * @param[in]  gridMap - grid map with the data
+ * @param[in]  queriedPosition - position for which the interpolated data is requested
+ * @param[out] index - indices of the middle knot for the interpolation
+ * @return - true if success
+ */
+bool getIndicesOfMiddleKnot(const GridMap &gridMap, const Position &queriedPosition, Index *index);
+
+/*
+ * Coordinates used for interpolation need to be shifted and scaled,
+ * since the original algorithm operates around the origin and with unit
+ * resolution
+ * @param[in]  gridMap - grid map with the data
+ * @param[in]  queriedPosition - position for which the interpolation is requested
+ * @param[out] position - normalized coordinates of the point for which the interpolation is requested
+ * @return - true if success
+ */
+bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPosition,
+                              Position *position);
+
+/*
+ * Queries the grid map for function values at the coordinates which are necessary for
+ * performing the interpolation. The right function values are then assembled
+ * in a matrix.
+ * @param[in]  gridMap - grid map with the data
+ * @param[in]  layer - name of the layer that we are interpolating
+ * @param[in]  queriedPosition - position for which the interpolation is requested
+ * @param[out] data - 4x4 matrix with 16 function values used for interpolation, see
+ *           R. Keys (1981). "Cubic convolution interpolation for digital image processing".
+ *           IEEE Transactions on Acoustics, Speech, and Signal Processing. 29 (6): 1153–1160.
+ *           for the details.
+ * @return - true if success
+ */
+bool assembleFunctionValueMatrix(const GridMap &gridMap, const std::string &layer,
+                                 const Position &queriedPosition, FunctionValueMatrix *data);
+
+/*
+ * Performs convolution in 1D. the function requires 4 function values
+ * to compute the convolution. The result is interpolated data in 1D.
+ * @param[in]  t - normalized coordinate (x or y)
+ * @param[in]  functionValues - vector of 4 function values necessary to perform
+ *                            interpolation in 1 dimension.
+ * @return - interpolated value at normalized coordinate t
+ */
+double convolve1D(double t, const Eigen::Vector4d &functionValues);
+
+/*
+ * Performs convolution in 1D. the function requires 4 function values
+ * to compute the convolution. The result is interpolated data in 1D.
+ * @param[in]  gridMap - grid map with discrete function values
+ * @param[in]  layer - name of the layer for which we want to perform interpolation
+ * @param[in]  queriedPosition - position for which the interpolation is requested
+ * @param[out] interpolatedValue - interpolated value at queried point
+ * @return - true if success
+ */
+bool evaluateBicubicConvolutionInterpolation(const GridMap &gridMap, const std::string &layer,
+                                             const Position &queriedPosition,
+                                             double *interpolatedValue);
+
+} /* namespace bicubic_conv */
+
+namespace bicubic {
+
+/*
+ * Enum for the derivatives direction
+ * to perform interpolation one needs
+ * derivatives w.r.t. to x and y dimension.
+ */
+enum class Dim2D: int {
+    X,
+    Y
+};
+
+/*!
+ * Matrix for cubic interpolation. Taken from:
+ * https://en.wikipedia.org/wiki/Bicubic_interpolation
+ */
+static const Eigen::Matrix4d bicubicInterpolationMatrix {
+    (Eigen::Matrix4d() << 1.0,  0.0,  0.0,  0.0,
+                          0.0,  0.0,  1.0,  0.0,
+                         -3.0,  3.0, -2.0, -1.0,
+                          2.0, -2.0,  1.0,  1.0).finished() };
+
+/*
+ * Data matrix that can hold function values
+ * these can be either function values at requested
+ * positions or their derivatives.
+ */
+struct DataMatrix
+{
+  double topLeft_ = 0.0;
+  double topRight_ = 0.0;
+  double bottomLeft_ = 0.0;
+  double bottomRight_ = 0.0;
+};
+
+/*
+ * Interpolation is performed on a unit square.
+ * Hence, we need to compute 4 corners of that unit square,
+ * and find their indices in the grid map. IndicesMatrix
+ * is a container that stores those indices. Each index
+ * contains two numbers (row number, column number) in the
+ * grid map.
+ */
+struct IndicesMatrix
+{
+  Index topLeft_ { 0, 0 };
+  Index topRight_ { 0, 0 };
+  Index bottomLeft_ { 0, 0 };
+  Index bottomRight_ { 0, 0 };
+};
+
+/*
+ * Makes sure that all indices in side the
+ * data structure IndicesMatrix are within the
+ * range of the grid map.
+ * @param[in] gridMap - input grid map with discrete function values
+ * @param[in/out] indices - indices that are bound to range, i.e.
+ *                          rows and columns are with ranges
+ */
+void bindIndicesToRange(const GridMap &gridMap, IndicesMatrix *indices);
+
+/*
+ * Performs bicubic interpolation at requested position.
+ * @param[in]  gridMap - grid map with discrete function values
+ * @param[in]  layer - name of the layer for which we want to perform interpolation
+ * @param[in]  queriedPosition - position for which the interpolation is requested
+ * @param[out] interpolatedValue - interpolated value at queried point
+ * @return - true if success
+ */
+bool evaluateBicubicInterpolation(const GridMap &gridMap, const std::string &layer,
+                                  const Position &queriedPosition, double *interpolatedValue);
+
+/*
+ * Deduces which points in the grid map close a unit square around the
+ * queried point and returns their indices (row and column number)
+ * @param[in]  gridMap - grid map with discrete function values
+ * @param[in]  queriedPosition - position for which the interpolation is requested
+ * @param[out] indicesMatrix - data structure with indices forming a unit square
+ *                            around the queried point
+ * @return - true if success
+ */
+bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedPosition,
+                                IndicesMatrix *indicesMatrix);
+
+/*
+ * Get index (row and column number) of a point in grid map, which
+ * is closest to the queried position.
+ * @param[in]  gridMap - grid map with discrete function values
+ * @param[in]  queriedPosition - position for which the interpolation is requested
+ * @param[out] index - indices of the closest point in grid_map
+ * @return - true if success
+ */
+bool getClosestPointIndices(const GridMap &gridMap, const Position &queriedPosition, Index *index);
+
+/*
+ * Retrieve function values from the grid map at requested indices.
+ * @param[in]  layerData - layer of a grid map with function values
+ * @param[in]  indices - indices (row and column numbers) for which function values are requested
+ * @param[out] data - requested function values
+ * @return - true if success
+ */
+bool getFunctionValues(const Matrix &layerData, const IndicesMatrix &indices, DataMatrix *data);
+
+/*
+ * Retrieve function derivative values from the grid map at requested indices. Function
+ * derivatives are approximated using central difference.
+ * @param[in]  layerData - layer of a grid map with function values
+ * @param[in]  indices - indices (row and column numbers) for which function derivative
+ *                       values are requested
+ * @param[in]  dim - dimension along which we want to evaluate partial derivatives (X or Y)
+ * @param[in]  resolution - resolution of the grid map
+ * @param[out] derivatives - values of derivatives at requested indices
+ * @return - true if success
+ */
+bool getFirstOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, Dim2D dim,
+                              double resolution, DataMatrix *derivatives);
+
+/*
+ * Retrieve second order function derivative values from the grid map at requested indices.
+ * Function derivatives are approximated using central difference. We compute partial derivative
+ * w.r.t to one coordinate and then the other. Note that the order of differentiation
+ * does not matter.
+ * @param[in]  layerData - layer of a grid map with function values
+ * @param[in]  indices - indices (row and column numbers) for which function derivative
+ *                       values are requested
+ * @param[in]  resolution - resolution of the grid map
+ * @param[out] derivatives - values of second order mixed derivatives at requested indices
+ * @return - true if success
+ */
+bool getMixedSecondOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices,
+                                    double resolution, DataMatrix *derivatives);
+
+/*
+ * First order derivative for a specific point determined by index.
+ * Approximated by central difference.
+ * See https://www.mathematik.uni-dortmund.de/~kuzmin/cfdintro/lecture4.pdf
+ * for details
+ * @param[in]  layerData - layer of a grid map with function values
+ * @param[in]  index - index (row and column number) for which function derivative
+ *                       value is requested
+ * @param[in]  dim - dimension along which we want to evaluate partial derivative (X or Y)
+ * @param[in]  resolution - resolution of the grid map
+ * @return - value of the derivative at requested index
+ */
+double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D dim,
+                              double resolution);
+
+/*
+ * Second order mixed derivative for a specific point determined by index.
+ * See https://www.mathematik.uni-dortmund.de/~kuzmin/cfdintro/lecture4.pdf
+ * for details
+ * @param[in]  layerData - layer of a grid map with function values
+ * @param[in]  index - index (row and column number) for which function derivative
+ *                       value is requested
+ * @param[in]  resolution - resolution of the grid map
+ * @return - value of the second order mixed derivative at requested index
+ */
+double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, double resolution);
+
+/*
+ * Evaluate polynomial at requested coordinates. the function will compute the polynomial
+ * coefficients and then evaluate it. See
+ * https://en.wikipedia.org/wiki/Bicubic_interpolation
+ * for details.
+ * @param[in]  functionValues - function values and derivatives required to
+ *                              compute polynomial coefficients
+ * @param[in]  tx - normalized x coordinate for which the interpolation should be computed
+ * @param[in]  ty - normalized y coordinate for which the interpolation should be computed
+ * @return - interpolated value at requested normalized coordinates.
+ */
+double evaluatePolynomial(const FunctionValueMatrix &functionValues, double tx, double ty);
+
+/*
+ * Assemble function value matrix from small sub-matrices containing function values
+ * or derivative values at the corners of the unit square.
+ * See https://en.wikipedia.org/wiki/Bicubic_interpolation for details.
+ *
+ * @param[in]  f - Function values at the corners of the unit square
+ * @param[in]  dfx - Partial derivative w.r.t to x at the corners of the unit square
+ * @param[in]  dfy - Partial derivative w.r.t to y at the corners of the unit square
+ * @param[in]  ddfxy - Second order partial derivative w.r.t to x and y at the corners of the unit square
+ * @param[out]  functionValues - function values and derivatives required to
+ *                              compute polynomial coefficients
+ */
+void assembleFunctionValueMatrix(const DataMatrix &f, const DataMatrix &dfx, const DataMatrix &dfy,
+                                 const DataMatrix &ddfxy, FunctionValueMatrix *functionValues);
+
+/*
+ * Coordinates used for interpolation need to be shifter and scaled,
+ * since the original algorithm operates on a unit square around the origin.
+ * @param[in]  gridMap - grid map with the data
+ * @param[in]  originIndex - index of a bottom left corner if the unit square in the grid map
+ *                            this corner is the origin for the normalized coordinates.
+ * @param[in]  queriedPosition - position for which the interpolation is requested
+ * @param[out] position - normalized coordinates of the point for which the interpolation is requested
+ * @return - true if success
+ */
+bool computeNormalizedCoordinates(const GridMap &gridMap, const Index &originIndex,
+                                  const Position &queriedPosition, Position *normalizedCoordinates);
+
+} /* namespace bicubic */
+
+} /* namespace grid_map*/

include/grid_map_core/GridMap.hpp

@@ -0,0 +1,589 @@
+/*
+ * GridMap.hpp
+ *
+ *  Created on: Jul 14, 2014
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include "grid_map_core/BufferRegion.hpp"
+#include "grid_map_core/SubmapGeometry.hpp"
+#include "grid_map_core/TypeDefs.hpp"
+
+// STL
+#include <unordered_map>
+#include <vector>
+
+// Eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace grid_map {
+
+class SubmapGeometry;
+
+/*!
+ * Grid map managing multiple overlaying maps holding float values.
+ * Data structure implemented as two-dimensional circular buffer so map
+ * can be moved efficiently.
+ *
+ * Data is defined with string keys. Examples are:
+ * - "elevation"
+ * - "variance"
+ * - "color"
+ * - "quality"
+ * - "surface_normal_x", "surface_normal_y", "surface_normal_z"
+ * etc.
+ */
+class GridMap {
+ public:
+  // Type traits for use with template methods/classes using GridMap as a template parameter.
+  typedef grid_map::DataType DataType;
+  typedef grid_map::Matrix Matrix;
+
+  /*!
+   * Constructor.
+   * @param layers a vector of strings containing the definition/description of the data layer.
+   */
+  explicit GridMap(const std::vector<std::string>& layers);
+
+  /*!
+   * Emtpy constructor.
+   */
+  GridMap();
+
+  /*!
+   * Default copy assign and copy constructors.
+   */
+  GridMap(const GridMap&) = default;
+  GridMap& operator=(const GridMap&) = default;
+  GridMap(GridMap&&) = default;
+  GridMap& operator=(GridMap&&) = default;
+
+  /*!
+   * Destructor.
+   */
+  virtual ~GridMap() = default;
+
+  /*!
+   * Set the geometry of the grid map. Clears all the data.
+   * @param length the side lengths in x, and y-direction of the grid map [m].
+   * @param resolution the cell size in [m/cell].
+   * @param position the 2d position of the grid map in the grid map frame [m].
+   */
+  void setGeometry(const Length& length, const double resolution, const Position& position = Position::Zero());
+
+  /*!
+   * Set the geometry of the grid map from submap geometry information.
+   * @param geometry the submap geometry information.
+   */
+  void setGeometry(const SubmapGeometry& geometry);
+
+  /*!
+   * Add a new empty data layer.
+   * @param layer the name of the layer.
+   * @value value the value to initialize the cells with.
+   */
+  void add(const std::string& layer, const float value = NAN);
+
+  /*!
+   * Add a new data layer (if the layer already exists, overwrite its data, otherwise add layer and data).
+   * @param layer the name of the layer.
+   * @param data the data to be added.
+   */
+  void add(const std::string& layer, const Matrix& data);
+
+  /*!
+   * Checks if data layer exists.
+   * @param layer the name of the layer.
+   * @return true if layer exists, false otherwise.
+   */
+  bool exists(const std::string& layer) const;
+
+  /*!
+   * Returns the grid map data for a layer as matrix.
+   * @param layer the name of the layer to be returned.
+   * @return grid map data as matrix.
+   * @throw std::out_of_range if no map layer with name `layer` is present.
+   */
+  const Matrix& get(const std::string& layer) const;
+
+  /*!
+   * Returns the grid map data for a layer as non-const. Use this method
+   * with care!
+   * @param layer the name of the layer to be returned.
+   * @return grid map data.
+   * @throw std::out_of_range if no map layer with name `layer` is present.
+   */
+  Matrix& get(const std::string& layer);
+
+  /*!
+   * Returns the grid map data for a layer as matrix.
+   * @param layer the name of the layer to be returned.
+   * @return grid map data as matrix.
+   * @throw std::out_of_range if no map layer with name `layer` is present.
+   */
+  const Matrix& operator[](const std::string& layer) const;
+
+  /*!
+   * Returns the grid map data for a layer as non-const. Use this method
+   * with care!
+   * @param layer the name of the layer to be returned.
+   * @return grid map data.
+   * @throw std::out_of_range if no map layer with name `layer` is present.
+   */
+  Matrix& operator[](const std::string& layer);
+
+  /*!
+   * Removes a layer from the grid map.
+   * @param layer the name of the layer to be removed.
+   * @return true if successful.
+   */
+  bool erase(const std::string& layer);
+
+  /*!
+   * Gets the names of the layers.
+   * @return the names of the layers.
+   */
+  const std::vector<std::string>& getLayers() const;
+
+  /*!
+   * Set the basic layers that need to be valid for a cell to be considered as valid.
+   * Also, the basic layers are set to NAN when clearing the cells with `clearBasic()`.
+   * By default the list of basic layers is empty.
+   * @param basicLayers the list of layers that are the basic layers of the map.
+   */
+  void setBasicLayers(const std::vector<std::string>& basicLayers);
+
+  /*!
+   * Gets the names of the basic layers.
+   * @return the names of the basic layers.
+   */
+  const std::vector<std::string>& getBasicLayers() const;
+
+  /*!
+   * True if basic layers are defined.
+   * @return true if basic layers are defined, false otherwise.
+   */
+  bool hasBasicLayers() const;
+
+  /*!
+   * Checks if another grid map contains the same layers as this grid map.
+   * The other grid map could contain more layers than the checked ones.
+   * Does not check the selection of basic layers.
+   * @param other the other grid map.
+   * @return true if the other grid map has the same layers, false otherwise.
+   */
+  bool hasSameLayers(const grid_map::GridMap& other) const;
+
+  /*!
+   * Get cell data at requested position.
+   * @param layer the name of the layer to be accessed.
+   * @param position the requested position.
+   * @return the data of the cell.
+   * @throw std::out_of_range if no map layer with name `layer` is present.
+   */
+  float& atPosition(const std::string& layer, const Position& position);
+
+  /*!
+   * Get cell data at requested position. Const version form above.
+   * @param layer the name of the layer to be accessed.
+   * @param position the requested position.
+   * @return the data of the cell.
+   * @throw std::out_of_range if no map layer with name `layer` is present.
+   * @throw std::runtime_error if the specified interpolation method is not implemented.
+   */
+  float atPosition(const std::string& layer, const Position& position,
+                   InterpolationMethods interpolationMethod = InterpolationMethods::INTER_NEAREST) const;
+
+  /*!
+   * Get cell data for requested index.
+   * @param layer the name of the layer to be accessed.
+   * @param index the requested index.
+   * @return the data of the cell.
+   * @throw std::out_of_range if no map layer with name `layer` is present.
+   */
+  float& at(const std::string& layer, const Index& index);
+
+  /*!
+   * Get cell data for requested index. Const version form above.
+   * @param layer the name of the layer to be accessed.
+   * @param index the requested index.
+   * @return the data of the cell.
+   * @throw std::out_of_range if no map layer with name `layer` is present.
+   */
+  float at(const std::string& layer, const Index& index) const;
+
+  /*!
+   * Gets the corresponding cell index for a position.
+   * @param[in] position the requested position.
+   * @param[out] index the corresponding index.
+   * @return true if successful, false if position outside of map.
+   */
+  bool getIndex(const Position& position, Index& index) const;
+
+  /*!
+   * Gets the 2d position of cell specified by the index (x, y of cell position) in
+   * the grid map frame.
+   * @param[in] index the index of the requested cell.
+   * @param[out] position the position of the data point in the parent frame.
+   * @return true if successful, false if index not within range of buffer.
+   */
+  bool getPosition(const Index& index, Position& position) const;
+
+  /*!
+   * Check if position is within the map boundaries.
+   * @param position the position to be checked.
+   * @return true if position is within map, false otherwise.
+   */
+  bool isInside(const Position& position) const;
+
+  /*!
+   * Checks if the index of all layers defined as basic types are valid,
+   * i.e. if all basic types are finite. Returns `false` if no basic types are defined.
+   * @param index the index to check.
+   * @return true if cell is valid, false otherwise.
+   */
+  bool isValid(const Index& index) const;
+
+  /*!
+   * Checks if cell at index is a valid (finite) for a certain layer.
+   * @param index the index to check.
+   * @param layer the name of the layer to be checked for validity.
+   * @return true if cell is valid, false otherwise.
+   */
+  bool isValid(const Index& index, const std::string& layer) const;
+
+  /*!
+   * Checks if cell at index is a valid (finite) for certain layers.
+   * @param index the index to check.
+   * @param layers the layers to be checked for validity.
+   * @return true if cell is valid, false otherwise.
+   */
+  bool isValid(const Index& index, const std::vector<std::string>& layers) const;
+
+  /*!
+   * Gets the 3d position of a data point (x, y of cell position & cell value as z) in
+   * the grid map frame. This is useful for data layers such as elevation.
+   * @param layer the name of the layer to be accessed.
+   * @param index the index of the requested cell.
+   * @param position the position of the data point in the parent frame.
+   * @return true if successful, false if no valid data available.
+   */
+  bool getPosition3(const std::string& layer, const Index& index, Position3& position) const;
+
+  /*!
+   * Gets the 3d vector of three layers with suffixes 'x', 'y', and 'z'.
+   * @param layerPrefix the prefix for the layer to bet get as vector.
+   * @param index the index of the requested cell.
+   * @param vector the vector with the values of the data type.
+   * @return true if successful, false if no valid data available.
+   */
+  bool getVector(const std::string& layerPrefix, const Index& index, Eigen::Vector3d& vector) const;
+
+  /*!
+   * Gets a submap from the map. The requested submap is specified with the requested
+   * location and length.
+   * Note: The returned submap may not have the requested length due to the borders
+   * of the map and discretization.
+   * @param[in] position the requested position of the submap (usually the center).
+   * @param[in] length the requested length of the submap.
+   * @param[out] isSuccess true if successful, false otherwise.
+   * @return submap (is empty if success is false).
+   */
+  GridMap getSubmap(const Position& position, const Length& length, bool& isSuccess) const;
+
+  /*!
+   * Gets a submap from the map. The requested submap is specified with the requested
+   * location and length.
+   * Note: The returned submap may not have the requested length due to the borders
+   * of the map and discretization.
+   * @param[in] position the requested position of the submap (usually the center).
+   * @param[in] length the requested length of the submap.
+   * @param[out] indexInSubmap the index of the requested position in the submap.
+   * @param[out] isSuccess true if successful, false otherwise.
+   * @return submap (is empty if success is false).
+   */
+  GridMap getSubmap(const Position& position, const Length& length, Index& indexInSubmap, bool& isSuccess) const;
+
+  /*!
+   * Apply isometric transformation (rotation + offset) to grid map and returns the transformed map.
+   * Note: The returned map may not have the same length since it's geometric description contains
+   * Note: The transformation will only be applied to the height layer of the grid map, other layers will remain untouched.
+   * the original map.
+   * @param[in] transform the requested transformation to apply.
+   * @param[in] heightLayerName the height layer of the map.
+   * @param[in] newFrameId frame index of the new map.
+   * @param[in] sampleRatio if zero or negative, no in-painting is used to fill missing points due to sparsity of the map. Otherwise,
+   *            four points are sampled around each grid cell to make sure that at least one of those points map to a new grid cell.
+   *            A sampleRatio of 1 corresponds to the the resolution of the grid map.
+   * @return transformed map.
+   * @throw std::out_of_range if no map layer with name `heightLayerName` is present.
+   */
+  GridMap getTransformedMap(const Eigen::Isometry3d& transform, const std::string& heightLayerName, const std::string& newFrameId,
+                            const double sampleRatio = 0.0) const;
+
+  /*!
+   * Set the position of the grid map.
+   * Note: This method does not change the data stored in the grid map and
+   * is complementary to the `move(...)` method. For a comparison between
+   * the `setPosition` and the `move` method, see the `move_demo_node.cpp`
+   * file of the `grid_map_demos` package.
+   * @param position the 2d position of the grid map in the grid map frame [m].
+   */
+  void setPosition(const Position& position);
+
+  /*!
+   * Relocates the region captured by grid map w.r.t. to the static grid map frame. Use this to move the grid map boundaries
+   * without relocating the grid map data. Takes care of all the data handling, such that the grid map data is stationary in the grid map
+   * frame.
+   * - Data in the overlapping region before and after the position change remains stored.
+   * - Data that falls outside the map at its new position is discarded.
+   *  - Cells that cover previously unknown regions are emptied (set to nan).
+   *  The data storage is implemented as two-dimensional circular buffer to minimize computational effort.
+   *
+   *  Note: Due to the circular buffer structure, neighbouring indices might not fall close in the map frame.
+   *  This assumption only holds for indices obtained by getUnwrappedIndex().
+   *
+   *  Note: For a comparison between the `setPosition` and the `move` method, see the `move_demo_node.cpp` file of the `grid_map_demos` package.
+   *
+   * @param position the new location of the grid map in the map frame.
+   * @param newRegions the regions of the newly covered / previously uncovered regions of the buffer.
+   * @return true if map has been moved, false otherwise.
+   */
+  bool move(const Position& position, std::vector<BufferRegion>& newRegions);
+
+  /*!
+   * Move the grid map w.r.t. to the grid map frame. Use this to move the grid map
+   * boundaries without moving the grid map data. Takes care of all the data handling,
+   * such that the grid map data is stationary in the grid map frame.
+   * @param position the new location of the grid map in the map frame.
+   * @return true if map has been moved, false otherwise.
+   */
+  bool move(const Position& position);
+
+  /*!
+   * Adds data from an other grid map to this grid map
+   * @param other the grid map to take data from.
+   * @param extendMap if true the grid map is resized that the other map fits within.
+   * @param overwriteData if true the new data replaces the old values, else only invalid cells are updated.
+   * @param copyAllLayer if true all layers are used to add data.
+   * @param layers the layers that are copied if not all layers are used.
+   * @return true if successful.
+   */
+  bool addDataFrom(const GridMap& other, bool extendMap, bool overwriteData, bool copyAllLayers,
+                   std::vector<std::string> layers = std::vector<std::string>());
+
+  /*!
+   * Extends the size of the grip map such that the other grid map fits within.
+   * @param other the grid map to extend the size to.
+   * @return true if successful.
+   */
+  bool extendToInclude(const GridMap& other);
+
+  /*!
+   * Clears all cells (set to NAN) for a layer.
+   * @param layer the layer to be cleared.
+   */
+  void clear(const std::string& layer);
+
+  /*!
+   * Clears all cells (set to NAN) for all basic layers.
+   * Header information (geometry etc.) remains valid.
+   */
+  void clearBasic();
+
+  /*!
+   * Clears all cells of all layers.
+   * If basic layers are used, clearBasic() is preferred as it is more efficient.
+   * Header information (geometry etc.) remains valid.
+   */
+  void clearAll();
+
+  /*!
+   * Set the timestamp of the grid map.
+   * @param timestamp the timestamp to set (in  nanoseconds).
+   */
+  void setTimestamp(const Time timestamp);
+
+  /*!
+   * Get the timestamp of the grid map.
+   * @return timestamp in nanoseconds.
+   */
+  Time getTimestamp() const;
+
+  /*!
+   * Resets the timestamp of the grid map (to zero).
+   */
+  void resetTimestamp();
+
+  /*!
+   * Set the frame id of the grid map.
+   * @param frameId the frame id to set.
+   */
+  void setFrameId(const std::string& frameId);
+
+  /*!
+   * Get the frameId of the grid map.
+   * @return frameId.
+   */
+  const std::string& getFrameId() const;
+
+  /*!
+   * Get the side length of the grid map.
+   * @return side length of the grid map.
+   */
+  const Length& getLength() const;
+
+  /*!
+   * Get the 2d position of the grid map in the grid map frame.
+   * @return position of the grid map in the grid map frame.
+   */
+  const Position& getPosition() const;
+
+  /*!
+   * Get the resolution of the grid map.
+   * @return resolution of the grid map in the xy plane [m/cell].
+   */
+  double getResolution() const;
+
+  /*!
+   * Get the grid map size (rows and cols of the data structure).
+   * @return grid map size.
+   */
+  const Size& getSize() const;
+
+  /*!
+   * Set the start index of the circular buffer.
+   * Use this method with caution!
+   * @return buffer start index.
+   */
+  void setStartIndex(const Index& startIndex);
+
+  /*!
+   * Get the start index of the circular buffer.
+   * @return buffer start index.
+   */
+  const Index& getStartIndex() const;
+
+  /*!
+   * Checks if the buffer is at start index (0,0).
+   * @return true if buffer is at default start index.
+   */
+  bool isDefaultStartIndex() const;
+
+  /*!
+   * Rearranges data such that the buffer start index is at (0,0).
+   */
+  void convertToDefaultStartIndex();
+
+  /*!
+   * Calculates the closest point to positionOutMap that is in the grid map.
+   * If positionOutMap is already in the grid map, that position is returned.
+   * @param[in] position position that should be approached as close as possible.
+   * @return position in map.
+   */
+  Position getClosestPositionInMap(const Position& position) const;
+
+ private:
+  /**
+   * Defines data validation check
+   * @param value
+   * @return true if value is valid
+   */
+  bool isValid(DataType value) const;
+
+  /*!
+   * Clear a number of columns of the grid map.
+   * @param index the left index for the columns to be reset.
+   * @param nCols the number of columns to reset.
+   */
+  void clearCols(unsigned int index, unsigned int nCols);
+
+  /*!
+   * Clear a number of rows of the grid map.
+   * @param index the upper index for the rows to be reset.
+   * @param nRows the number of rows to reset.
+   */
+  void clearRows(unsigned int index, unsigned int nRows);
+
+  /*!
+   * Get cell data at requested position, linearly interpolated from 2x2 cells.
+   * @param layer the name of the layer to be accessed.
+   * @param position the requested position.
+   * @param value the data of the cell.
+   * @return true if linear interpolation was successful.
+   */
+  bool atPositionLinearInterpolated(const std::string& layer, const Position& position, float& value) const;
+
+  /*!
+   * Get cell data at requested position, cubic convolution
+   * interpolated from 4x4 cells. At the edge of the map,
+   * the algorithm assumes that height continues with the slope 0.
+   * I.e. the border cells just repeat outside of the map
+   * Taken from: https://en.wikipedia.org/wiki/Bicubic_interpolation
+   * @param[in] layer the name of the layer to be accessed.
+   * @param[in] position the requested position.
+   * @param[out] value the data of the cell.
+   * @return true if bicubic convolution interpolation was successful.
+   */
+  bool atPositionBicubicConvolutionInterpolated(const std::string& layer, const Position& position, float& value) const;
+
+  /*!
+   * Get cell data at requested position, cubic interpolated
+   * on a square. At the edge of the map,
+   * the algorithm assumes that height continues with the slope 0.
+   * I.e. the border cells just repeat outside of the map
+   * Taken from: https://en.wikipedia.org/wiki/Bicubic_interpolation
+   * @param[in] layer the name of the layer to be accessed.
+   * @param[in] position the requested position.
+   * @param[out] value the data of the cell.
+   * @return true if bicubic interpolation was successful.
+   */
+  bool atPositionBicubicInterpolated(const std::string& layer, const Position& position, float& value) const;
+
+  /*!
+   * Resize the buffer.
+   * @param bufferSize the requested buffer size.
+   */
+  void resize(const Index& bufferSize);
+
+  //! Frame id of the grid map.
+  std::string frameId_;
+
+  //! Timestamp of the grid map (nanoseconds).
+  Time timestamp_;
+
+  //! Grid map data stored as layers of matrices.
+  std::unordered_map<std::string, Matrix> data_;
+
+  //! Names of the data layers.
+  std::vector<std::string> layers_;
+
+  //! List of layers from `data_` that are the basic grid map layers.
+  //! This means that for a cell to be valid, all basic layers need to be valid.
+  //! Also, the basic layers are set to NAN when clearing the map with `clear()`.
+  std::vector<std::string> basicLayers_;
+
+  //! Side length of the map in x- and y-direction [m].
+  Length length_;
+
+  //! Map resolution in xy plane [m/cell].
+  double resolution_;
+
+  //! Map position in the grid map frame [m].
+  Position position_;
+
+  //! Size of the buffer (rows and cols of the data structure).
+  Size size_;
+
+  //! Circular buffer start indices.
+  Index startIndex_;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace grid_map

include/grid_map_core/GridMapMath.hpp

@@ -0,0 +1,344 @@
+/*
+ * GridMapMath.hpp
+ *
+ *  Created on: Dec 2, 2013
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include "grid_map_core/TypeDefs.hpp"
+#include "grid_map_core/BufferRegion.hpp"
+
+#include <Eigen/Core>
+#include <vector>
+#include <map>
+
+namespace grid_map {
+
+union Color
+{
+    unsigned long longColor_;
+    float floatColor_;
+};
+
+/*!
+ * Gets the position of a cell specified by its index in the map frame.
+ * @param[out] position the position of the center of the cell in the map frame.
+ * @param[in] index of the cell.
+ * @param[in] mapLength the lengths in x and y direction.
+ * @param[in] mapPosition the position of the map.
+ * @param[in] resolution the resolution of the map.
+ * @param[in] bufferSize the size of the buffer (optional).
+ * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
+ * @return true if successful, false if index not within range of buffer.
+ */
+bool getPositionFromIndex(Position& position,
+                          const Index& index,
+                          const Length& mapLength,
+                          const Position& mapPosition,
+                          const double& resolution,
+                          const Size& bufferSize,
+                          const Index& bufferStartIndex = Index::Zero());
+
+/*!
+ * Gets the index of the cell which contains a position in the map frame.
+ * @param[out] index of the cell.
+ * @param[in] position the position in the map frame.
+ * @param[in] mapLength the lengths in x and y direction.
+ * @param[in] mapPosition the position of the map.
+ * @param[in] resolution the resolution of the map.
+ * @param[in] bufferSize the size of the buffer (optional).
+ * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
+ * @return true if successful, false if position outside of map.
+ */
+bool getIndexFromPosition(Index& index,
+                          const Position& position,
+                          const Length& mapLength,
+                          const Position& mapPosition,
+                          const double& resolution,
+                          const Size& bufferSize,
+                          const Index& bufferStartIndex = Index::Zero());
+
+/*!
+ * Checks if position is within the map boundaries.
+ * @param[in] position the position which is to be checked.
+ * @param[in] mapLength the length of the map.
+ * @param[in] mapPosition the position of the map.
+ * @return true if position is within map, false otherwise.
+ */
+bool checkIfPositionWithinMap(const Position& position,
+                              const Length& mapLength,
+                              const Position& mapPosition);
+
+/*!
+ * Gets the position of the data structure origin.
+ * @param[in] position the position of the map.
+ * @param[in] mapLength the map length.
+ * @param[out] positionOfOrigin the position of the data structure origin.
+ */
+void getPositionOfDataStructureOrigin(const Position& position,
+                                      const Length& mapLength,
+                                      Position& positionOfOrigin);
+
+/*!
+ * Computes how many cells/indices the map is moved based on a position shift in
+ * the grid map frame. Use this function if you are moving the grid map
+ * and want to ensure that the cells match before and after.
+ * @param[out] indexShift the corresponding shift of the indices.
+ * @param[in] positionShift the desired position shift.
+ * @param[in] resolution the resolution of the map.
+ * @return true if successful.
+ */
+bool getIndexShiftFromPositionShift(Index& indexShift,
+                                    const Vector& positionShift,
+                                    const double& resolution);
+
+/*!
+ * Computes the corresponding position shift from a index shift. Use this function
+ * if you are moving the grid map and want to ensure that the cells match
+ * before and after.
+ * @param[out] positionShift the corresponding shift in position in the grid map frame.
+ * @param[in] indexShift the desired shift of the indices.
+ * @param[in] resolution the resolution of the map.
+ * @return true if successful.
+ */
+bool getPositionShiftFromIndexShift(Vector& positionShift,
+                                    const Index& indexShift,
+                                    const double& resolution);
+
+/*!
+ * Checks if index is within range of the buffer.
+ * @param[in] index to check.
+ * @param[in] bufferSize the size of the buffer.
+ * @return true if index is within, and false if index is outside of the buffer.
+ */
+bool checkIfIndexInRange(const Index& index, const Size& bufferSize);
+
+/*!
+ * Bounds an index that runs out of the range of the buffer.
+ * This means that an index that overflows is stopped at the last valid index.
+ * This is the 2d version of boundIndexToRange(int&, const int&).
+ * @param[in/out] index the indices that will be bounded to the valid region of the buffer.
+ * @param[in] bufferSize the size of the buffer.
+ */
+void boundIndexToRange(Index& index, const Size& bufferSize);
+
+/*!
+ * Bounds an index that runs out of the range of the buffer.
+ * This means that an index that overflows is stopped at the last valid index.
+ * @param[in/out] index the index that will be bounded to the valid region of the buffer.
+ * @param[in] bufferSize the size of the buffer.
+ */
+void boundIndexToRange(int& index, const int& bufferSize);
+
+/*!
+ * Wraps an index that runs out of the range of the buffer back into allowed the region.
+ * This means that an index that overflows is reset to zero.
+ * This is the 2d version of wrapIndexToRange(int&, const int&).
+ * @param[in/out] index the indices that will be wrapped into the valid region of the buffer.
+ * @param[in] bufferSize the size of the buffer.
+ */
+void wrapIndexToRange(Index& index, const Size& bufferSize);
+
+/*!
+ * Wraps an index that runs out of the range of the buffer back into allowed the region.
+ * This means that an index that overflows is reset to zero.
+ * @param[in/out] index the index that will be wrapped into the valid region of the buffer.
+ * @param[in] bufferSize the size of the buffer.
+ */
+void wrapIndexToRange(int& index, int bufferSize);
+
+/*!
+ * Bound (cuts off) the position to lie inside the map.
+ * This means that an index that overflows is stopped at the last valid index.
+ * @param[in/out] position the position to be bounded.
+ * @param[in] mapLength the lengths in x and y direction.
+ * @param[in] mapPosition the position of the map.
+ */
+void boundPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition);
+
+/*!
+ * Provides the alignment transformation from the buffer order (outer/inner storage)
+ * and the map frame (x/y-coordinate).
+ * @return the alignment transformation.
+ */
+Eigen::Matrix2i getBufferOrderToMapFrameAlignment();
+
+/*!
+ * Given a map and a desired submap (defined by position and size), this function computes
+ * various information about the submap. The returned submap might be smaller than the requested
+ * size as it respects the boundaries of the map.
+ * @param[out] submapTopLeftIndex the top left index of the returned submap.
+ * @param[out] submapBufferSize the buffer size of the returned submap.
+ * @param[out] submapPosition the position of the submap (center) in the map frame.
+ * @param[out] submapLength the length of the submap.
+ * @param[out] requestedIndexInSubmap the index in the submap that corresponds to the requested
+ *             position of the submap.
+ * @param[in] requestedSubmapPosition the requested submap position (center) in the map frame.
+ * @param[in] requestedSubmapLength the requested submap length.
+ * @param[in] mapLength the lengths in x and y direction.
+ * @param[in] mapPosition the position of the map.
+ * @param[in] resolution the resolution of the map.
+ * @param[in] bufferSize the buffer size of the map.
+ * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
+ * @return true if successful.
+ */
+bool getSubmapInformation(Index& submapTopLeftIndex,
+                          Size& submapBufferSize,
+                          Position& submapPosition,
+                          Length& submapLength,
+                          Index& requestedIndexInSubmap,
+                          const Position& requestedSubmapPosition,
+                          const Length& requestedSubmapLength,
+                          const Length& mapLength,
+                          const Position& mapPosition,
+                          const double& resolution,
+                          const Size& bufferSize,
+                          const Index& bufferStartIndex = Index::Zero());
+
+/*!
+ * Computes the buffer size of a submap given a top left and a lower right index.
+ * @param topLeftIndex the top left index in the map.
+ * @param bottomRightIndex the bottom right index in the map.
+ * @return buffer size for the submap.
+ */
+Size getSubmapSizeFromCornerIndices(const Index& topLeftIndex, const Index& bottomRightIndex,
+                                    const Size& bufferSize, const Index& bufferStartIndex);
+
+/*!
+ * Computes the regions in the circular buffer that make up the data for
+ * a requested submap.
+ * @param[out] submapBufferRegions the list of buffer regions that make up the submap.
+ * @param[in] submapIndex the index (top-left) for the requested submap.
+ * @param[in] submapBufferSize the size of the requested submap.
+ * @param[in] bufferSize the buffer size of the map.
+ * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
+ * @return true if successful, false if requested submap is not fully contained in the map.
+ */
+bool getBufferRegionsForSubmap(std::vector<BufferRegion>& submapBufferRegions,
+                               const Index& submapIndex,
+                               const Size& submapBufferSize,
+                               const Size& bufferSize,
+                               const Index& bufferStartIndex = Index::Zero());
+
+/*!
+ * Increases the index by one to iterate through the map.
+ * Increments either to the neighboring index to the right or to
+ * the start of the lower row. Returns false if end of iterations are reached.
+ * @param[in/out] index the index in the map that is incremented (corrected for the circular buffer).
+ * @param[in] bufferSize the map buffer size.
+ * @param[in] bufferStartIndex the map buffer start index.
+ * @return true if successfully incremented indices, false if end of iteration limits are reached.
+ */
+bool incrementIndex(Index& index, const Size& bufferSize,
+                    const Index& bufferStartIndex = Index::Zero());
+
+/*!
+ * Increases the index by one to iterate through the cells of a submap.
+ * Increments either to the neighboring index to the right or to
+ * the start of the lower row. Returns false if end of iterations are reached.
+ *
+ * Note: This function does not check if submap actually fits to the map. This needs
+ * to be checked before separately.
+ *
+ * @param[in/out] submapIndex the index in the submap that is incremented.
+ * @param[out] index the index in the map that is incremented (corrected for the circular buffer).
+ * @param[in] submapTopLefIndex the top left index of the submap.
+ * @param[in] submapBufferSize the submap buffer size.
+ * @param[in] bufferSize the map buffer size.
+ * @param[in] bufferStartIndex the map buffer start index.
+ * @return true if successfully incremented indices, false if end of iteration limits are reached.
+ */
+bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& submapTopLeftIndex,
+                             const Size& submapBufferSize, const Size& bufferSize,
+                             const Index& bufferStartIndex = Index::Zero());
+
+/*!
+ * Retrieve the index as unwrapped index, i.e., as the corresponding index of a
+ * grid map with no circular buffer offset.
+ * @param bufferIndex the index in the circular buffer.
+ * @param bufferSize the map buffer size.
+ * @param bufferStartIndex the map buffer start index.
+ * @return the unwrapped index.
+ */
+Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize,
+                              const Index& bufferStartIndex);
+
+/*!
+ * Retrieve the index of the buffer from a unwrapped index (reverse from function above).
+ * @param index the unwrapped index.
+ * @param bufferSize the map buffer size.
+ * @param bufferStartIndex the map buffer start index.
+ * @return the buffer index.
+ */
+Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize, const Index& bufferStartIndex);
+
+/*!
+ * Returns the linear index (1-dim.) corresponding to the regular index (2-dim.) for either
+ * row- or column-major format.
+ * Note: Eigen is defaulting to column-major format.
+ * @param[in] index the regular 2d index.
+ * @param[in] bufferSize the map buffer size.
+ * @param[in] (optional) rowMajor if the linear index is generated for row-major format.
+ * @return the linear 1d index.
+ */
+size_t getLinearIndexFromIndex(const Index& index, const Size& bufferSize, bool rowMajor = false);
+
+/*!
+ * Returns the regular index (2-dim.) corresponding to the linear index (1-dim.) for a given buffer size.
+ * @param[in] linearIndex the he linear 1d index.
+ * @param[in] bufferSize the map buffer size.
+ * @param[in] (optional) rowMajor if the linear index is generated for row-major format.
+ * @return the regular 2d index.
+ */
+Index getIndexFromLinearIndex(size_t linearIndex, const Size& bufferSize, bool rowMajor = false);
+
+/*!
+ * Transforms an int color value (concatenated RGB values) to an int color vector (RGB from 0-255).
+ * @param [in] colorValue the concatenated RGB color value.
+ * @param [out] colorVector the color vector in RGB from 0-255.
+ * @return true if successful.
+ */
+bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3i& colorVector);
+
+/*!
+ * Transforms an int color value (concatenated RGB values) to a float color vector (RGB from 0.0-1.0).
+ * @param [in] colorValue the concatenated RGB color value.
+ * @param [out] colorVector the color vector in RGB from 0.0-1.0.
+ * @return true if successful.
+ */
+bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3f& colorVector);
+
+/*!
+ * Transforms a float color value (concatenated 3 single-byte value) to a float color vector (RGB from 0.0-1.0).
+ * @param [in] colorValue the concatenated RGB color value.
+ * @param [out] colorVector the color vector in RGB from 0.0-1.0.
+ * @return true if successful.
+ */
+bool colorValueToVector(const float& colorValue, Eigen::Vector3f& colorVector);
+
+/*!
+ * Transforms an int color vector (RGB from 0-255) to a concatenated RGB int color.
+ * @param [in] colorVector the color vector in RGB from 0-255.
+ * @param [out] colorValue the concatenated RGB color value.
+ * @return true if successful.
+ */
+bool colorVectorToValue(const Eigen::Vector3i& colorVector, unsigned long& colorValue);
+
+/*!
+ * Transforms a color vector (RGB from 0-255) to a concatenated 3 single-byte float value.
+ * @param [in] colorVector the color vector in RGB from 0-255.
+ * @param [out] colorValue the concatenated RGB color value.
+ */
+void colorVectorToValue(const Eigen::Vector3i& colorVector, float& colorValue);
+
+/*!
+ * Transforms a color vector (RGB from 0.0-1.0) to a concatenated 3 single-byte float value.
+ * @param [in] colorVector the color vector in RGB from 0.0-1.0.
+ * @param [out] colorValue the concatenated RGB color value.
+ */
+void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue);
+
+}  // namespace grid_map

include/grid_map_core/Polygon.hpp

@@ -0,0 +1,249 @@
+/*
+ * Polygon.hpp
+ *
+ *  Created on: Nov 7, 2014
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include <grid_map_core/TypeDefs.hpp>
+
+// STD
+#include <vector>
+
+// Eigen
+#include <Eigen/Core>
+
+namespace grid_map {
+
+class Polygon
+{
+ public:
+
+  enum class TriangulationMethods {
+    FAN // Fan triangulation (only for convex polygons).
+  };
+
+  /*!
+   * Default constructor.
+   */
+  Polygon();
+
+  /*!
+   * Constructor with vertices.
+   * @param vertices the points of the polygon.
+   */
+  Polygon(std::vector<Position> vertices);
+
+  /*!
+   * Check if point is inside polygon.
+   * @param point the point to be checked.
+   * @return true if inside, false otherwise.
+   */
+  bool isInside(const Position& point) const;
+
+  /*!
+   * Add a vertex to the polygon
+   * @param vertex the point to be added.
+   */
+  void addVertex(const Position& vertex);
+
+  /*!
+   * Get the vertex with index.
+   * @param index the index of the requested vertex.
+   * @return the requested vertex.
+   */
+  const Position& getVertex(size_t index) const;
+
+  /*!
+   * Removes all vertices from the polygon.
+   */
+  void removeVertices();
+
+  /*!
+   * Get vertex operator overload.
+   * @param index the index of the requested vertex.
+   * @return the requested vertex.
+   */
+  const Position& operator [](size_t index) const;
+
+  /*!
+   * Returns the vertices of the polygon.
+   * @return the vertices of the polygon.
+   */
+  const std::vector<Position>& getVertices() const;
+
+  /*!
+   * Returns the number of vertices.
+   * @return the number of vertices.
+   */
+  size_t nVertices() const;
+
+  /*!
+   * Set the timestamp of the polygon.
+   * @param timestamp the timestamp to set (in  nanoseconds).
+   */
+  void setTimestamp(uint64_t timestamp);
+
+  /*!
+   * Get the timestamp of the polygon.
+   * @return timestamp in nanoseconds.
+   */
+  uint64_t getTimestamp() const;
+
+  /*!
+   * Resets the timestamp of the polygon (to zero).
+   */
+  void resetTimestamp();
+
+  /*!
+   * Set the frame id of the polygon.
+   * @param frameId the frame id to set.
+   */
+  void setFrameId(const std::string& frameId);
+
+  /*!
+   * Get the frameId of the polygon.
+   * @return frameId.
+   */
+  const std::string& getFrameId() const;
+
+  /*!
+   * Get the area of the polygon. The polygon has to be
+   * "simple", i.e. not crossing itself.
+   * @return area of the polygon.
+   */
+  double getArea() const;
+
+  /*!
+   * Get the centroid of polygon. The polygon has to be
+   * "simple", i.e. not crossing itself.
+   * @return centroid of polygon.
+   */
+  Position getCentroid() const;
+
+  /*!
+   * Gets the bounding box of the polygon.
+   * @param center the center of the bounding box.
+   * @param length the side lengths of the bounding box.
+   */
+  void getBoundingBox(Position& center, Length& length) const;
+
+  /*!
+   * Convert polygon to inequality constraints which most tightly contain the points; i.e.,
+   * create constraints to bound the convex hull of polygon. The inequality constraints are
+   * represented as A and b, a set of constraints such that A*x <= b defining the region of
+   * space enclosing the convex hull.
+   * Based on the VERT2CON MATLAB method by Michael Kleder:
+   * http://www.mathworks.com/matlabcentral/fileexchange/7895-vert2con-vertices-to-constraints
+   * @param A the A matrix in of the inequality constraint.
+   * @param b the b matrix in of the inequality constraint.
+   * @return true if conversion successful, false otherwise.
+   */
+  bool convertToInequalityConstraints(Eigen::MatrixXd& A,
+                                      Eigen::VectorXd& b) const;
+
+  /*!
+   * Offsets the polygon inward (buffering) by a margin.
+   * Use a negative margin to offset the polygon outward.
+   * @param margin the margin to offset the polygon by (in [m]).
+   * @return true if successful, false otherwise.
+   */
+  bool offsetInward(double margin);
+
+  /*!
+   * If only two vertices are given, this methods generates a
+   * `thickened` line polygon with four vertices.
+   * @param thickness the desired thickness of the line.
+   * @return true if successful, false otherwise.
+   */
+  bool thickenLine(double thickness);
+
+  /*!
+   * Return a triangulated version of the polygon.
+   * @return a list of triangle polygons covering the same polygon.
+   */
+  std::vector<Polygon> triangulate(const TriangulationMethods& method = TriangulationMethods::FAN) const;
+
+  /*!
+   * Approximates a circle with a polygon.
+   * @param[in] center the center position of the circle.
+   * @param[in] radius radius of the circle.
+   * @param[in] nVertices number of vertices of the approximation polygon. Default = 20.
+   * @return circle as polygon.
+   */
+  static Polygon fromCircle(Position center, double radius,
+                            int nVertices = 20);
+
+  /*!
+   * Approximates two circles with a convex hull and returns it as polygon.
+   * @param[in] center1 the center position of the first circle.
+   * @param[in] center2 the center position of the second circle.
+   * @param[in] radius radius of the circles.
+   * @param[in] nVertices number of vertices of the approximation polygon. Default = 20.
+   * @return convex hull of the two circles as polygon.
+   */
+  static Polygon convexHullOfTwoCircles(Position center1,
+                                        Position center2,
+                                        double radius,
+                                        int nVertices = 20);
+
+  /*!
+   * Computes the convex hull of two polygons and returns it as polygon.
+   * @param[in] polygon1 the first input polygon.
+   * @param[in] polygon2 the second input polygon.
+   * @return convex hull as polygon.
+   */
+  static Polygon convexHull(Polygon& polygon1, Polygon& polygon2);
+
+  /*!
+   * Computes the convex hull of given points, using Andrew's monotone chain convex hull algorithm, and returns it as polygon.
+   * @param[in] points points to use to compute the convex hull used to create the polygon.
+   * @return convex hull as polygon.
+   */
+  static Polygon monotoneChainConvexHullOfPoints(const std::vector<Position>& points);
+
+ protected:
+
+  /*!
+   * Returns true if the vector1 and vector2 are sorted lexicographically.
+   * @param[in] vector1 the first input vector.
+   * @param[in] vector2 the second input vector.
+   */
+  static bool sortVertices(const Eigen::Vector2d& vector1,
+                           const Eigen::Vector2d& vector2);
+
+  /*!
+   * Returns the 2D cross product of vector1 and vector2.
+   * @param[in] vector1 the first input vector.
+   * @param[in] vector2 the second input vector.
+   */
+  static double computeCrossProduct2D(const Eigen::Vector2d& vector1,
+                                      const Eigen::Vector2d& vector2);
+
+  /*!
+   * Returns true if OAB makes a clockwise turn or if the OA and OB vectors are collinear.
+   * @param[in] pointO point of the origin O, used to compute OA and OB.
+   * @param[in] pointA input point A, used to compute OA.
+   * @param[in] pointB input point B, used to compute OB.
+   */
+  static double vectorsMakeClockwiseTurn(const Eigen::Vector2d& pointO,
+                                         const Eigen::Vector2d& pointA,
+                                         const Eigen::Vector2d& pointB);
+  // NOLINTBEGIN(misc-non-private-member-variables-in-classes)
+  //! Frame id of the polygon.
+  std::string frameId_;
+
+  //! Timestamp of the polygon (nanoseconds).
+  uint64_t timestamp_;
+
+  //! Vertices of the polygon.
+  std::vector<Position> vertices_;
+  // NOLINTEND(misc-non-private-member-variables-in-classes)
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+} /* namespace grid_map */

include/grid_map_core/SubmapGeometry.hpp

@@ -0,0 +1,73 @@
+/*
+ * SubmapGeometry.hpp
+ *
+ *  Created on: Aug 18, 2015
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include <grid_map_core/GridMap.hpp>
+
+namespace grid_map {
+
+class GridMap;
+
+/*!
+ * This class holds information about the geometry of submap
+ * region of a grid map. Note that, this class does NOT hold
+ * the any data of the grid map.
+ */
+class SubmapGeometry
+{
+ public:
+
+  /*!
+   * Constructor. Note that the requested position and length
+   * of the submap is adapted to fit the geometry of the parent
+   * grid map.
+   * @param[in] gridMap the parent grid map containing the submap.
+   * @param[in] position the requested submap position (center).
+   * @param[in] length the requested submap length.
+   * @param[out] isSuccess true if successful, false otherwise.
+   */
+  SubmapGeometry(const GridMap& gridMap, const Position& position, const Length& length,
+                 bool& isSuccess);
+
+  virtual ~SubmapGeometry() = default;
+
+  const GridMap& getGridMap() const;
+  const Length& getLength() const;
+  const Position& getPosition() const;
+  const Index& getRequestedIndexInSubmap() const;
+  const Size& getSize() const;
+  double getResolution() const;
+  const Index& getStartIndex() const;
+
+ private:
+
+  //! Parent grid map of the submap.
+  const GridMap& gridMap_;
+
+  //! Start index (typically top left) index of the submap.
+  Index startIndex_;
+
+  //! Size of the submap.
+  Size size_;
+
+  //! Position (center) of the submap.
+  Position position_;
+
+  //! Length of the submap.
+  Length length_;
+
+  //! Index in the submap that corresponds to the requested
+  //! position of the submap.
+  Index requestedIndexInSubmap_;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+} /* namespace grid_map */

include/grid_map_core/TypeDefs.hpp

@@ -0,0 +1,47 @@
+/*
+ * TypeDefs.hpp
+ *
+ *  Created on: March 18, 2014
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+// Eigen
+#pragma once
+
+#include <Eigen/Core>
+
+namespace grid_map {
+
+  using Matrix = Eigen::MatrixXf;
+  using DataType = Matrix::Scalar;
+  using Position = Eigen::Vector2d;
+  using Vector = Eigen::Vector2d;
+  using Position3 = Eigen::Vector3d;
+  using Vector3 = Eigen::Vector3d;
+  using Index = Eigen::Array2i;
+  using Size = Eigen::Array2i;
+  using Length = Eigen::Array2d;
+  using Time = uint64_t;
+
+  /*
+   * Interpolations are ordered in the order
+   * of increasing accuracy and computational complexity.
+   * INTER_NEAREST - fastest, but least accurate,
+   * INTER_CUBIC - slowest, but the most accurate.
+   * see:
+   * https://en.wikipedia.org/wiki/Bicubic_interpolation
+   * https://web.archive.org/web/20051024202307/http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm
+   * for more info. Cubic convolution algorithm is also known as piecewise cubic
+   * interpolation and in general does not guarantee continuous
+   * first derivatives.
+   */
+  enum class InterpolationMethods{
+      INTER_NEAREST, // nearest neighbor interpolation
+      INTER_LINEAR,   // bilinear interpolation
+      INTER_CUBIC_CONVOLUTION, //piecewise bicubic interpolation using convolution algorithm
+      INTER_CUBIC // standard bicubic interpolation
+  };
+
+}  // namespace grid_map
+

include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp

@@ -0,0 +1,32 @@
+#pragma once
+
+Scalar numberOfFinites() const
+{
+  if (SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) {
+    return Scalar(0);
+  }
+  return Scalar((derived().array() == derived().array()).count());
+}
+
+Scalar sumOfFinites() const
+{
+  if (SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) {
+    return Scalar(0);
+  }
+  return Scalar(this->redux(Eigen::internal::scalar_sum_of_finites_op<Scalar>()));
+}
+
+Scalar meanOfFinites() const
+{
+  return Scalar(this->redux(Eigen::internal::scalar_sum_of_finites_op<Scalar>())) / this->numberOfFinites();
+}
+
+Scalar minCoeffOfFinites() const
+{
+  return Scalar(this->redux(Eigen::internal::scalar_min_of_finites_op<Scalar>()));
+}
+
+Scalar maxCoeffOfFinites() const
+{
+  return Scalar(this->redux(Eigen::internal::scalar_max_of_finites_op<Scalar>()));
+}

include/grid_map_core/eigen_plugins/Functors.hpp

@@ -0,0 +1,28 @@
+/*
+ * Functors.hpp
+ *
+ *  Created on: Nov 23, 2015
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+namespace grid_map {
+
+template<typename Scalar>
+struct Clamp
+{
+  Clamp(const Scalar& min, const Scalar& max)
+      : min_(min),
+        max_(max)
+  {
+  }
+  Scalar operator()(const Scalar& x) const
+  {
+    return x < min_ ? min_ : (x > max_ ? max_ : x);
+  }
+  Scalar min_, max_;
+};
+
+}  // namespace grid_map

include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp

@@ -0,0 +1,59 @@
+#include <cmath>
+
+template<typename Scalar> struct scalar_sum_of_finites_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_sum_of_finites_op)
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const {
+    using std::isfinite;
+    if (isfinite(a) && isfinite(b)) return a + b;
+    if (isfinite(a)) return a;
+    if (isfinite(b)) return b;
+    return a + b;
+  }
+};
+template<typename Scalar>
+struct functor_traits<scalar_sum_of_finites_op<Scalar> > {
+  enum {
+    Cost = 2 * NumTraits<Scalar>::ReadCost + NumTraits<Scalar>::AddCost,
+    PacketAccess = false
+  };
+};
+
+template<typename Scalar>
+struct scalar_min_of_finites_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_min_of_finites_op)
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const {
+    using std::min;
+    using std::isfinite;
+    if (isfinite(a) && isfinite(b)) return (min)(a, b);
+    if (isfinite(a)) return a;
+    if (isfinite(b)) return b;
+    return (min)(a, b);
+  }
+};
+template<typename Scalar>
+struct functor_traits<scalar_min_of_finites_op<Scalar> > {
+  enum {
+    Cost = NumTraits<Scalar>::AddCost,
+    PacketAccess = false
+  };
+};
+
+template<typename Scalar>
+struct scalar_max_of_finites_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_max_of_finites_op)
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const {
+    using std::max;
+    using std::isfinite;
+    if (isfinite(a) && isfinite(b)) return (max)(a, b);
+    if (isfinite(a)) return a;
+    if (isfinite(b)) return b;
+    return (max)(a, b);
+  }
+};
+template<typename Scalar>
+struct functor_traits<scalar_max_of_finites_op<Scalar> > {
+  enum {
+    Cost = NumTraits<Scalar>::AddCost,
+    PacketAccess = false
+  };
+};

include/grid_map_core/grid_map_core.hpp

@@ -0,0 +1,18 @@
+/*
+ * grid_map_core.hpp
+ *
+ *  Created on: Jul 14, 2014
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include "grid_map_core/TypeDefs.hpp"
+#include "grid_map_core/GridMap.hpp"
+#include "grid_map_core/SubmapGeometry.hpp"
+#include "grid_map_core/GridMapMath.hpp"
+#include "grid_map_core/BufferRegion.hpp"
+#include "grid_map_core/Polygon.hpp"
+#include "grid_map_core/iterators/iterators.hpp"
+#include "grid_map_core/eigen_plugins/Functors.hpp"

include/grid_map_core/gtest_eigen.hpp

@@ -0,0 +1,165 @@
+/**
+ * @file   gtest.hpp
+ * @author Paul Furgale <paul.furgale@gmail.com>
+ * @date   Mon Dec 12 11:54:20 2011
+ * @brief  Code to simplify Eigen integration into GTest. Pretty basic but the error messages are good.
+ */
+
+#pragma once
+
+#include <gtest/gtest.h>
+#include <Eigen/Core>
+#include <iostream>
+#include <cmath>
+
+namespace grid_map {
+
+    template<int N>
+    Eigen::Matrix<double,N,N> randomCovariance()
+    {
+      Eigen::Matrix<double,N,N> U;
+      U.setRandom();
+      return U.transpose() * U + 5.0 * Eigen::Matrix<double,N,N>::Identity();
+    }
+
+    inline Eigen::MatrixXd randomCovarianceXd(int N)
+    {
+        Eigen::MatrixXd U(N,N);
+        U.setRandom();
+        return U.transpose() * U + 5.0 * Eigen::MatrixXd::Identity(N,N);
+    }
+
+    template<typename M1, typename M2>
+    void assertEqual(const M1 & A, const M2 & B, std::string const & message = "")
+    {
+        ASSERT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n";
+      ASSERT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n";
+
+      for(int r = 0; r < A.rows(); r++)
+		{
+		  for(int c = 0; c < A.cols(); c++)
+			{
+		    if (std::isnan(A(r,c))) {
+		      ASSERT_TRUE(std::isnan(B(r,c))) << message << "\nNaN check failed at (" << r << "," << c << ")\n"
+                                  << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
+		    } else {
+			    ASSERT_EQ(A(r,c),B(r,c)) << message << "\nEquality comparison failed at (" << r << "," << c << ")\n"
+                                       << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
+		    }
+			}
+		}
+    }
+
+    template<typename M1, typename M2, typename T>
+    void assertNear(const M1 & A, const M2 & B, T tolerance, std::string const & message = "")
+    {
+      // Note: If these assertions fail, they only abort this subroutine.
+      // see: http://code.google.com/p/googletest/wiki/AdvancedGuide#Using_Assertions_in_Sub-routines
+      // \todo better handling of this
+        ASSERT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n";
+      ASSERT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n";
+
+      for(int r = 0; r < A.rows(); r++)
+		{
+		  for(int c = 0; c < A.cols(); c++)
+			{
+		    if (std::isnan(A(r,c))) {
+		      ASSERT_TRUE(std::isnan(B(r,c))) << message << "\nNaN check failed at (" << r << "," << c << ")\n"
+                                  << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
+		    } else {
+			    ASSERT_NEAR(A(r,c),B(r,c),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n"
+												   << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
+		    }
+			}
+		}
+    }
+
+    template<typename M1, typename M2, typename T>
+    void expectNear(const M1 & A, const M2 & B, T tolerance, std::string const & message = "")
+    {
+      EXPECT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n";
+      EXPECT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n";
+
+      for(int r = 0; r < A.rows(); r++)
+        {
+          for(int c = 0; c < A.cols(); c++)
+            {
+              if (std::isnan(A(r,c))) {
+                EXPECT_TRUE(std::isnan(B(r,c))) << message << "\nNaN check failed at (" << r << "," << c << ")\n"
+                            << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
+              } else {
+                EXPECT_NEAR(A(r,c),B(r,c),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n"
+                             << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
+              }
+            }
+        }
+    }
+
+    template<typename M1>
+    void assertFinite(const M1 & A, std::string const &  /*message*/ = "")
+    {
+      std::cout << ("test") << std::endl;
+      for(int r = 0; r < A.rows(); r++)
+		{
+		  for(int c = 0; c < A.cols(); c++)
+			{
+			  ASSERT_TRUE(std::isfinite(A(r,c))) << std::endl << "Check for finite values failed at A(" << r << "," << c << "). Matrix A:" << std::endl << A << std::endl;
+			}
+		}
+    }
+
+    inline bool compareRelative(double a, double b, double percentTolerance, double* percentError = nullptr) {
+      // \todo: does anyone have a better idea?
+      double fa = fabs(a);
+      double fb = fabs(b);
+      if ((fa < 1e-15 && fb < 1e-15) ||  // Both zero.
+          (fa == 0.0 && fb < 1e-6) ||    // One exactly zero and the other small
+          (fb == 0.0 && fa < 1e-6)) {    // ditto
+        return true;
+      }
+
+      double diff = fabs(a - b) / std::max(fa, fb);
+      if (diff > percentTolerance * 1e-2) {
+        if (percentError != nullptr) {
+          *percentError = diff * 100.0;
+        }
+        return false;
+      }
+      return true;
+    }
+
+#define ASSERT_DOUBLE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG)				\
+    ASSERT_EQ((size_t)(A).rows(), (size_t)(B).rows())  << (MSG) << "\nMatrix " << #A << ":\n" << (A) << "\nand matrix " << #B << "\n" << (B) << "\nare not the same size"; \
+    ASSERT_EQ((size_t)(A).cols(), (size_t)(B).cols())  << (MSG) << "\nMatrix " << #A << ":\n" << (A) << "\nand matrix " << #B << "\n" << (B) << "\nare not the same size"; \
+    for(int r = 0; r < (A).rows(); r++)									\
+      {																	\
+		for(int c = 0; c < (A).cols(); c++)								\
+		  {																\
+			double percentError = 0.0;									\
+			ASSERT_TRUE(grid_map::compareRelative( (A)(r,c), (B)(r,c), PERCENT_TOLERANCE, &percentError)) \
+			  << (MSG) << "\nComparing:\n"								\
+			  << #A << "(" << r << "," << c << ") = " << (A)(r,c) << std::endl \
+			  << #B << "(" << r << "," << c << ") = " << (B)(r,c) << std::endl \
+			  << "Error was " << percentError << "% > " << (PERCENT_TOLERANCE) << "%\n" \
+			  << "\nMatrix " << #A << ":\n" << (A) << "\nand matrix " << #B << "\n" << (B); \
+		  }																\
+      }
+
+#define ASSERT_DOUBLE_SPARSE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG)       \
+    ASSERT_EQ((size_t)(A).rows(), (size_t)(B).rows())  << (MSG) << "\nMatrix " << #A << ":\n" << (A) << "\nand matrix " << #B << "\n" << (B) << "\nare not the same size"; \
+    ASSERT_EQ((size_t)(A).cols(), (size_t)(B).cols())  << (MSG) << "\nMatrix " << #A << ":\n" << (A) << "\nand matrix " << #B << "\n" << (B) << "\nare not the same size"; \
+    for(int r = 0; r < (A).rows(); r++)                 \
+      {                                 \
+    for(int c = 0; c < (A).cols(); c++)               \
+      {                               \
+      double percentError = 0.0;                  \
+      ASSERT_TRUE(grid_map::compareRelative( (A).coeff(r,c), (B).coeff(r,c), PERCENT_TOLERANCE, &percentError)) \
+        << (MSG) << "\nComparing:\n"                \
+        << #A << "(" << r << "," << c << ") = " << (A).coeff(r,c) << std::endl \
+        << #B << "(" << r << "," << c << ") = " << (B).coeff(r,c) << std::endl \
+        << "Error was " << percentError << "% > " << (PERCENT_TOLERANCE) << "%\n" \
+        << "\nMatrix " << #A << ":\n" << (A) << "\nand matrix " << #B << "\n" << (B); \
+      }                               \
+      }
+
+}  // namespace grid_map

include/grid_map_core/iterators/CircleIterator.hpp

@@ -0,0 +1,101 @@
+/*
+ * CircleIterator.hpp
+ *
+ *  Created on: Nov 13, 2014
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include "grid_map_core/GridMap.hpp"
+#include "grid_map_core/iterators/SubmapIterator.hpp"
+
+#include <Eigen/Core>
+
+#include <memory>
+
+namespace grid_map {
+
+/*!
+ * Iterator class to iterate through a circular area of the map.
+ */
+class CircleIterator
+{
+public:
+
+  /*!
+   * Constructor.
+   * @param gridMap the grid map to iterate on.
+   * @param center the position of the circle center.
+   * @param radius the radius of the circle.
+   */
+  CircleIterator(const GridMap& gridMap, const Position& center, double radius);
+
+  /*!
+   * Compare to another iterator.
+   * @return whether the current iterator points to a different address than the other one.
+   */
+  bool operator !=(const CircleIterator& other) const;
+
+  /*!
+   * Dereference the iterator with const.
+   * @return the value to which the iterator is pointing.
+   */
+  const Index& operator *() const;
+
+  /*!
+   * Increase the iterator to the next element.
+   * @return a reference to the updated iterator.
+   */
+  CircleIterator& operator ++();
+
+  /*!
+   * Indicates if iterator is past end.
+   * @return true if iterator is out of scope, false if end has not been reached.
+   */
+  bool isPastEnd() const;
+
+private:
+
+  /*!
+   * Check if current index is inside the circle.
+   * @return true if inside, false otherwise.
+   */
+  bool isInside() const;
+
+  /*!
+   * Finds the submap that fully contains the circle and returns the parameters.
+   * @param[in] center the position of the circle center.
+   * @param[in] radius the radius of the circle.
+   * @param[out] startIndex the start index of the submap.
+   * @param[out] bufferSize the buffer size of the submap.
+   */
+  void findSubmapParameters(const Position& center, double radius,
+                            Index& startIndex, Size& bufferSize) const;
+
+  //! Position of the circle center;
+  Position center_;
+
+  //! Radius of the circle.
+  double radius_;
+
+  //! Square of the radius (for efficiency).
+  double radiusSquare_;
+
+  //! Grid submap iterator. // TODO Think of using unique_ptr instead.
+  std::shared_ptr<SubmapIterator> internalIterator_;
+
+  //! Map information needed to get position from iterator.
+  Length mapLength_;
+  Position mapPosition_;
+  double resolution_;
+  Size bufferSize_;
+  Index bufferStartIndex_;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace grid_map
+

include/grid_map_core/iterators/EllipseIterator.hpp

@@ -0,0 +1,110 @@
+/*
+ * EllipseIterator.hpp
+ *
+ *  Created on: Dec 2, 2015
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include "grid_map_core/GridMap.hpp"
+#include "grid_map_core/iterators/SubmapIterator.hpp"
+
+#include <Eigen/Core>
+
+#include <memory>
+
+namespace grid_map {
+
+/*!
+ * Iterator class to iterate through a ellipsoid area of the map.
+ * The main axis of the ellipse are aligned with the map frame.
+ */
+class EllipseIterator
+{
+public:
+
+  /*!
+   * Constructor.
+   * @param gridMap the grid map to iterate on.
+   * @param center the position of the ellipse center.
+   * @param length the length of the main axis.
+   * @param angle the rotation angle of the ellipse (in [rad]).
+   */
+  EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, double rotation = 0.0);
+
+  /*!
+   * Compare to another iterator.
+   * @return whether the current iterator points to a different address than the other one.
+   */
+  bool operator !=(const EllipseIterator& other) const;
+
+  /*!
+   * Dereference the iterator with const.
+   * @return the value to which the iterator is pointing.
+   */
+  const Index& operator *() const;
+
+  /*!
+   * Increase the iterator to the next element.
+   * @return a reference to the updated iterator.
+   */
+  EllipseIterator& operator ++();
+
+  /*!
+   * Indicates if iterator is past end.
+   * @return true if iterator is out of scope, false if end has not been reached.
+   */
+  bool isPastEnd() const;
+
+  /*!
+   * Returns the size of the submap covered by the iterator.
+   * @return the size of the submap covered by the iterator.
+   */
+  const Size& getSubmapSize() const;
+
+private:
+
+  /*!
+   * Check if current index is inside the ellipse.
+   * @return true if inside, false otherwise.
+   */
+  bool isInside() const;
+
+  /*!
+   * Finds the submap that fully contains the ellipse and returns the parameters.
+   * @param[in] center the position of the ellipse center.
+   * @param[in] length the length of the main axis.
+   * @param[in] angle the rotation angle of the ellipse (in [rad]).
+   * @param[out] startIndex the start index of the submap.
+   * @param[out] bufferSize the buffer size of the submap.
+   */
+  void findSubmapParameters(const Position& center, const Length& length, double rotation,
+                            Index& startIndex, Size& bufferSize) const;
+
+  //! Position of the circle center;
+  Position center_;
+
+  //! Square length of the semi axis.
+  Eigen::Array2d semiAxisSquare_;
+
+  //! Sine and cosine values of the rotation angle as transformation matrix.
+  Eigen::Matrix2d transformMatrix_;
+
+  //! Grid submap iterator. // TODO Think of using unique_ptr instead.
+  std::shared_ptr<SubmapIterator> internalIterator_;
+
+  //! Map information needed to get position from iterator.
+  Length mapLength_;
+  Position mapPosition_;
+  double resolution_;
+  Size bufferSize_;
+  Index bufferStartIndex_;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace grid_map
+

include/grid_map_core/iterators/GridMapIterator.hpp

@@ -0,0 +1,105 @@
+/*
+ * GridMapIterator.hpp
+ *
+ *  Created on: Sep 22, 2014
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include "grid_map_core/GridMap.hpp"
+
+// Eigen
+#include <Eigen/Core>
+
+namespace grid_map {
+
+/*!
+ * Iterator class to iterate trough the entire grid map.
+ */
+class GridMapIterator
+{
+public:
+
+  /*!
+   * Constructor.
+   * @param gridMap the grid map to iterate on.
+   */
+  GridMapIterator(const grid_map::GridMap &gridMap);
+
+  /*!
+   * Copy constructor.
+   * @param other the object to copy.
+   */
+  GridMapIterator(const GridMapIterator* other);
+
+  /*!
+   * Compare to another iterator.
+   * @return whether the current iterator points to a different address than the other one.
+   */
+  bool operator !=(const GridMapIterator& other) const;
+
+  /*!
+   * Dereference the iterator to return the regular index (2-dim.) of the cell
+   * to which the iterator is pointing at.
+   * @return the regular index (2-dim.) of the cell on which the iterator is pointing.
+   */
+  Index operator *() const;
+
+  /*!
+   * Returns the the linear (1-dim.) index of the cell the iterator is pointing at.
+   * Note: Use this access for improved efficiency when working with large maps.
+   * Example: See `runGridMapIteratorVersion3()` of `grid_map_demos/src/iterator_benchmark.cpp`.
+   * @return the 1d linear index.
+   */
+  const size_t& getLinearIndex() const;
+
+  /*!
+   * Retrieve the index as unwrapped index, i.e., as the corresponding index of a
+   * grid map with no circular buffer offset.
+   */
+  Index getUnwrappedIndex() const;
+
+  /*!
+   * Increase the iterator to the next element.
+   * @return a reference to the updated iterator.
+   */
+  virtual GridMapIterator& operator ++();
+
+  /*!
+   * Return the end iterator
+   * @return the end iterator (useful when performing normal iterator processing with ++).
+   */
+  GridMapIterator end() const;
+
+  /*!
+   * Indicates if iterator is past end.
+   * @return true if iterator is out of scope, false if end has not been reached.
+   */
+  bool isPastEnd() const;
+
+ protected:
+  // NOLINTBEGIN(misc-non-private-member-variables-in-classes)
+  //! Size of the buffer.
+  Size size_;
+
+  //! Start index of the circular buffer.
+  Index startIndex_;
+
+  //! Linear size of the data.
+  size_t linearSize_;
+
+  //! Linear index.
+  size_t linearIndex_;
+
+  //! Is iterator out of scope.
+  bool isPastEnd_;
+  // NOLINTEND(misc-non-private-member-variables-in-classes)
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace grid_map
+

include/grid_map_core/iterators/LineIterator.hpp

@@ -0,0 +1,126 @@
+/*
+ * LineIterator.hpp
+ *
+ *  Created on: Nov 13, 2014
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include "grid_map_core/GridMap.hpp"
+#include "grid_map_core/iterators/SubmapIterator.hpp"
+
+#include <Eigen/Core>
+
+namespace grid_map {
+
+/*!
+ * Iterator class to iterate over a line in the map.
+ * Based on Bresenham Line Drawing algorithm.
+ */
+class LineIterator
+{
+public:
+
+  /*!
+   * Constructor.
+   * @param gridMap the grid map to iterate on.
+   * @param start the starting point of the line.
+   * @param end the ending point of the line.
+   * @throw std::invalid_argument if start and end impose an ill conditioned line iteration.
+   */
+  LineIterator(const grid_map::GridMap& gridMap, const Position& start, const Position& end);
+
+  /*!
+   * Constructor.
+   * @param gridMap the grid map to iterate on.
+   * @param start the starting index of the line.
+   * @param end the ending index of the line.
+   */
+  LineIterator(const grid_map::GridMap& gridMap, const Index& start, const Index& end);
+
+  /*!
+   * Compare to another iterator.
+   * @return whether the current iterator points to a different address than the other one.
+   */
+  bool operator !=(const LineIterator& other) const;
+
+  /*!
+   * Dereference the iterator with const.
+   * @return the value to which the iterator is pointing.
+   */
+  const Index& operator *() const;
+
+  /*!
+   * Increase the iterator to the next element.
+   * @return a reference to the updated iterator.
+   */
+  LineIterator& operator ++();
+
+  /*!
+   * Indicates if iterator is past end.
+   * @return true if iterator is out of scope, false if end has not been reached.
+   */
+  bool isPastEnd() const;
+
+private:
+
+
+  /*!
+   * Construct function.
+   * @param gridMap the grid map to iterate on.
+   * @param start the starting index of the line.
+   * @param end the ending index of the line.
+   * @return true if successful, false otherwise.
+   */
+  bool initialize(const grid_map::GridMap& gridMap, const Index& start, const Index& end);
+
+  /*!
+   * Computes the parameters requires for the line drawing algorithm.
+   */
+  void initializeIterationParameters();
+
+  /*!
+   * Finds the index of a position on a line within the limits of the map.
+   * @param[in] gridMap the grid map that defines the map boundaries.
+   * @param[in] start the position that will be limited to the map range.
+   * @param[in] end the ending position of the line.
+   * @param[out] index the index of the moved start position.
+   * @return true if successful, false otherwise.
+   */
+  static bool getIndexLimitedToMapRange(const grid_map::GridMap& gridMap, const Position& start,
+                                 const Position& end, Index& index);
+
+  //! Current index.
+  Index index_;
+
+  //! Starting index of the line.
+  Index start_;
+
+  //! Ending index of the line.
+  Index end_;
+
+  //! Current cell number.
+  unsigned int iCell_ = 0;
+
+  //! Number of cells in the line.
+  unsigned int nCells_ = 0;
+
+  //! Helper variables for Bresenham Line Drawing algorithm.
+  Size increment1_, increment2_;
+  int denominator_{0}, numerator_{0}, numeratorAdd_{0};
+
+  //! Map information needed to get position from iterator.
+  Length mapLength_;
+  Position mapPosition_;
+  double resolution_{NAN};
+  Size bufferSize_;
+  Index bufferStartIndex_;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace grid_map
+

include/grid_map_core/iterators/PolygonIterator.hpp

@@ -0,0 +1,91 @@
+/*
+ * PolygonIterator.hpp
+ *
+ *  Created on: Sep 19, 2014
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include "grid_map_core/GridMap.hpp"
+#include "grid_map_core/Polygon.hpp"
+#include "grid_map_core/iterators/SubmapIterator.hpp"
+
+#include <memory>
+
+namespace grid_map {
+
+/*!
+ * Iterator class to iterate through a polygonal area of the map.
+ */
+class PolygonIterator
+{
+public:
+
+  /*!
+   * Constructor.
+   * @param gridMap the grid map to iterate on.
+   * @param polygon the polygonal area to iterate on.
+   */
+  PolygonIterator(const grid_map::GridMap& gridMap, const grid_map::Polygon& polygon);
+
+  /*!
+   * Compare to another iterator.
+   * @return whether the current iterator points to a different address than the other one.
+   */
+  bool operator !=(const PolygonIterator& other) const;
+
+  /*!
+   * Dereference the iterator with const.
+   * @return the value to which the iterator is pointing.
+   */
+  const Index& operator *() const;
+
+  /*!
+   * Increase the iterator to the next element.
+   * @return a reference to the updated iterator.
+   */
+  PolygonIterator& operator ++();
+
+  /*!
+   * Indicates if iterator is past end.
+   * @return true if iterator is out of scope, false if end has not been reached.
+   */
+  bool isPastEnd() const;
+
+private:
+
+  /*!
+   * Check if current index is inside polygon.
+   * @return true if inside, false otherwise.
+   */
+  bool isInside() const;
+
+  /*!
+   * Finds the submap that fully contains the polygon and returns the parameters.
+   * @param[in] polygon the polygon to get the submap for.
+   * @param[out] startIndex the start index of the submap.
+   * @param[out] bufferSize the buffer size of the submap.
+   */
+  void findSubmapParameters(const grid_map::Polygon& polygon, Index& startIndex,Size& bufferSize) const;
+
+  //! Polygon to iterate on.
+  grid_map::Polygon polygon_;
+
+  //! Grid submap iterator.
+  std::shared_ptr<SubmapIterator> internalIterator_;
+
+  //! Map information needed to get position from iterator.
+  Length mapLength_;
+  Position mapPosition_;
+  double resolution_;
+  Size bufferSize_;
+  Index bufferStartIndex_;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace grid_map
+

include/grid_map_core/iterators/SlidingWindowIterator.hpp

@@ -0,0 +1,95 @@
+/*
+ * SlidingWindowIterator.hpp
+ *
+ *  Created on: Aug 17, 2017
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include "grid_map_core/GridMap.hpp"
+
+#include "grid_map_core/iterators/GridMapIterator.hpp"
+
+#include <Eigen/Core>
+
+namespace grid_map {
+
+/*!
+ * Iterator class to iterate trough the entire grid map with access to a layer's
+ * data through a sliding window.
+ * Note: This iterator only works with maps with zero start index.
+ */
+class SlidingWindowIterator : public GridMapIterator
+{
+public:
+
+  enum class EdgeHandling {
+    INSIDE, // Only visit indices that are surrounded by a full window.
+    CROP, // Crop data matrix with missing cells at edges.
+    EMPTY, // Fill in missing edges with empty cells (NAN-value).
+    MEAN // Fill in missing edges with MEAN of valid values.
+  };
+
+  /*!
+   * Constructor.
+   * @param gridMap the grid map to iterate on.
+   * @param layer the layer on which the data is accessed.
+   * @param edgeHandling the method to handle edges of the map.
+   * @param windowSize the size of the moving window in number of cells (has to be an odd number!).
+   */
+  SlidingWindowIterator(const GridMap& gridMap, const std::string& layer,
+                        const EdgeHandling& edgeHandling = EdgeHandling::CROP,
+                        size_t windowSize = 3);
+
+  /*!
+   * Copy constructor.
+   * @param other the object to copy.
+   */
+  explicit SlidingWindowIterator(const SlidingWindowIterator* other);
+
+  /*!
+   * Set the side length of the moving window (in m).
+   * @param gridMap the grid map to iterate on.
+   * @param windowLength the side length of the window (in m).
+   */
+  void setWindowLength(const GridMap& gridMap, double windowLength);
+
+  /*!
+   * Increase the iterator to the next element.
+   * @return a reference to the updated iterator.
+   */
+  SlidingWindowIterator& operator ++() override;
+
+  /*!
+   * Return the data of the sliding window.
+   * @return the data of the sliding window.
+   */
+  Matrix getData() const;
+
+private:
+  //! Setup members.
+  void setup(const GridMap& gridMap);
+
+  //! Check if data for current index is fully inside map.
+  bool dataInsideMap() const;
+
+  //! Edge handling method.
+  const EdgeHandling edgeHandling_;
+
+  //! Data.
+  const Matrix& data_;
+
+  //! Size of the sliding window.
+  size_t windowSize_;
+
+  //! Size of the border of the window around the center cell.
+  size_t windowMargin_{0};
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace grid_map
+

include/grid_map_core/iterators/SpiralIterator.hpp

@@ -0,0 +1,109 @@
+/*
+ * SpiralIterator.hpp
+ *
+ *  Created on: Jul 7, 2015
+ *      Author: Martin Wermelinger
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include "grid_map_core/GridMap.hpp"
+
+#include <Eigen/Core>
+#include <memory>
+#include <vector>
+
+namespace grid_map {
+
+/*!
+ * Iterator class to iterate through a circular area of the map with a spiral.
+ */
+class SpiralIterator
+{
+public:
+
+  /*!
+   * Constructor.
+   * @param gridMap the grid map to iterate on.
+   * @param center the position of the circle center.
+   * @param radius the radius of the circle.
+   */
+  SpiralIterator(const grid_map::GridMap& gridMap, Eigen::Vector2d  center, double radius);
+
+  /*!
+   * Compare to another iterator.
+   * @return whether the current iterator points to a different address than the other one.
+   */
+  bool operator !=(const SpiralIterator& other) const;
+
+  /*!
+   * Dereference the iterator with const.
+   * @return the value to which the iterator is pointing.
+   */
+  const Eigen::Array2i& operator *() const;
+
+  /*!
+   * Increase the iterator to the next element.
+   * @return a reference to the updated iterator.
+   */
+  SpiralIterator& operator ++();
+
+  /*!
+   * Indicates if iterator is past end.
+   * @return true if iterator is out of scope, false if end has not been reached.
+   */
+  bool isPastEnd() const;
+
+  /*!
+   * Gets the radius of current ring that is iterated through.
+   * @return radius of the current ring that is used for iteration.
+   */
+  double getCurrentRadius() const;
+
+private:
+
+  /*!
+   * Check if index is inside the circle.
+   * @return true if inside, false otherwise.
+   */
+  bool isInside(const Index& index) const;
+
+  /*!
+   * Uses the current distance to get the points of a ring
+   * around the center.
+   */
+  void generateRing();
+
+  static int signum(const int val) {
+      return static_cast<int>(0 < val) - static_cast<int>(val < 0);
+  }
+
+  //! Position of the circle center;
+  Position center_;
+  Index indexCenter_;
+
+
+  //! Radius of the circle.
+  double radius_;
+
+  //! Square of the radius for efficiency.
+  double radiusSquare_;
+
+  //! Number of rings into the circle is divided.
+  unsigned int nRings_;
+  unsigned int distance_;
+  std::vector<Index> pointsRing_;
+
+  //! Map information needed to get position from iterator.
+  Length mapLength_;
+  Position mapPosition_;
+  double resolution_;
+  Size bufferSize_;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace grid_map
+

include/grid_map_core/iterators/SubmapIterator.hpp

@@ -0,0 +1,119 @@
+/*
+ * SubmapIterator.hpp
+ *
+ *  Created on: Sep 12, 2014
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include "grid_map_core/GridMap.hpp"
+#include "grid_map_core/SubmapGeometry.hpp"
+#include "grid_map_core/BufferRegion.hpp"
+
+#include <Eigen/Core>
+
+namespace grid_map {
+
+/*!
+ * Iterator class to iterate through a rectangular part of the map (submap).
+ * Before using this iterator, make sure that the requested submap is
+ * actually contained in the grid map.
+ */
+class SubmapIterator
+{
+public:
+
+  /*!
+   * Constructor.
+   * @param submap the submap geometry to iterate over.
+   */
+  SubmapIterator(const grid_map::SubmapGeometry& submap);
+
+  /*!
+   * Constructor.
+   * @param submap the buffer region of a grid map to iterate over.
+   */
+  SubmapIterator(const grid_map::GridMap& gridMap, const grid_map::BufferRegion& bufferRegion);
+
+  /*!
+   * Constructor.
+   * @param gridMap the grid map to iterate on.
+   * @param submapStartIndex the start index of the submap, typically top-left index.
+   * @param submapSize the size of the submap to iterate on.
+   */
+  SubmapIterator(const grid_map::GridMap& gridMap, const Index& submapStartIndex,
+                 const Size& submapSize);
+
+  /*!
+   * Copy constructor.
+   * @param other the object to copy.
+   */
+  SubmapIterator(const SubmapIterator* other);
+
+  /*!
+   * Compare to another iterator.
+   * @return whether the current iterator points to a different address than the other one.
+   */
+  bool operator !=(const SubmapIterator& other) const;
+
+  /*!
+   * Dereference the iterator with const.
+   * @return the value to which the iterator is pointing.
+   */
+  const Index& operator *() const;
+
+  /*!
+   * Get the current index in the submap.
+   * @return the current index in the submap.
+   */
+  const Index& getSubmapIndex() const;
+
+  /*!
+   * Increase the iterator to the next element.
+   * @return a reference to the updated iterator.
+   */
+  SubmapIterator& operator ++();
+
+  /*!
+   * Indicates if iterator is past end.
+   * @return true if iterator is out of scope, false if end has not been reached.
+   */
+  bool isPastEnd() const;
+
+  /*!
+   * Returns the size of the submap covered by the iterator.
+   * @return the size of the submap covered by the iterator.
+   */
+  const Size& getSubmapSize() const;
+
+private:
+
+  //! Size of the buffer.
+  Size size_;
+
+  //! Start index of the circular buffer.
+  Index startIndex_;
+
+  //! Current index.
+  Index index_;
+
+  //! Submap buffer size.
+  Size submapSize_;
+
+  //! Top left index of the submap.
+  Index submapStartIndex_;
+
+  //! Current index in the submap.
+  Index submapIndex_;
+
+  //! Is iterator out of scope.
+  bool isPastEnd_;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace grid_map
+

include/grid_map_core/iterators/iterators.hpp

@@ -0,0 +1,18 @@
+/*
+ * iterators.hpp
+ *
+ *  Created on: Sep 22, 2014
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include "grid_map_core/iterators/GridMapIterator.hpp"
+#include "grid_map_core/iterators/SubmapIterator.hpp"
+#include "grid_map_core/iterators/CircleIterator.hpp"
+#include "grid_map_core/iterators/EllipseIterator.hpp"
+#include "grid_map_core/iterators/SpiralIterator.hpp"
+#include "grid_map_core/iterators/LineIterator.hpp"
+#include "grid_map_core/iterators/PolygonIterator.hpp"
+#include "grid_map_core/iterators/SlidingWindowIterator.hpp"

include/grid_map_core/utils/testing.hpp

@@ -0,0 +1,37 @@
+/**
+ * Copied from ANYbotics/eigen_utils, to avoid testing dependency.
+ */
+
+#pragma once
+
+#include <gtest/gtest.h>
+#include <Eigen/Core>
+
+#define ASSERT_MATRICES_EQ_WITH_NAN(first, second) assertMatrixesEqualWithNan((first), #first, (second), #second, __LINE__)
+static void assertMatrixesEqualWithNan(Eigen::Ref<const Eigen::MatrixXf> first, std::string firstName,
+                                       Eigen::Ref<const Eigen::MatrixXf> second, std::string secondName, int line) {
+  ASSERT_EQ(first.rows(), second.rows());
+  ASSERT_EQ(first.cols(), second.cols());
+
+  bool matricesAreEqual = true;
+  for (Eigen::Index row = 0; row < first.rows() && matricesAreEqual; ++row) {
+    for (Eigen::Index col = 0; col < first.cols() && matricesAreEqual; ++col) {
+      bool ifRealThenValid = first.block<1, 1>(row, col).isApprox(second.block<1, 1>(row, col));
+      bool bothNaN = std::isnan(first(row, col)) && std::isnan(second(row, col));
+      if (ifRealThenValid || bothNaN) {
+        continue;
+      } else {
+        matricesAreEqual = false;
+      }
+    }
+  }
+
+  Eigen::IOFormat compactFormat(2, 0, ",", "\n", "[", "]");
+  ASSERT_TRUE(matricesAreEqual)                                       // NO LINT
+      << "L. " << std::to_string(line) << ": Matrices are not equal"  // NO LINT
+      << "\n"                                                         // NO LINT
+      << firstName << "\n"                                            // NO LINT
+      << first.format(compactFormat) << "\n"                          // NO LINT
+      << secondName << "\n"                                           // NO LINT
+      << second.format(compactFormat) << "\n";                        // NO LINT
+}

package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>grid_map_core</name>
+  <version>1.7.18</version>
+  <description>Universal grid map library to manage two-dimensional grid maps with multiple data layers.</description>
+  <maintainer email="mwulf@anybotics.com">Maximilian Wulf</maintainer>
+  <maintainer email="ynava@anybotics.com">Yoshua Nava</maintainer>
+  <license>BSD</license>
+  <url type="website">http://github.com/anybotics/grid_map</url>
+  <url type="bugtracker">http://github.com/anybotics/grid_map/issues</url>
+  <author email="pfankhauser@anybotics.com">Péter Fankhauser</author>
+  <buildtool_depend>catkin</buildtool_depend>
+<!--   <build_depend>cmake_clang_tools</build_depend> -->
+  <depend>eigen</depend>
+<!--   <test_depend>cmake_code_coverage</test_depend> -->
+  <test_depend>gtest</test_depend>
+</package>

src/BufferRegion.cpp

@@ -0,0 +1,56 @@
+/*
+ * BufferRegion.cpp
+ *
+ *  Created on: Aug 19, 2015
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+#include <grid_map_core/BufferRegion.hpp>
+
+namespace grid_map {
+
+BufferRegion::BufferRegion() : startIndex_(Index::Zero()),
+    size_(Size::Zero()),
+    quadrant_(BufferRegion::Quadrant::Undefined)
+{
+}
+
+BufferRegion::BufferRegion(Index index, Size size, BufferRegion::Quadrant quadrant) : startIndex_(std::move(index)),
+    size_(std::move(size)),
+    quadrant_(std::move(quadrant))
+{
+}
+
+const Index& BufferRegion::getStartIndex() const
+{
+  return startIndex_;
+}
+
+void BufferRegion::setStartIndex(const Index& startIndex)
+{
+  startIndex_ = startIndex;
+}
+
+const Size& BufferRegion::getSize() const
+{
+  return size_;
+}
+
+void BufferRegion::setSize(const Size& size)
+{
+  size_ = size;
+}
+
+BufferRegion::Quadrant BufferRegion::getQuadrant() const
+{
+  return quadrant_;
+}
+
+void BufferRegion::setQuadrant(BufferRegion::Quadrant type)
+{
+  quadrant_ = type;
+}
+
+} /* namespace grid_map */
+
+

src/CubicInterpolation.cpp

@@ -0,0 +1,436 @@
+/*
+ * CubicInterpolation.cpp
+ *
+ *  Created on: Jan 21, 2020
+ *      Author: Edo Jelavic
+ *      Institute: ETH Zurich, Robotic Systems Lab
+ */
+
+#include "grid_map_core/CubicInterpolation.hpp"
+
+#include "grid_map_core/GridMap.hpp"
+
+namespace grid_map {
+
+unsigned int bindIndexToRange(unsigned int idReq, unsigned int nElem)
+{
+  if (idReq >= nElem) {
+    return (nElem - 1);
+  }
+  return idReq;
+}
+
+double getLayerValue(const Matrix &layerMat, unsigned int rowReq, unsigned int colReq)
+{
+  const auto numCol = static_cast<unsigned int>(layerMat.cols());
+  const auto  numRow = static_cast<unsigned int>(layerMat.rows());
+  const unsigned int iBoundToRange = bindIndexToRange(rowReq, numRow);
+  const unsigned int jBoundToRange = bindIndexToRange(colReq, numCol);
+  return layerMat(iBoundToRange, jBoundToRange);
+}
+
+
+/**
+ * BICUBIC CONVOLUTION INTERPOLATION ALGORITHM
+ * also known as piecewise bicubic interpolation,
+ * it does not ensure continuity of the first derivatives.
+ * see:
+ * https://en.wikipedia.org/wiki/Bicubic_interpolation
+ * https://web.archive.org/web/20051024202307/http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm
+ */
+
+namespace bicubic_conv {
+
+bool evaluateBicubicConvolutionInterpolation(const GridMap &gridMap, const std::string &layer,
+                                             const Position &queriedPosition,
+                                             double *interpolatedValue)
+{
+  FunctionValueMatrix functionValues;
+  if (!assembleFunctionValueMatrix(gridMap, layer, queriedPosition, &functionValues)) {
+    return false;
+  }
+
+  Position normalizedCoordinate;
+  if (!getNormalizedCoordinates(gridMap, queriedPosition, &normalizedCoordinate)) {
+    return false;
+  }
+
+  const double tx = normalizedCoordinate.x();
+  const double ty = normalizedCoordinate.y();
+
+  //bm1 stands for b minus one, i.e. index decreased by one
+  //b2 stands for b plus 2, i.e. index increased by two
+  const double bm1 = convolve1D(tx, functionValues.row(0));
+  const double b0 = convolve1D(tx, functionValues.row(1));
+  const double b1 = convolve1D(tx, functionValues.row(2));
+  const double b2 = convolve1D(tx, functionValues.row(3));
+  const Eigen::Vector4d vectorBs(bm1, b0, b1, b2);
+  *interpolatedValue = convolve1D(ty, vectorBs);
+  return true;
+}
+
+double convolve1D(double t, const Eigen::Vector4d &functionValues)
+{
+  const Eigen::Vector4d tVector(1.0, t, t * t, t * t * t);
+  const Eigen::Vector4d temp = cubicInterpolationConvolutionMatrix
+      * functionValues;
+  const double retVal = 0.5 * tVector.transpose() * temp;
+  return retVal;
+}
+
+bool assembleFunctionValueMatrix(const GridMap &gridMap, const std::string &layer,
+                        const Position &queriedPosition, FunctionValueMatrix *data)
+{
+
+  Index middleKnotIndex;
+  if (!getIndicesOfMiddleKnot(gridMap, queriedPosition, &middleKnotIndex)) {
+    return false;
+  }
+
+  const Matrix &layerMatrix = gridMap.get(layer);
+  auto f = [&layerMatrix](unsigned int rowReq, unsigned int colReq) {
+    double retVal = getLayerValue(layerMatrix, rowReq, colReq);
+    return retVal;
+  };
+
+  const unsigned int i = middleKnotIndex.x();
+  const unsigned int j = middleKnotIndex.y();
+  /*
+   * Notation taken from: https://en.wikipedia.org/wiki/Bicubic_interpolation
+   * increasing f's indices is flipped w.r.t. to the above since in the article
+   * they use a coordinate frame centered around (i,j). Therefore:
+   * f(i+1,j-1) in their notation corresponds to f(i-1,j+1) in ours. This is
+   * because our coordinate frame sits in the top left corner, see
+   * https://github.com/ANYbotics/grid_map
+   */
+  *data << f(i + 1, j + 1), f(i, j + 1), f(i - 1, j + 1), f(i - 2, j + 1), f(i + 1, j), f(i, j), f(
+      i - 1, j), f(i - 2, j), f(i + 1, j - 1), f(i, j - 1), f(i - 1, j - 1), f(i - 2, j - 1), f(
+      i + 1, j - 2), f(i, j - 2), f(i - 1, j - 2), f(i - 2, j - 2);
+
+  return true;
+}
+
+bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPosition,
+                              Position *position)
+{
+  Index index;
+  if (!getIndicesOfMiddleKnot(gridMap, queriedPosition, &index)) {
+    return false;
+  }
+
+  Position middleKnot;
+  if (!gridMap.getPosition(index, middleKnot)) {
+    return false;
+  }
+
+  position->x() = (queriedPosition.x() - middleKnot.x()) / gridMap.getResolution();
+  position->y() = (queriedPosition.y() - middleKnot.y()) / gridMap.getResolution();
+
+  return true;
+}
+
+bool getIndicesOfMiddleKnot(const GridMap &gridMap, const Position &queriedPosition, Index *index)
+{
+  return gridMap.getIndex(queriedPosition, *index);
+}
+
+} /* namespace bicubic_conv */
+
+/**
+ * BICUBIC INTERPOLATION ALGORITHM
+ * it does ensure continuity of the first derivatives.
+ * More expensive to compute than bicubic convolution interpolation
+ * see:
+ * https://en.wikipedia.org/wiki/Bicubic_interpolation
+ * https://web.archive.org/web/20051024202307/http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm
+ */
+
+namespace bicubic {
+
+bool evaluateBicubicInterpolation(const GridMap &gridMap, const std::string &layer,
+                                  const Position &queriedPosition, double *interpolatedValue)
+{
+
+  const Matrix& layerMat = gridMap.get(layer);
+  const double resolution = gridMap.getResolution();
+
+  // get indices of data points needed for interpolation
+  IndicesMatrix unitSquareCornerIndices;
+  if (!getUnitSquareCornerIndices(gridMap, queriedPosition, &unitSquareCornerIndices)) {
+    return false;
+  }
+
+  // get function values
+  DataMatrix f;
+  if (!getFunctionValues(layerMat, unitSquareCornerIndices, &f)) {
+    return false;
+  }
+
+  // get the first derivatives in x
+  DataMatrix dfx;
+  if (!getFirstOrderDerivatives(layerMat, unitSquareCornerIndices, Dim2D::X, resolution, &dfx)) {
+    return false;
+  }
+
+  // the first derivatives in y
+  DataMatrix dfy;
+  if (!getFirstOrderDerivatives(layerMat, unitSquareCornerIndices, Dim2D::Y, resolution, &dfy)) {
+    return false;
+  }
+  // mixed derivatives in x y
+  DataMatrix ddfxy;
+  if (!getMixedSecondOrderDerivatives(layerMat, unitSquareCornerIndices, resolution, &ddfxy)) {
+    return false;
+  }
+
+  // assemble function value matrix matrix
+  FunctionValueMatrix functionValues;
+  assembleFunctionValueMatrix(f, dfx, dfy, ddfxy, &functionValues);
+
+  // get normalized coordinates
+  Position normalizedCoordinates;
+  if (!computeNormalizedCoordinates(gridMap, unitSquareCornerIndices.bottomLeft_, queriedPosition,
+                                    &normalizedCoordinates)) {
+    return false;
+  }
+
+  // evaluate polynomial
+  *interpolatedValue = evaluatePolynomial(functionValues, normalizedCoordinates.x(),
+                                          normalizedCoordinates.y());
+
+  return true;
+}
+
+bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedPosition,
+                                IndicesMatrix *indicesMatrix)
+{
+
+  Index closestPointId;
+  if (!getClosestPointIndices(gridMap, queriedPosition, &closestPointId)) {
+    return false;
+  }
+
+  Position closestPoint;
+  if (!gridMap.getPosition(closestPointId, closestPoint)) {
+    return false;
+  }
+
+  const int idx0 = closestPointId.x();
+  const int idy0 = closestPointId.y();
+  const double x0 = closestPoint.x();
+  const double y0 = closestPoint.y();
+  const double x = queriedPosition.x();
+  const double y = queriedPosition.y();
+
+  if (x > x0) {  //first or fourth quadrant
+    if (y > y0) {  //first quadrant
+      indicesMatrix->topLeft_ = Index(idx0, idy0 - 1);
+      indicesMatrix->topRight_ = Index(idx0 - 1, idy0 - 1);
+      indicesMatrix->bottomLeft_ = Index(idx0, idy0);
+      indicesMatrix->bottomRight_ = Index(idx0 - 1, idy0);
+    } else {  // fourth quadrant
+      indicesMatrix->topLeft_ = Index(idx0, idy0);
+      indicesMatrix->topRight_ = Index(idx0 - 1, idy0);
+      indicesMatrix->bottomLeft_ = Index(idx0, idy0 + 1);
+      indicesMatrix->bottomRight_ = Index(idx0 - 1, idy0 + 1);
+    }
+  } else {  // second or third quadrant
+    if (y > y0) {  //second quadrant
+      indicesMatrix->topLeft_ = Index(idx0 + 1, idy0 - 1);
+      indicesMatrix->topRight_ = Index(idx0, idy0 - 1);
+      indicesMatrix->bottomLeft_ = Index(idx0 + 1, idy0);
+      indicesMatrix->bottomRight_ = Index(idx0, idy0);
+    } else {  // third quadrant
+      indicesMatrix->topLeft_ = Index(idx0 + 1, idy0);
+      indicesMatrix->topRight_ = Index(idx0, idy0);
+      indicesMatrix->bottomLeft_ = Index(idx0 + 1, idy0 + 1);
+      indicesMatrix->bottomRight_ = Index(idx0, idy0 + 1);
+    }
+  }
+
+  bindIndicesToRange(gridMap, indicesMatrix);
+
+  return true;
+
+}
+
+bool getClosestPointIndices(const GridMap &gridMap, const Position &queriedPosition, Index *index)
+{
+  return gridMap.getIndex(queriedPosition, *index);
+}
+
+bool computeNormalizedCoordinates(const GridMap &gridMap, const Index &originIndex,
+                                  const Position &queriedPosition, Position *normalizedCoordinates)
+{
+
+  Position origin;
+  if (!gridMap.getPosition(originIndex, origin)) {
+    return false;
+  }
+
+  normalizedCoordinates->x() = (queriedPosition.x() - origin.x()) / gridMap.getResolution();
+  normalizedCoordinates->y() = (queriedPosition.y() - origin.y()) / gridMap.getResolution();
+
+  return true;
+
+}
+
+bool getFunctionValues(const Matrix &layerData, const IndicesMatrix &indices, DataMatrix *data)
+{
+  data->topLeft_ = layerData(indices.topLeft_.x(), indices.topLeft_.y());
+  data->topRight_ = layerData(indices.topRight_.x(), indices.topRight_.y());
+  data->bottomLeft_ = layerData(indices.bottomLeft_.x(), indices.bottomLeft_.y());
+  data->bottomRight_ = layerData(indices.bottomRight_.x(), indices.bottomRight_.y());
+  return true;
+}
+
+void bindIndicesToRange(const GridMap &gridMap, IndicesMatrix *indices)
+{
+  const unsigned int numCol = gridMap.getSize().y();
+  const unsigned int numRow = gridMap.getSize().x();
+
+  //top left
+  {
+    const unsigned int iBoundToRange = bindIndexToRange(indices->topLeft_.x(), numRow);
+    const unsigned int jBoundToRange = bindIndexToRange(indices->topLeft_.y(), numCol);
+    indices->topLeft_ = Index(iBoundToRange, jBoundToRange);
+  }
+
+  //top right
+  {
+    const unsigned int iBoundToRange = bindIndexToRange(indices->topRight_.x(), numRow);
+    const unsigned int jBoundToRange = bindIndexToRange(indices->topRight_.y(), numCol);
+    indices->topRight_ = Index(iBoundToRange, jBoundToRange);
+  }
+
+  //bottom left
+  {
+    const unsigned int iBoundToRange = bindIndexToRange(indices->bottomLeft_.x(), numRow);
+    const unsigned int jBoundToRange = bindIndexToRange(indices->bottomLeft_.y(), numCol);
+    indices->bottomLeft_ = Index(iBoundToRange, jBoundToRange);
+  }
+
+  //bottom right
+  {
+    const unsigned int iBoundToRange = bindIndexToRange(indices->bottomRight_.x(), numRow);
+    const unsigned int jBoundToRange = bindIndexToRange(indices->bottomRight_.y(), numCol);
+    indices->bottomRight_ = Index(iBoundToRange, jBoundToRange);
+  }
+
+}
+
+bool getFirstOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, Dim2D dim,
+                              double resolution, DataMatrix *derivatives)
+{
+  derivatives->topLeft_ = firstOrderDerivativeAt(layerData, indices.topLeft_, dim, resolution);
+  derivatives->topRight_ = firstOrderDerivativeAt(layerData, indices.topRight_, dim, resolution);
+  derivatives->bottomLeft_ = firstOrderDerivativeAt(layerData, indices.bottomLeft_, dim,
+                                                    resolution);
+  derivatives->bottomRight_ = firstOrderDerivativeAt(layerData, indices.bottomRight_, dim,
+                                                     resolution);
+  return true;
+}
+
+double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D dim,
+                              double resolution)
+{
+  const auto numCol{static_cast<unsigned int>(layerData.cols())};
+  const auto numRow{static_cast<unsigned int>(layerData.rows())};
+
+  double left;
+  double right;
+  switch (dim) {
+    case Dim2D::X: {
+      left = layerData(bindIndexToRange(index.x() + 1, numRow), index.y());
+      right = layerData(bindIndexToRange(index.x() - 1, numRow), index.y());
+      break;
+    }
+    case Dim2D::Y: {
+      left = layerData(index.x(), bindIndexToRange(index.y() + 1, numCol));
+      right = layerData(index.x(), bindIndexToRange(index.y() - 1, numCol));
+      break;
+    }
+    default: {
+      throw std::runtime_error("Unknown derivative direction");
+    }
+  }
+
+  const double perturbation = resolution;
+  // central difference approximation
+  // we need to multiply with resolution since we are
+  // operating in scaled coordinates
+  return (right - left) / (2.0 * perturbation) * resolution;
+}
+
+double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, double resolution)
+{
+  /*
+   * no need for dimensions since we have to differentiate w.r.t. x and y
+   * the order doesn't matter. Derivative values are the same.
+   * Taken from https://www.mathematik.uni-dortmund.de/~kuzmin/cfdintro/lecture4.pdf
+   */
+  const auto numCol{static_cast<unsigned int>(layerData.cols())};
+  const auto numRow{static_cast<unsigned int>(layerData.rows())};
+
+  const double f11 = layerData(bindIndexToRange(index.x() - 1, numRow),
+                               bindIndexToRange(index.y() - 1, numCol));
+  const double f1m1 = layerData(bindIndexToRange(index.x() - 1, numRow),
+                                bindIndexToRange(index.y() + 1, numCol));
+  const double fm11 = layerData(bindIndexToRange(index.x() + 1, numRow),
+                                bindIndexToRange(index.y() - 1, numCol));
+  const double fm1m1 = layerData(bindIndexToRange(index.x() + 1, numRow),
+                                 bindIndexToRange(index.y() + 1, numCol));
+
+  const double perturbation = resolution;
+  // central difference approximation
+  // we need to multiply with resolution^2 since we are
+  // operating in scaled coordinates. Second derivative scales
+  // with the square of the resolution
+  return (f11 - f1m1 - fm11 + fm1m1) / (4.0 * perturbation * perturbation) * resolution * resolution;
+
+}
+
+bool getMixedSecondOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices,
+                                    double resolution, DataMatrix *derivatives)
+{
+  derivatives->topLeft_ = mixedSecondOrderDerivativeAt(layerData, indices.topLeft_, resolution);
+  derivatives->topRight_ = mixedSecondOrderDerivativeAt(layerData, indices.topRight_, resolution);
+  derivatives->bottomLeft_ = mixedSecondOrderDerivativeAt(layerData, indices.bottomLeft_,
+                                                          resolution);
+  derivatives->bottomRight_ = mixedSecondOrderDerivativeAt(layerData, indices.bottomRight_,
+                                                           resolution);
+  return true;
+}
+
+double evaluatePolynomial(const FunctionValueMatrix &functionValues, double tx, double ty)
+{
+  const Eigen::Vector4d xVector(1, tx, tx * tx, tx * tx * tx);
+  const Eigen::Vector4d yVector(1, ty, ty * ty, ty * ty * ty);
+  const Eigen::Matrix4d tempMat = functionValues
+      * bicubicInterpolationMatrix.transpose();
+  const Eigen::Matrix4d polynomialCoeffMatrix = bicubicInterpolationMatrix * tempMat;
+  const Eigen::Vector4d tempVec = polynomialCoeffMatrix * yVector;
+  return xVector.transpose() * tempVec;
+}
+
+void assembleFunctionValueMatrix(const DataMatrix &f, const DataMatrix &dfx, const DataMatrix &dfy,
+                                 const DataMatrix &ddfxy, FunctionValueMatrix *functionValues)
+{
+  auto toEigenMatrix = [](const DataMatrix &d)-> Eigen::Matrix2d {
+    Eigen::Matrix2d e;
+    e(0,0) = d.bottomLeft_;
+    e(1,0) = d.bottomRight_;
+    e(0,1) = d.topLeft_;
+    e(1,1) = d.topRight_;
+    return e;
+  };
+
+  functionValues->block<2, 2>(0, 0) = toEigenMatrix(f);
+  functionValues->block<2, 2>(2, 2) = toEigenMatrix(ddfxy);
+  functionValues->block<2, 2>(0, 2) = toEigenMatrix(dfy);
+  functionValues->block<2, 2>(2, 0) = toEigenMatrix(dfx);
+}
+
+} /* namespace bicubic*/
+
+} /* namespace grid_map */

src/GridMap.cpp

@@ -0,0 +1,891 @@
+/*
+ * GridMap.cpp
+ *
+ *  Created on: Jul 14, 2014
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/GridMap.hpp"
+
+#include "grid_map_core/CubicInterpolation.hpp"
+#include "grid_map_core/GridMapMath.hpp"
+#include "grid_map_core/SubmapGeometry.hpp"
+#include "grid_map_core/iterators/GridMapIterator.hpp"
+
+#include <cmath>
+#include <algorithm>
+#include <cassert>
+#include <iostream>
+#include <stdexcept>
+
+using std::cout;
+using std::endl;
+using std::isfinite;
+
+namespace grid_map {
+
+GridMap::GridMap(const std::vector<std::string>& layers) {
+  position_.setZero();
+  length_.setZero();
+  resolution_ = 0.0;
+  size_.setZero();
+  startIndex_.setZero();
+  timestamp_ = 0;
+  layers_ = layers;
+
+  for (auto& layer : layers_) {
+    data_.insert(std::pair<std::string, Matrix>(layer, Matrix()));
+  }
+}
+
+GridMap::GridMap() : GridMap(std::vector<std::string>()) {}
+
+void GridMap::setGeometry(const Length& length, const double resolution, const Position& position) {
+  assert(length(0) > 0.0);
+  assert(length(1) > 0.0);
+  assert(resolution > 0.0);
+
+  Size size;
+  size(0) = static_cast<int>(round(length(0) / resolution));  // There is no round() function in Eigen.
+  size(1) = static_cast<int>(round(length(1) / resolution));
+  resize(size);
+  clearAll();
+
+  resolution_ = resolution;
+  length_ = (size_.cast<double>() * resolution_).matrix();
+  position_ = position;
+  startIndex_.setZero();
+}
+
+void GridMap::setGeometry(const SubmapGeometry& geometry) {
+  setGeometry(geometry.getLength(), geometry.getResolution(), geometry.getPosition());
+}
+
+void GridMap::setBasicLayers(const std::vector<std::string>& basicLayers) {
+  basicLayers_ = basicLayers;
+}
+
+const std::vector<std::string>& GridMap::getBasicLayers() const {
+  return basicLayers_;
+}
+
+bool GridMap::hasBasicLayers() const {
+  return !basicLayers_.empty();
+}
+
+bool GridMap::hasSameLayers(const GridMap& other) const {
+  return std::all_of(layers_.begin(), layers_.end(),
+                     [&](const std::string& layer){return other.exists(layer);});
+}
+
+void GridMap::add(const std::string& layer, const float value) {
+  add(layer, Matrix::Constant(size_(0), size_(1), value));
+}
+
+void GridMap::add(const std::string& layer, const Matrix& data) {
+  assert(size_(0) == data.rows());
+  assert(size_(1) == data.cols());
+
+  if (exists(layer)) {
+    // Type exists already, overwrite its data.
+    data_.at(layer) = data;
+  } else {
+    // Type does not exist yet, add type and data.
+    data_.insert(std::pair<std::string, Matrix>(layer, data));
+    layers_.push_back(layer);
+  }
+}
+
+bool GridMap::exists(const std::string& layer) const {
+  return !(data_.find(layer) == data_.end());
+}
+
+const Matrix& GridMap::get(const std::string& layer) const {
+  try {
+    return data_.at(layer);
+  } catch (const std::out_of_range& exception) {
+    throw std::out_of_range("GridMap::get(...) : No map layer '" + layer + "' available.");
+  }
+}
+
+Matrix& GridMap::get(const std::string& layer) {
+  try {
+    return data_.at(layer);
+  } catch (const std::out_of_range& exception) {
+    throw std::out_of_range("GridMap::get(...) : No map layer of type '" + layer + "' available.");
+  }
+}
+
+const Matrix& GridMap::operator[](const std::string& layer) const {
+  return get(layer);
+}
+
+Matrix& GridMap::operator[](const std::string& layer) {
+  return get(layer);
+}
+
+bool GridMap::erase(const std::string& layer) {
+  const auto dataIterator = data_.find(layer);
+  if (dataIterator == data_.end()) {
+    return false;
+  }
+  data_.erase(dataIterator);
+
+  const auto layerIterator = std::find(layers_.begin(), layers_.end(), layer);
+  if (layerIterator == layers_.end()) {
+    return false;
+  }
+  layers_.erase(layerIterator);
+
+  const auto basicLayerIterator = std::find(basicLayers_.begin(), basicLayers_.end(), layer);
+  if (basicLayerIterator != basicLayers_.end()) {
+    basicLayers_.erase(basicLayerIterator);
+  }
+
+  return true;
+}
+
+const std::vector<std::string>& GridMap::getLayers() const {
+  return layers_;
+}
+
+float& GridMap::atPosition(const std::string& layer, const Position& position) {
+  Index index;
+  if (getIndex(position, index)) {
+    return at(layer, index);
+  }
+  throw std::out_of_range("GridMap::atPosition(...) : Position is out of range.");
+}
+
+float GridMap::atPosition(const std::string& layer, const Position& position, InterpolationMethods interpolationMethod) const {
+
+  bool skipNextSwitchCase = false;
+  switch (interpolationMethod) {
+    case InterpolationMethods::INTER_CUBIC_CONVOLUTION: {
+      float value;
+      if (atPositionBicubicConvolutionInterpolated(layer, position, value)) {
+        return value;
+      } else {
+        interpolationMethod = InterpolationMethods::INTER_LINEAR;
+        skipNextSwitchCase = true;
+      }
+      [[fallthrough]];
+    }
+    case InterpolationMethods::INTER_CUBIC: {
+      if (!skipNextSwitchCase) {
+        float value;
+        if (atPositionBicubicInterpolated(layer, position, value)) {
+          return value;
+        } else {
+          interpolationMethod = InterpolationMethods::INTER_LINEAR;
+        }
+      }
+      [[fallthrough]];
+    }
+    case InterpolationMethods::INTER_LINEAR: {
+      float value;
+      if (atPositionLinearInterpolated(layer, position, value)){
+        return value;
+      }
+      else {
+        interpolationMethod = InterpolationMethods::INTER_NEAREST;
+      }
+      [[fallthrough]];
+    }
+    case InterpolationMethods::INTER_NEAREST: {
+      Index index;
+      if (getIndex(position, index)) {
+        return at(layer, index);
+      } else {
+        throw std::out_of_range("GridMap::atPosition(...) : Position is out of range.");
+      }
+      break;
+    }
+    default:
+      throw std::runtime_error(
+          "GridMap::atPosition(...) : Specified "
+          "interpolation method not implemented.");
+  }
+}
+
+float& GridMap::at(const std::string& layer, const Index& index) {
+  try {
+    return data_.at(layer)(index(0), index(1));
+  } catch (const std::out_of_range& exception) {
+    throw std::out_of_range("GridMap::at(...) : No map layer '" + layer + "' available.");
+  }
+}
+
+float GridMap::at(const std::string& layer, const Index& index) const {
+  try {
+    return data_.at(layer)(index(0), index(1));
+  } catch (const std::out_of_range& exception) {
+    throw std::out_of_range("GridMap::at(...) : No map layer '" + layer + "' available.");
+  }
+}
+
+bool GridMap::getIndex(const Position& position, Index& index) const {
+  return getIndexFromPosition(index, position, length_, position_, resolution_, size_, startIndex_);
+}
+
+bool GridMap::getPosition(const Index& index, Position& position) const {
+  return getPositionFromIndex(position, index, length_, position_, resolution_, size_, startIndex_);
+}
+
+bool GridMap::isInside(const Position& position) const {
+  return checkIfPositionWithinMap(position, length_, position_);
+}
+
+bool GridMap::isValid(DataType value) const {
+  return isfinite(value);
+}
+
+bool GridMap::isValid(const Index& index) const {
+  return isValid(index, basicLayers_);
+}
+
+bool GridMap::isValid(const Index& index, const std::string& layer) const {
+  return isValid(at(layer, index));
+}
+
+bool GridMap::isValid(const Index& index, const std::vector<std::string>& layers) const {
+  if (layers.empty()) {
+    return false;
+  }
+  return std::all_of(layers.begin(), layers.end(),
+              [&](const std::string& layer){return isValid(index, layer);});
+}
+
+bool GridMap::getPosition3(const std::string& layer, const Index& index, Position3& position) const {
+  const auto value = at(layer, index);
+  if (!isValid(value)) {
+    return false;
+  }
+  Position position2d;
+  getPosition(index, position2d);
+  position.head(2) = position2d;
+  position.z() = value;
+  return true;
+}
+
+bool GridMap::getVector(const std::string& layerPrefix, const Index& index, Eigen::Vector3d& vector) const {
+  Eigen::Vector3d temp{at(layerPrefix + "x", index), at(layerPrefix + "y", index), at(layerPrefix + "z", index)};
+  if (!isValid(temp[0]) || !isValid(temp[1]) || !isValid(temp[2])) { // NOLINT (implicit-float-conversion)
+    return false;
+  } else {
+    vector = temp;
+    return true;
+  }
+}
+
+GridMap GridMap::getSubmap(const Position& position, const Length& length, bool& isSuccess) const {
+  Index index;
+  return getSubmap(position, length, index, isSuccess);
+}
+
+GridMap GridMap::getSubmap(const Position& position, const Length& length, Index& /*indexInSubmap*/, bool& isSuccess) const {
+  // Submap to generate.
+  GridMap submap(layers_);
+  submap.setBasicLayers(basicLayers_);
+  submap.setTimestamp(timestamp_);
+  submap.setFrameId(frameId_);
+
+  // Get submap geometric information.
+  SubmapGeometry submapInformation(*this, position, length, isSuccess);
+  if (!isSuccess) {
+    return GridMap{layers_};
+  }
+  submap.setGeometry(submapInformation);
+  submap.startIndex_.setZero();  // Because of the way we copy the data below.
+
+  // Copy data.
+  std::vector<BufferRegion> bufferRegions;
+
+  if (!getBufferRegionsForSubmap(bufferRegions, submapInformation.getStartIndex(), submap.getSize(), size_, startIndex_)) {
+    cout << "Cannot access submap of this size." << endl;
+    isSuccess = false;
+    return GridMap{layers_};
+  }
+
+  for (const auto& data : data_) {
+    for (const auto& bufferRegion : bufferRegions) {
+      Index index = bufferRegion.getStartIndex();
+      Size size = bufferRegion.getSize();
+
+      if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopLeft) {
+        submap.data_[data.first].topLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
+      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopRight) {
+        submap.data_[data.first].topRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
+      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomLeft) {
+        submap.data_[data.first].bottomLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
+      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomRight) {
+        submap.data_[data.first].bottomRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
+      }
+    }
+  }
+
+  isSuccess = true;
+  return submap;
+}
+
+GridMap GridMap::getTransformedMap(const Eigen::Isometry3d& transform, const std::string& heightLayerName, const std::string& newFrameId,
+                                   const double sampleRatio) const {
+  // Check if height layer is valid.
+  if (!exists(heightLayerName)) {
+    throw std::out_of_range("GridMap::getTransformedMap(...) : No map layer '" + heightLayerName + "' available.");
+  }
+
+  // Initialization.
+  std::vector<Position3> positionSamples;
+  Position3 center;
+  Index newIndex;
+
+  const double sampleLength = resolution_ * sampleRatio;
+
+  // Find edges in new coordinate frame.
+  const double halfLengthX = length_.x() * 0.5;
+  const double halfLengthY = length_.y() * 0.5;
+  const Position3 topLeftCorner(position_.x() + halfLengthX, position_.y() + halfLengthY, 0.0);
+  const Position3 topRightCorner(position_.x() + halfLengthX, position_.y() - halfLengthY, 0.0);
+  const Position3 bottomLeftCorner(position_.x() - halfLengthX, position_.y() + halfLengthY, 0.0);
+  const Position3 bottomRightCorner(position_.x() - halfLengthX, position_.y() - halfLengthY, 0.0);
+
+  std::vector<Position3> newEdges;
+  newEdges.reserve(4);
+  newEdges.push_back(transform * topLeftCorner);
+  newEdges.push_back(transform * topRightCorner);
+  newEdges.push_back(transform * bottomLeftCorner);
+  newEdges.push_back(transform * bottomRightCorner);
+
+  // Find new grid center.
+  Position3 newCenter = Position3::Zero();
+  for (const auto& newEdge : newEdges) {
+    newCenter += newEdge;
+  }
+  newCenter *= 0.25;
+
+  // Find new grid length.
+  Length maxLengthFromCenter = Length(0.0, 0.0);
+  for (const auto& newEdge : newEdges) {
+    Position3 positionCenterToEdge = newEdge - newCenter;
+    maxLengthFromCenter.x() = std::fmax(std::fabs(positionCenterToEdge.x()), maxLengthFromCenter.x());
+    maxLengthFromCenter.y() = std::fmax(std::fabs(positionCenterToEdge.y()), maxLengthFromCenter.y());
+  }
+  Length newLength = 2.0 * maxLengthFromCenter;
+
+  // Create new grid map.
+  GridMap newMap(layers_);
+  newMap.setBasicLayers(basicLayers_);
+  newMap.setTimestamp(timestamp_);
+  newMap.setFrameId(newFrameId);
+  newMap.setGeometry(newLength, resolution_, Position(newCenter.x(), newCenter.y()));
+  newMap.startIndex_.setZero();
+
+  for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
+    // Get position at current index.
+    if (!getPosition3(heightLayerName, *iterator, center)) {
+      continue;
+    }
+
+    // Sample four points around the center cell.
+    positionSamples.clear();
+
+    if (sampleRatio > 0.0) {
+      positionSamples.reserve(5);
+      positionSamples.push_back(center);
+      positionSamples.emplace_back(center.x() - sampleLength, center.y(), center.z());
+      positionSamples.emplace_back(center.x() + sampleLength, center.y(), center.z());
+      positionSamples.emplace_back(center.x(), center.y() - sampleLength, center.z());
+      positionSamples.emplace_back(center.x(), center.y() + sampleLength, center.z());
+    } else {
+      positionSamples.push_back(center);
+    }
+
+    // Transform the sampled points and register to the new map.
+    for (const auto& position : positionSamples) {
+      const Position3 transformedPosition = transform * position;
+
+      // Get new index.
+      if (!newMap.getIndex(Position(transformedPosition.x(), transformedPosition.y()), newIndex)) {
+        continue;
+      }
+
+      // Check if we have already assigned a value (preferably larger height
+      // values -> inpainting).
+      const auto newExistingValue = newMap.at(heightLayerName, newIndex);
+      if (!std::isnan(newExistingValue) && newExistingValue > transformedPosition.z()) {
+        continue;
+      }
+
+      // Copy the layers.
+      for (const auto& layer : layers_) {
+        const auto currentValueInOldGrid = at(layer, *iterator);
+        auto& newValue = newMap.at(layer, newIndex);
+        if (layer == heightLayerName) {
+          newValue = static_cast<float>(transformedPosition.z());
+        }  // adjust height
+        else {
+          newValue = currentValueInOldGrid;
+        }  // re-assign
+      }
+    }
+  }
+
+  return newMap;
+}
+
+void GridMap::setPosition(const Position& position) {
+  position_ = position;
+}
+
+bool GridMap::move(const Position& position, std::vector<BufferRegion>& newRegions) {
+  Index indexShift;
+  Position positionShift = position - position_;
+  getIndexShiftFromPositionShift(indexShift, positionShift, resolution_);
+  Position alignedPositionShift;
+  getPositionShiftFromIndexShift(alignedPositionShift, indexShift, resolution_);
+
+  // Delete fields that fall out of map (and become empty cells).
+  for (int i = 0; i < indexShift.size(); i++) {
+    if (indexShift(i) != 0) {
+      if (abs(indexShift(i)) >= getSize()(i)) {
+        // Entire map is dropped.
+        clearAll();
+        newRegions.emplace_back(Index(0, 0), getSize(), BufferRegion::Quadrant::Undefined);
+      } else {
+        // Drop cells out of map.
+        int sign = (indexShift(i) > 0 ? 1 : -1);
+        int startIndex = startIndex_(i) - (sign < 0 ? 1 : 0);
+        int endIndex = startIndex - sign + indexShift(i);
+        int nCells = abs(indexShift(i));
+        int index = (sign > 0 ? startIndex : endIndex);
+        wrapIndexToRange(index, getSize()(i));
+
+        if (index + nCells <= getSize()(i)) {
+          // One region to drop.
+          if (i == 0) {
+            clearRows(index, nCells);
+            newRegions.emplace_back(Index(index, 0), Size(nCells, getSize()(1)), BufferRegion::Quadrant::Undefined);
+          } else if (i == 1) {
+            clearCols(index, nCells);
+            newRegions.emplace_back(Index(0, index), Size(getSize()(0), nCells), BufferRegion::Quadrant::Undefined);
+          }
+        } else {
+          // Two regions to drop.
+          int firstIndex = index;
+          int firstNCells = getSize()(i) - firstIndex;
+          if (i == 0) {
+            clearRows(firstIndex, firstNCells);
+            newRegions.emplace_back(Index(firstIndex, 0), Size(firstNCells, getSize()(1)), BufferRegion::Quadrant::Undefined);
+          } else if (i == 1) {
+            clearCols(firstIndex, firstNCells);
+            newRegions.emplace_back(Index(0, firstIndex), Size(getSize()(0), firstNCells), BufferRegion::Quadrant::Undefined);
+          }
+
+          int secondIndex = 0;
+          int secondNCells = nCells - firstNCells;
+          if (i == 0) {
+            clearRows(secondIndex, secondNCells);
+            newRegions.emplace_back(Index(secondIndex, 0), Size(secondNCells, getSize()(1)), BufferRegion::Quadrant::Undefined);
+          } else if (i == 1) {
+            clearCols(secondIndex, secondNCells);
+            newRegions.emplace_back(Index(0, secondIndex), Size(getSize()(0), secondNCells), BufferRegion::Quadrant::Undefined);
+          }
+        }
+      }
+    }
+  }
+
+  // Update information.
+  startIndex_ += indexShift;
+  wrapIndexToRange(startIndex_, getSize());
+  position_ += alignedPositionShift;
+
+  // Check if map has been moved at all.
+  return indexShift.any();
+}
+
+bool GridMap::move(const Position& position) {
+  std::vector<BufferRegion> newRegions;
+  return move(position, newRegions);
+}
+
+bool GridMap::addDataFrom(const GridMap& other, bool extendMap, bool overwriteData, bool copyAllLayers, std::vector<std::string> layers) {
+  // Set the layers to copy.
+  if (copyAllLayers) {
+    layers = other.getLayers();
+  }
+
+  // Resize map.
+  if (extendMap) {
+    extendToInclude(other);
+  }
+
+  // Check if all layers to copy exist and add missing layers.
+  for (const auto& layer : layers) {
+    if (std::find(layers_.begin(), layers_.end(), layer) == layers_.end()) {
+      add(layer);
+    }
+  }
+  // Copy data.
+  for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
+    if (isValid(*iterator) && !overwriteData) {
+      continue;
+    }
+    Position position;
+    getPosition(*iterator, position);
+    Index index;
+    if (!other.isInside(position)) {
+      continue;
+    }
+    other.getIndex(position, index);
+    for (const auto& layer : layers) {
+      if (!other.isValid(index, layer)) {
+        continue;
+      }
+      at(layer, *iterator) = other.at(layer, index);
+    }
+  }
+
+  return true;
+}
+
+bool GridMap::extendToInclude(const GridMap& other) {
+  // Get dimension of maps.
+  Position topLeftCorner(position_.x() + length_.x() / 2.0, position_.y() + length_.y() / 2.0);
+  Position bottomRightCorner(position_.x() - length_.x() / 2.0, position_.y() - length_.y() / 2.0);
+  Position topLeftCornerOther(other.getPosition().x() + other.getLength().x() / 2.0, other.getPosition().y() + other.getLength().y() / 2.0);
+  Position bottomRightCornerOther(other.getPosition().x() - other.getLength().x() / 2.0,
+                                  other.getPosition().y() - other.getLength().y() / 2.0);
+  // Check if map needs to be resized.
+  bool resizeMap = false;
+  Position extendedMapPosition = position_;
+  Length extendedMapLength = length_;
+  if (topLeftCornerOther.x() > topLeftCorner.x()) {
+    extendedMapPosition.x() += (topLeftCornerOther.x() - topLeftCorner.x()) / 2.0;
+    extendedMapLength.x() += topLeftCornerOther.x() - topLeftCorner.x();
+    resizeMap = true;
+  }
+  if (topLeftCornerOther.y() > topLeftCorner.y()) {
+    extendedMapPosition.y() += (topLeftCornerOther.y() - topLeftCorner.y()) / 2.0;
+    extendedMapLength.y() += topLeftCornerOther.y() - topLeftCorner.y();
+    resizeMap = true;
+  }
+  if (bottomRightCornerOther.x() < bottomRightCorner.x()) {
+    extendedMapPosition.x() -= (bottomRightCorner.x() - bottomRightCornerOther.x()) / 2.0;
+    extendedMapLength.x() += bottomRightCorner.x() - bottomRightCornerOther.x();
+    resizeMap = true;
+  }
+  if (bottomRightCornerOther.y() < bottomRightCorner.y()) {
+    extendedMapPosition.y() -= (bottomRightCorner.y() - bottomRightCornerOther.y()) / 2.0;
+    extendedMapLength.y() += bottomRightCorner.y() - bottomRightCornerOther.y();
+    resizeMap = true;
+  }
+  // Resize map and copy data to new map.
+  if (resizeMap) {
+    GridMap mapCopy = *this;
+    setGeometry(extendedMapLength, resolution_, extendedMapPosition);
+    // Align new map with old one.
+    Vector shift = position_ - mapCopy.getPosition();
+    shift.x() = std::fmod(shift.x(), resolution_);
+    shift.y() = std::fmod(shift.y(), resolution_);
+    if (std::abs(shift.x()) < resolution_ / 2.0) {
+      position_.x() -= shift.x();
+    } else {
+      position_.x() += resolution_ - shift.x();
+    }
+    if (size_.x() % 2 != mapCopy.getSize().x() % 2) {
+      position_.x() += -std::copysign(resolution_ / 2.0, shift.x());
+    }
+    if (std::abs(shift.y()) < resolution_ / 2.0) {
+      position_.y() -= shift.y();
+    } else {
+      position_.y() += resolution_ - shift.y();
+    }
+    if (size_.y() % 2 != mapCopy.getSize().y() % 2) {
+      position_.y() += -std::copysign(resolution_ / 2.0, shift.y());
+    }
+    // Copy data.
+    for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
+      if (isValid(*iterator)) {
+        continue;
+      }
+      Position position;
+      getPosition(*iterator, position);
+      Index index;
+      if (!mapCopy.isInside(position)) {
+        continue;
+      }
+      mapCopy.getIndex(position, index);
+      for (const auto& layer : layers_) {
+        at(layer, *iterator) = mapCopy.at(layer, index);
+      }
+    }
+  }
+  return true;
+}
+
+void GridMap::setTimestamp(const Time timestamp) {
+  timestamp_ = timestamp;
+}
+
+Time GridMap::getTimestamp() const {
+  return timestamp_;
+}
+
+void GridMap::resetTimestamp() {
+  timestamp_ = 0.0;
+}
+
+void GridMap::setFrameId(const std::string& frameId) {
+  frameId_ = frameId;
+}
+
+const std::string& GridMap::getFrameId() const {
+  return frameId_;
+}
+
+const Length& GridMap::getLength() const {
+  return length_;
+}
+
+const Position& GridMap::getPosition() const {
+  return position_;
+}
+
+double GridMap::getResolution() const {
+  return resolution_;
+}
+
+const Size& GridMap::getSize() const {
+  return size_;
+}
+
+void GridMap::setStartIndex(const Index& startIndex) {
+  startIndex_ = startIndex;
+}
+
+const Index& GridMap::getStartIndex() const {
+  return startIndex_;
+}
+
+bool GridMap::isDefaultStartIndex() const {
+  return (startIndex_ == 0).all();
+}
+
+void GridMap::convertToDefaultStartIndex() {
+  if (isDefaultStartIndex()) {
+    return;
+  }
+
+  std::vector<BufferRegion> bufferRegions;
+  if (!getBufferRegionsForSubmap(bufferRegions, startIndex_, size_, size_, startIndex_)) {
+    throw std::out_of_range("Cannot access submap of this size.");
+  }
+
+  for (auto& data : data_) {
+    auto tempData(data.second);
+    for (const auto& bufferRegion : bufferRegions) {
+      Index index = bufferRegion.getStartIndex();
+      Size size = bufferRegion.getSize();
+
+      if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopLeft) {
+        tempData.topLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
+      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopRight) {
+        tempData.topRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
+      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomLeft) {
+        tempData.bottomLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
+      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomRight) {
+        tempData.bottomRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
+      }
+    }
+    data.second = tempData;
+  }
+
+  startIndex_.setZero();
+}
+
+Position GridMap::getClosestPositionInMap(const Position& position) const {
+  if (getSize().x() < 1 || getSize().y() < 1) {
+    return position_;
+  }
+
+  if (isInside(position)) {
+    return position;
+  }
+
+  Position positionInMap = position;
+
+  // Find edges.
+  const double halfLengthX = length_.x() * 0.5;
+  const double halfLengthY = length_.y() * 0.5;
+  const Position3 topLeftCorner(position_.x() + halfLengthX, position_.y() + halfLengthY, 0.0);
+  const Position3 topRightCorner(position_.x() + halfLengthX, position_.y() - halfLengthY, 0.0);
+  const Position3 bottomLeftCorner(position_.x() - halfLengthX, position_.y() + halfLengthY, 0.0);
+  const Position3 bottomRightCorner(position_.x() - halfLengthX, position_.y() - halfLengthY, 0.0);
+
+  // Find constraints.
+  const double maxX = topRightCorner.x();
+  const double minX = bottomRightCorner.x();
+  const double maxY = bottomLeftCorner.y();
+  const double minY = bottomRightCorner.y();
+
+  // Clip to box constraints and correct for indexing precision.
+  // Points on the border can lead to invalid indices because the cells represent half open intervals, i.e. [...).
+  positionInMap.x() = std::fmin(positionInMap.x(), maxX - std::numeric_limits<double>::epsilon());
+  positionInMap.y() = std::fmin(positionInMap.y(), maxY - std::numeric_limits<double>::epsilon());
+
+  positionInMap.x() = std::fmax(positionInMap.x(), minX + std::numeric_limits<double>::epsilon());
+  positionInMap.y() = std::fmax(positionInMap.y(), minY + std::numeric_limits<double>::epsilon());
+
+  return positionInMap;
+}
+
+void GridMap::clear(const std::string& layer) {
+  try {
+    data_.at(layer).setConstant(NAN);
+  } catch (const std::out_of_range& exception) {
+    throw std::out_of_range("GridMap::clear(...) : No map layer '" + layer + "' available.");
+  }
+}
+
+void GridMap::clearBasic() {
+  for (auto& layer : basicLayers_) {
+    clear(layer);
+  }
+}
+
+void GridMap::clearAll() {
+  for (auto& data : data_) {
+    data.second.setConstant(NAN);
+  }
+}
+
+void GridMap::clearRows(unsigned int index, unsigned int nRows) {
+  for (auto& layer : layers_) {
+    data_.at(layer).block(index, 0, nRows, getSize()(1)).setConstant(NAN);
+  }
+}
+
+void GridMap::clearCols(unsigned int index, unsigned int nCols) {
+  for (auto& layer : layers_) {
+    data_.at(layer).block(0, index, getSize()(0), nCols).setConstant(NAN);
+  }
+}
+
+bool GridMap::atPositionLinearInterpolated(const std::string& layer, const Position& position, float& value) const {
+  Position point;
+  Index indices[4];
+  bool idxTempDir;
+  size_t idxShift[4];
+
+  getIndex(position, indices[0]);
+  getPosition(indices[0], point);
+
+  if (position.x() >= point.x()) {
+    indices[1] = indices[0] + Index(-1, 0);  // Second point is above first point.
+    idxTempDir = true;
+  } else {
+    indices[1] = indices[0] + Index(+1, 0);
+    idxTempDir = false;
+  }
+  if (position.y() >= point.y()) {
+    indices[2] = indices[0] + Index(0, -1);  // Third point is right of first point.
+    if (idxTempDir) {
+      idxShift[0] = 0;
+      idxShift[1] = 1;
+      idxShift[2] = 2;
+      idxShift[3] = 3;
+    } else {
+      idxShift[0] = 1;
+      idxShift[1] = 0;
+      idxShift[2] = 3;
+      idxShift[3] = 2;
+    }
+
+  } else {
+    indices[2] = indices[0] + Index(0, +1);
+    if (idxTempDir) {
+      idxShift[0] = 2;
+      idxShift[1] = 3;
+      idxShift[2] = 0;
+      idxShift[3] = 1;
+    } else {
+      idxShift[0] = 3;
+      idxShift[1] = 2;
+      idxShift[2] = 1;
+      idxShift[3] = 0;
+    }
+  }
+  indices[3].x() = indices[1].x();
+  indices[3].y() = indices[2].y();
+
+  const Size& mapSize = getSize();
+  const size_t bufferSize = mapSize(0) * mapSize(1);
+  const size_t startIndexLin = getLinearIndexFromIndex(startIndex_, mapSize);
+  const size_t endIndexLin = startIndexLin + bufferSize;
+  const auto& layerMat = operator[](layer);
+  float f[4];
+
+  for (size_t i = 0; i < 4; ++i) {
+    const size_t indexLin = getLinearIndexFromIndex(indices[idxShift[i]], mapSize);
+    if ((indexLin < startIndexLin) || (indexLin > endIndexLin)) {
+      return false;
+    }
+    f[i] = layerMat(indexLin);
+  }
+
+  getPosition(indices[idxShift[0]], point);
+  const Position positionRed = (position - point) / resolution_;
+  const Position positionRedFlip = Position(1., 1.) - positionRed;
+
+  
+  value = f[0] * positionRedFlip.x() * positionRedFlip.y() + f[1] * positionRed.x() * positionRedFlip.y() + // NOLINT (implicit-float-conversion)
+          f[2] * positionRedFlip.x() * positionRed.y() + f[3] * positionRed.x() * positionRed.y(); // NOLINT (implicit-float-conversion)
+  return true;
+}
+
+void GridMap::resize(const Index& size) {
+  size_ = size;
+  for (auto& data : data_) {
+    data.second.resize(size_(0), size_(1));
+  }
+}
+
+
+bool GridMap::atPositionBicubicConvolutionInterpolated(const std::string& layer, const Position& position,
+                                            float& value) const
+{
+  double interpolatedValue = 0.0;
+  if (!bicubic_conv::evaluateBicubicConvolutionInterpolation(*this, layer, position, &interpolatedValue)) {
+    return false;
+  }
+
+  if (!std::isfinite(interpolatedValue)) {
+    return false;
+  }
+  value = static_cast<float>(interpolatedValue);
+
+  return true;
+}
+
+bool GridMap::atPositionBicubicInterpolated(const std::string& layer, const Position& position,
+                                            float& value) const
+{
+  double interpolatedValue = 0.0;
+  if (!bicubic::evaluateBicubicInterpolation(*this, layer, position, &interpolatedValue)) {
+    return false;
+  }
+
+  if (!std::isfinite(interpolatedValue)) {
+    return false;
+  }
+  value = static_cast<float>(interpolatedValue);
+
+  return true;
+
+}
+
+
+}  // namespace grid_map

src/GridMapMath.cpp

@@ -0,0 +1,600 @@
+/*
+ * GridMapMath.cpp
+ *
+ *  Created on: Dec 2, 2013
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/GridMapMath.hpp"
+
+// fabs
+#include <cmath>
+
+// Limits
+#include <limits>
+
+using std::numeric_limits;
+
+namespace grid_map {
+
+namespace internal {
+
+/*!
+ * Gets the vector from the center of the map to the origin
+ * of the map data structure.
+ * @param[out] vectorToOrigin the vector from the center of the map the origin of the map data structure.
+ * @param[in] mapLength the lengths in x and y direction.
+ * @return true if successful.
+ */
+inline bool getVectorToOrigin(Vector& vectorToOrigin, const Length& mapLength)
+{
+  vectorToOrigin = (0.5 * mapLength).matrix();
+  return true;
+}
+
+/*!
+ * Gets the vector from the center of the map to the center
+ * of the first cell of the map data.
+ * @param[out] vectorToFirstCell the vector from the center of the cell to the center of the map.
+ * @param[in] mapLength the lengths in x and y direction.
+ * @param[in] resolution the resolution of the map.
+ * @return true if successful.
+ */
+inline bool getVectorToFirstCell(Vector& vectorToFirstCell,
+                                 const Length& mapLength, const double& resolution)
+{
+  Vector vectorToOrigin;
+  getVectorToOrigin(vectorToOrigin, mapLength);
+
+  // Vector to center of cell.
+  vectorToFirstCell = (vectorToOrigin.array() - 0.5 * resolution).matrix();
+  return true;
+}
+
+inline Eigen::Matrix2i getBufferOrderToMapFrameTransformation()
+{
+  return -Eigen::Matrix2i::Identity();
+}
+
+inline Vector transformBufferOrderToMapFrame(const Index& index) {
+  return {-index[0], -index[1]};
+}
+
+inline Eigen::Matrix2i getMapFrameToBufferOrderTransformation()
+{
+  return getBufferOrderToMapFrameTransformation().transpose();
+}
+
+inline Index transformMapFrameToBufferOrder(const Vector& vector) {
+  return {-vector[0], -vector[1]};
+}
+
+inline Index transformMapFrameToBufferOrder(const Eigen::Vector2i& vector) {
+  return {-vector[0], -vector[1]};
+}
+
+inline bool checkIfStartIndexAtDefaultPosition(const Index& bufferStartIndex)
+{
+  return ((bufferStartIndex == 0).all());
+}
+
+inline Vector getIndexVectorFromIndex(
+    const Index& index,
+    const Size& bufferSize,
+    const Index& bufferStartIndex)
+{
+  Index unwrappedIndex;
+  unwrappedIndex = getIndexFromBufferIndex(index, bufferSize, bufferStartIndex);
+  return transformBufferOrderToMapFrame(unwrappedIndex);
+}
+
+inline Index getIndexFromIndexVector(
+    const Vector& indexVector,
+    const Size& bufferSize,
+    const Index& bufferStartIndex)
+{
+  Index index = transformMapFrameToBufferOrder(indexVector);
+  return getBufferIndexFromIndex(index, bufferSize, bufferStartIndex);
+}
+
+inline BufferRegion::Quadrant getQuadrant(const Index& index, const Index& bufferStartIndex) {
+  if (index[0] >= bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) {
+    return BufferRegion::Quadrant::TopLeft;
+  }
+  if (index[0] >= bufferStartIndex[0] && index[1] < bufferStartIndex[1]) {
+    return BufferRegion::Quadrant::TopRight;
+  }
+  if (index[0] < bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) {
+    return BufferRegion::Quadrant::BottomLeft;
+  }
+  if (index[0] < bufferStartIndex[0] && index[1] < bufferStartIndex[1]) {
+    return BufferRegion::Quadrant::BottomRight;
+  }
+  return BufferRegion::Quadrant::Undefined;
+}
+
+}  // namespace internal
+
+using internal::checkIfStartIndexAtDefaultPosition;
+using internal::getBufferOrderToMapFrameTransformation;
+using internal::getIndexFromIndexVector;
+using internal::getIndexVectorFromIndex;
+using internal::getMapFrameToBufferOrderTransformation;
+using internal::getQuadrant;
+using internal::getVectorToFirstCell;
+using internal::getVectorToOrigin;
+using internal::transformBufferOrderToMapFrame;
+using internal::transformMapFrameToBufferOrder;
+
+bool getPositionFromIndex(Position& position,
+                          const Index& index,
+                          const Length& mapLength,
+                          const Position& mapPosition,
+                          const double& resolution,
+                          const Size& bufferSize,
+                          const Index& bufferStartIndex)
+{
+  if (!checkIfIndexInRange(index, bufferSize)) {
+    return false;
+  }
+  Vector offset;
+  getVectorToFirstCell(offset, mapLength, resolution);
+  position = mapPosition + offset + resolution * getIndexVectorFromIndex(index, bufferSize, bufferStartIndex);
+  return true;
+}
+
+bool getIndexFromPosition(Index& index,
+                          const Position& position,
+                          const Length& mapLength,
+                          const Position& mapPosition,
+                          const double& resolution,
+                          const Size& bufferSize,
+                          const Index& bufferStartIndex)
+{
+  Vector offset;
+  getVectorToOrigin(offset, mapLength);
+  Vector indexVector = ((position - offset - mapPosition).array() / resolution).matrix();
+  index = getIndexFromIndexVector(indexVector, bufferSize, bufferStartIndex);
+  return checkIfPositionWithinMap(position, mapLength, mapPosition) && checkIfIndexInRange(index, bufferSize);
+}
+
+bool checkIfPositionWithinMap(const Position& position,
+                              const Length& mapLength,
+                              const Position& mapPosition)
+{
+  Vector offset;
+  getVectorToOrigin(offset, mapLength);
+  Position positionTransformed = getMapFrameToBufferOrderTransformation().cast<double>() * (position - mapPosition - offset);
+
+  return positionTransformed.x() >= 0.0 && positionTransformed.y() >= 0.0
+      && positionTransformed.x() < mapLength(0) && positionTransformed.y() < mapLength(1);
+}
+
+void getPositionOfDataStructureOrigin(const Position& position,
+                                      const Length& mapLength,
+                                      Position& positionOfOrigin)
+{
+  Vector vectorToOrigin;
+  getVectorToOrigin(vectorToOrigin, mapLength);
+  positionOfOrigin = position + vectorToOrigin;
+}
+
+bool getIndexShiftFromPositionShift(Index& indexShift,
+                                    const Vector& positionShift,
+                                    const double& resolution)
+{
+  Vector indexShiftVectorTemp = (positionShift.array() / resolution).matrix();
+  Eigen::Vector2i indexShiftVector;
+
+  for (int i = 0; i < indexShiftVector.size(); i++) {
+    indexShiftVector[i] = static_cast<int>(indexShiftVectorTemp[i] + 0.5 * (indexShiftVectorTemp[i] > 0 ? 1 : -1));
+  }
+
+  indexShift = transformMapFrameToBufferOrder(indexShiftVector);
+  return true;
+}
+
+bool getPositionShiftFromIndexShift(Vector& positionShift,
+                                    const Index& indexShift,
+                                    const double& resolution)
+{
+  positionShift = transformBufferOrderToMapFrame(indexShift) * resolution;
+  return true;
+}
+
+bool checkIfIndexInRange(const Index& index, const Size& bufferSize)
+{
+  return index[0] >= 0 && index[1] >= 0 && index[0] < bufferSize[0] && index[1] < bufferSize[1];
+}
+
+void boundIndexToRange(Index& index, const Size& bufferSize)
+{
+  for (int i = 0; i < index.size(); i++) {
+    boundIndexToRange(index[i], bufferSize[i]);
+  }
+}
+
+void boundIndexToRange(int& index, const int& bufferSize)
+{
+  if (index < 0) {
+    index = 0;
+  } else if (index >= bufferSize) {
+    index = bufferSize - 1;
+  }
+}
+
+void wrapIndexToRange(Index& index, const Size& bufferSize)
+{
+  for (int i = 0; i < index.size(); i++) {
+    wrapIndexToRange(index[i], bufferSize[i]);
+  }
+}
+
+void wrapIndexToRange(int& index, int bufferSize)
+{
+  // Try shortcuts before resorting to the expensive modulo operation.
+  if (index < bufferSize){
+    if(index >= 0){ // within the wanted range
+      return;
+    } else if(index >= -bufferSize){ // Index is below range, but not more than one span of the range.
+      index +=bufferSize;
+      return;
+    }else{ // Index is largely below range.
+      index = index % bufferSize;
+      index += bufferSize;
+    }
+  }else if(index < bufferSize*2){ // Index is above range, but not more than one span of the range.
+    index -= bufferSize;
+    return;
+  } else{ // Index is largely above range.
+    index = index % bufferSize;
+  }
+}
+
+void boundPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition)
+{
+  Vector vectorToOrigin;
+  getVectorToOrigin(vectorToOrigin, mapLength);
+  Position positionShifted = position - mapPosition + vectorToOrigin;
+
+  // We have to make sure to stay inside the map.
+  for (int i = 0; i < positionShifted.size(); i++) {
+
+    double epsilon = 10.0 * numeric_limits<double>::epsilon(); // TODO Why is the factor 10 necessary.
+    if (std::fabs(position(i)) > 1.0) {
+      epsilon *= std::fabs(position(i));
+    }
+
+    if (positionShifted(i) <= 0) {
+      positionShifted(i) = epsilon;
+      continue;
+    }
+    if (positionShifted(i) >= mapLength(i)) {
+      positionShifted(i) = mapLength(i) - epsilon;
+      continue;
+    }
+  }
+
+  position = positionShifted + mapPosition - vectorToOrigin;
+}
+
+Eigen::Matrix2i getBufferOrderToMapFrameAlignment()
+{
+  return getBufferOrderToMapFrameTransformation().array().abs().matrix();
+}
+
+bool getSubmapInformation(Index& submapTopLeftIndex,
+                          Size& submapBufferSize,
+                          Position& submapPosition,
+                          Length& submapLength,
+                          Index& requestedIndexInSubmap,
+                          const Position& requestedSubmapPosition,
+                          const Length& requestedSubmapLength,
+                          const Length& mapLength,
+                          const Position& mapPosition,
+                          const double& resolution,
+                          const Size& bufferSize,
+                          const Index& bufferStartIndex)
+{
+  // (Top left / bottom right corresponds to the position in the matrix, not the map frame)
+  const Eigen::Matrix2d halfTransform = 0.5 * getMapFrameToBufferOrderTransformation().cast<double>();
+
+  // Corners of submap.
+  Position topLeftPosition = requestedSubmapPosition - halfTransform * requestedSubmapLength.matrix();
+  boundPositionToRange(topLeftPosition, mapLength, mapPosition);
+  if (!getIndexFromPosition(submapTopLeftIndex, topLeftPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) {
+    return false;
+  }
+  Index topLeftIndex;
+  topLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex);
+
+  Position bottomRightPosition = requestedSubmapPosition + halfTransform * requestedSubmapLength.matrix();
+  boundPositionToRange(bottomRightPosition, mapLength, mapPosition);
+  Index bottomRightIndex;
+  if (!getIndexFromPosition(bottomRightIndex, bottomRightPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) {
+    return false;
+  }
+  bottomRightIndex = getIndexFromBufferIndex(bottomRightIndex, bufferSize, bufferStartIndex);
+
+  // Get the position of the top left corner of the generated submap.
+  Position topLeftCorner;
+  if (!getPositionFromIndex(topLeftCorner, submapTopLeftIndex, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) {
+    return false;
+  }
+  topLeftCorner -= halfTransform * Position::Constant(resolution);
+
+  // Size of submap.
+  submapBufferSize = bottomRightIndex - topLeftIndex + Index::Ones();
+
+  // Length of the submap.
+  submapLength = submapBufferSize.cast<double>() * resolution;
+
+  // Position of submap.
+  Vector vectorToSubmapOrigin;
+  getVectorToOrigin(vectorToSubmapOrigin, submapLength);
+  submapPosition = topLeftCorner - vectorToSubmapOrigin;
+
+  // Get the index of the cell which corresponds the requested
+  // position of the submap.
+  return getIndexFromPosition(requestedIndexInSubmap, requestedSubmapPosition, submapLength, submapPosition, resolution, submapBufferSize);
+}
+
+Size getSubmapSizeFromCornerIndices(const Index& topLeftIndex, const Index& bottomRightIndex,
+                                    const Size& bufferSize, const Index& bufferStartIndex)
+{
+  const Index unwrappedTopLeftIndex = getIndexFromBufferIndex(topLeftIndex, bufferSize, bufferStartIndex);
+  const Index unwrappedBottomRightIndex = getIndexFromBufferIndex(bottomRightIndex, bufferSize, bufferStartIndex);
+  return Size(unwrappedBottomRightIndex - unwrappedTopLeftIndex + Size::Ones());
+}
+
+bool getBufferRegionsForSubmap(std::vector<BufferRegion>& submapBufferRegions,
+                               const Index& submapIndex,
+                               const Size& submapBufferSize,
+                               const Size& bufferSize,
+                               const Index& bufferStartIndex)
+{
+  if ((getIndexFromBufferIndex(submapIndex, bufferSize, bufferStartIndex) + submapBufferSize > bufferSize).any()) {
+    return false;
+  }
+
+  submapBufferRegions.clear();
+
+  Index bottomRightIndex = submapIndex + submapBufferSize - Index::Ones();
+  wrapIndexToRange(bottomRightIndex, bufferSize);
+
+  BufferRegion::Quadrant quadrantOfTopLeft = getQuadrant(submapIndex, bufferStartIndex);
+  BufferRegion::Quadrant quadrantOfBottomRight = getQuadrant(bottomRightIndex, bufferStartIndex);
+
+  if (quadrantOfTopLeft == BufferRegion::Quadrant::TopLeft) {
+
+    if (quadrantOfBottomRight == BufferRegion::Quadrant::TopLeft) {
+      submapBufferRegions.emplace_back(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopLeft);
+      return true;
+    }
+
+    if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) {
+      Size topLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1));
+      submapBufferRegions.emplace_back(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft);
+
+      Index topRightIndex(submapIndex(0), 0);
+      Size topRightSize(submapBufferSize(0), submapBufferSize(1) - topLeftSize(1));
+      submapBufferRegions.emplace_back(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight);
+      return true;
+    }
+
+    if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) {
+      Size topLeftSize(bufferSize(0) - submapIndex(0), submapBufferSize(1));
+      submapBufferRegions.emplace_back(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft);
+
+      Index bottomLeftIndex(0, submapIndex(1));
+      Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), submapBufferSize(1));
+      submapBufferRegions.emplace_back(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft);
+      return true;
+    }
+
+    if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
+      Size topLeftSize(bufferSize(0) - submapIndex(0), bufferSize(1) - submapIndex(1));
+      submapBufferRegions.emplace_back(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft);
+
+      Index topRightIndex(submapIndex(0), 0);
+      Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1) - topLeftSize(1));
+      submapBufferRegions.emplace_back(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight);
+
+      Index bottomLeftIndex(0, submapIndex(1));
+      Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), bufferSize(1) - submapIndex(1));
+      submapBufferRegions.emplace_back(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft);
+
+      Index bottomRightIndex = Index::Zero();
+      Size bottomRightSize(bottomLeftSize(0), topRightSize(1));
+      submapBufferRegions.emplace_back(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight);
+      return true;
+    }
+
+  } else if (quadrantOfTopLeft == BufferRegion::Quadrant::TopRight) {
+
+    if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) {
+      submapBufferRegions.emplace_back(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopRight);
+      return true;
+    }
+
+    if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
+
+      Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1));
+      submapBufferRegions.emplace_back(submapIndex, topRightSize, BufferRegion::Quadrant::TopRight);
+
+      Index bottomRightIndex(0, submapIndex(1));
+      Size bottomRightSize(submapBufferSize(0) - topRightSize(0), submapBufferSize(1));
+      submapBufferRegions.emplace_back(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight);
+      return true;
+    }
+
+  } else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomLeft) {
+
+    if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) {
+      submapBufferRegions.emplace_back(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomLeft);
+      return true;
+    }
+
+    if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
+      Size bottomLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1));
+      submapBufferRegions.emplace_back(submapIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft);
+
+      Index bottomRightIndex(submapIndex(0), 0);
+      Size bottomRightSize(submapBufferSize(0), submapBufferSize(1) - bottomLeftSize(1));
+      submapBufferRegions.emplace_back(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight);
+      return true;
+    }
+
+  } else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomRight) {
+
+    if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
+      submapBufferRegions.emplace_back(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomRight);
+      return true;
+    }
+
+  }
+
+  return false;
+}
+
+bool incrementIndex(Index& index, const Size& bufferSize, const Index& bufferStartIndex)
+{
+  Index unwrappedIndex = getIndexFromBufferIndex(index, bufferSize, bufferStartIndex);
+
+  // Increment index.
+  if (unwrappedIndex(1) + 1 < bufferSize(1)) {
+    // Same row.
+    unwrappedIndex[1]++;
+  } else {
+    // Next row.
+    unwrappedIndex[0]++;
+    unwrappedIndex[1] = 0;
+  }
+
+  // End of iterations reached.
+  if (!checkIfIndexInRange(unwrappedIndex, bufferSize)) {
+    return false;
+  }
+
+  // Return true iterated index.
+  index = getBufferIndexFromIndex(unwrappedIndex, bufferSize, bufferStartIndex);
+  return true;
+}
+
+bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& submapTopLeftIndex,
+                             const Size& submapBufferSize, const Size& bufferSize,
+                             const Index& bufferStartIndex)
+{
+  // Copy the data first, only copy it back if everything is within range.
+  Index tempIndex = index;
+  Index tempSubmapIndex = submapIndex;
+
+  // Increment submap index.
+  if (tempSubmapIndex[1] + 1 < submapBufferSize[1]) {
+    // Same row.
+    tempSubmapIndex[1]++;
+  } else {
+    // Next row.
+    tempSubmapIndex[0]++;
+    tempSubmapIndex[1] = 0;
+  }
+
+  // End of iterations reached.
+  if (!checkIfIndexInRange(tempSubmapIndex, submapBufferSize)) {
+    return false;
+  }
+
+  // Get corresponding index in map.
+  Index unwrappedSubmapTopLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex);
+  tempIndex = getBufferIndexFromIndex(unwrappedSubmapTopLeftIndex + tempSubmapIndex, bufferSize, bufferStartIndex);
+
+  // Copy data back.
+  index = tempIndex;
+  submapIndex = tempSubmapIndex;
+  return true;
+}
+
+Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize, const Index& bufferStartIndex)
+{
+  if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) {
+    return bufferIndex;
+  }
+
+  Index index = bufferIndex - bufferStartIndex;
+  wrapIndexToRange(index, bufferSize);
+  return index;
+}
+
+Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize, const Index& bufferStartIndex)
+{
+  if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) {
+    return index;
+  }
+
+  Index bufferIndex = index + bufferStartIndex;
+  wrapIndexToRange(bufferIndex, bufferSize);
+  return bufferIndex;
+}
+
+size_t getLinearIndexFromIndex(const Index& index, const Size& bufferSize, const bool rowMajor)
+{
+  if (!rowMajor) {
+    return index(1) * bufferSize(0) + index(0);
+  }
+  return index(0) * bufferSize(1) + index(1);
+}
+
+Index getIndexFromLinearIndex(const size_t linearIndex, const Size& bufferSize, const bool rowMajor)
+{
+  if (!rowMajor) {
+    return Index((int)linearIndex % bufferSize(0), (int)linearIndex / bufferSize(0));
+  }
+  return Index((int)linearIndex / bufferSize(1), (int)linearIndex % bufferSize(1));
+}
+
+bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3i& colorVector)
+{
+  colorVector(0) = (colorValue >> 16) & 0x0000ff;
+  colorVector(1) = (colorValue >> 8) & 0x0000ff;
+  colorVector(2) =  colorValue & 0x0000ff;
+  return true;
+}
+
+bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3f& colorVector)
+{
+  Eigen::Vector3i tempColorVector;
+  colorValueToVector(colorValue, tempColorVector);
+  colorVector = ((tempColorVector.cast<float>()).array() / 255.0).matrix();
+  return true;
+}
+
+bool colorValueToVector(const float& colorValue, Eigen::Vector3f& colorVector)
+{
+  // cppcheck-suppress invalidPointerCast
+  const unsigned long tempColorValue = *reinterpret_cast<const unsigned long*>(&colorValue);
+  colorValueToVector(tempColorValue, colorVector);
+  return true;
+}
+
+bool colorVectorToValue(const Eigen::Vector3i& colorVector, unsigned long& colorValue)
+{
+  colorValue = ((int)colorVector(0)) << 16 | ((int)colorVector(1)) << 8 | ((int)colorVector(2));
+  return true;
+}
+
+void colorVectorToValue(const Eigen::Vector3i& colorVector, float& colorValue)
+{
+  Color colors;
+  colors.longColor_ = (colorVector(0) << 16) + (colorVector(1) << 8) + colorVector(2);
+  colorValue = colors.floatColor_;
+}
+
+void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue)
+{
+  Eigen::Vector3i tempColorVector = (colorVector * 255.0).cast<int>();
+  colorVectorToValue(tempColorVector, colorValue);
+}
+
+}  // namespace grid_map
+

src/Polygon.cpp

@@ -0,0 +1,352 @@
+/*
+ * Polygon.cpp
+ *
+ *  Created on: Nov 7, 2014
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#include <grid_map_core/Polygon.hpp>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <limits>
+#include <algorithm>
+
+namespace grid_map {
+
+Polygon::Polygon()
+    : timestamp_(0)
+{
+}
+
+Polygon::Polygon(std::vector<Position> vertices)
+    : Polygon()
+{
+  vertices_ = vertices;
+}
+
+bool Polygon::isInside(const Position& point) const
+{
+  int cross = 0;
+  for (size_t i = 0, j = vertices_.size() - 1; i < vertices_.size(); j = i++) {
+    if ( ((vertices_[i].y() > point.y()) != (vertices_[j].y() > point.y()))
+           && (point.x() < (vertices_[j].x() - vertices_[i].x()) * (point.y() - vertices_[i].y()) /
+            (vertices_[j].y() - vertices_[i].y()) + vertices_[i].x()) )
+    {
+      cross++;
+    }
+  }
+  return bool(cross % 2);
+}
+
+void Polygon::addVertex(const Position& vertex)
+{
+  vertices_.push_back(vertex);
+}
+
+const Position& Polygon::getVertex(const size_t index) const
+{
+  return vertices_.at(index);
+}
+
+void Polygon::removeVertices()
+{
+  vertices_.clear();
+}
+
+const Position& Polygon::operator [](const size_t index) const
+{
+  return getVertex(index);
+}
+
+const std::vector<Position>& Polygon::getVertices() const
+{
+  return vertices_;
+}
+
+size_t Polygon::nVertices() const
+{
+  return vertices_.size();
+}
+
+const std::string& Polygon::getFrameId() const
+{
+  return frameId_;
+}
+
+void Polygon::setFrameId(const std::string& frameId)
+{
+  frameId_ = frameId;
+}
+
+uint64_t Polygon::getTimestamp() const
+{
+  return timestamp_;
+}
+
+void Polygon::setTimestamp(const uint64_t timestamp)
+{
+  timestamp_ = timestamp;
+}
+
+void Polygon::resetTimestamp()
+{
+  timestamp_ = 0.0;
+}
+
+double Polygon::getArea() const
+{
+  double area = 0.0;
+  int j = vertices_.size() - 1;
+  for (size_t i = 0; i < vertices_.size(); i++) {
+    area += (vertices_.at(j).x() + vertices_.at(i).x())
+        * (vertices_.at(j).y() - vertices_.at(i).y());
+    j = i;
+  }
+  return std::abs(area / 2.0);
+}
+
+Position Polygon::getCentroid() const
+{
+  Position centroid = Position::Zero();
+  std::vector<Position> vertices = getVertices();
+  vertices.push_back(vertices.at(0));
+  double area = 0.0;
+  for (size_t i = 0; i < vertices.size() - 1; i++) {
+    const double a = vertices[i].x() * vertices[i+1].y() - vertices[i+1].x() * vertices[i].y();
+    area += a;
+    centroid.x() += a * (vertices[i].x() + vertices[i+1].x());
+    centroid.y() += a * (vertices[i].y() + vertices[i+1].y());
+  }
+  area *= 0.5;
+  centroid /= (6.0 * area);
+  return centroid;
+}
+
+void Polygon::getBoundingBox(Position& center, Length& length) const
+{
+  double minX = std::numeric_limits<double>::infinity();
+  double maxX = -std::numeric_limits<double>::infinity();
+  double minY = std::numeric_limits<double>::infinity();
+  double maxY = -std::numeric_limits<double>::infinity();
+  for (const auto& vertex : vertices_) {
+    if (vertex.x() > maxX) maxX = vertex.x();
+    if (vertex.y() > maxY) maxY = vertex.y();
+    if (vertex.x() < minX) minX = vertex.x();
+    if (vertex.y() < minY) minY = vertex.y();
+  }
+  center.x() = (minX + maxX) / 2.0;
+  center.y() = (minY + maxY) / 2.0;
+  length.x() = (maxX - minX);
+  length.y() = (maxY - minY);
+}
+
+bool Polygon::convertToInequalityConstraints(Eigen::MatrixXd& A, Eigen::VectorXd& b) const
+{
+  Eigen::MatrixXd V(nVertices(), 2);
+  for (unsigned int i = 0; i < nVertices(); ++i)
+    V.row(i) = vertices_[i];
+
+  // Create k, a list of indices from V forming the convex hull.
+  // TODO: Assuming counter-clockwise ordered convex polygon.
+  // MATLAB: k = convhulln(V);
+  Eigen::MatrixXi k;
+  k.resizeLike(V);
+  for (unsigned int i = 0; i < V.rows(); ++i)
+    k.row(i) << i, (i+1) % V.rows();
+  Eigen::RowVectorXd c = V.colwise().mean();
+  V.rowwise() -= c;
+  A = Eigen::MatrixXd::Constant(k.rows(), V.cols(), NAN);
+
+  unsigned int rc = 0;
+  for (unsigned int ix = 0; ix < k.rows(); ++ix) {
+    Eigen::MatrixXd F(2, V.cols());
+    F.row(0) << V.row(k(ix, 0));
+    F.row(1) << V.row(k(ix, 1));
+    Eigen::FullPivLU<Eigen::MatrixXd> luDecomp(F);
+    if (luDecomp.rank() == F.rows()) {
+      A.row(rc) = F.colPivHouseholderQr().solve(Eigen::VectorXd::Ones(F.rows()));
+      ++rc;
+    }
+  }
+
+  A = A.topRows(rc);
+  b = Eigen::VectorXd::Ones(A.rows());
+  b = b + A * c.transpose();
+
+  return true;
+}
+
+bool Polygon::thickenLine(const double thickness)
+{
+  if (vertices_.size() != 2) return false;
+  const Vector connection(vertices_[1] - vertices_[0]);
+  const Vector orthogonal = thickness * Vector(connection.y(), -connection.x()).normalized();
+  std::vector<Position> newVertices;
+  newVertices.reserve(4);
+  newVertices.push_back(vertices_[0] + orthogonal);
+  newVertices.push_back(vertices_[0] - orthogonal);
+  newVertices.push_back(vertices_[1] - orthogonal);
+  newVertices.push_back(vertices_[1] + orthogonal);
+  vertices_ = newVertices;
+  return true;
+}
+
+bool Polygon::offsetInward(const double margin)
+{
+  // Create a list of indices of the neighbours of each vertex.
+  // TODO: Assuming counter-clockwise ordered convex polygon.
+  std::vector<Eigen::Array2i> neighbourIndices;
+  const unsigned int n = nVertices();
+  neighbourIndices.resize(n);
+  for (unsigned int i = 0; i < n; ++i) {
+    neighbourIndices[i] << (i > 0 ? (i-1)%n : n-1), (i + 1) % n;
+  }
+
+  std::vector<Position> copy(vertices_);
+  for (unsigned int i = 0; i < neighbourIndices.size(); ++i) {
+    Eigen::Vector2d v1 = vertices_[neighbourIndices[i](0)] - vertices_[i];
+    Eigen::Vector2d v2 = vertices_[neighbourIndices[i](1)] - vertices_[i];
+    v1.normalize();
+    v2.normalize();
+    const double angle = acos(v1.dot(v2));
+    copy[i] += margin / sin(angle) * (v1 + v2);
+  }
+  vertices_ = copy;
+  return true;
+}
+
+std::vector<Polygon> Polygon::triangulate(const TriangulationMethods& /*method*/) const
+{
+  // TODO Add more triangulation methods.
+  // https://en.wikipedia.org/wiki/Polygon_triangulation
+  std::vector<Polygon> polygons;
+  if (vertices_.size() < 3)
+    return polygons;
+
+  size_t nPolygons = vertices_.size() - 2;
+  polygons.reserve(nPolygons);
+
+  if (nPolygons < 1) {
+    // Special case.
+    polygons.push_back(*this);
+  } else {
+    // General case.
+    for (size_t i = 0; i < nPolygons; ++i) {
+      Polygon polygon({vertices_[0], vertices_[i + 1], vertices_[i + 2]});
+      polygons.push_back((polygon));
+    }
+  }
+
+  return polygons;
+}
+
+Polygon Polygon::fromCircle(const Position center, const double radius,
+                                  const int nVertices)
+{
+  Eigen::Vector2d centerToVertex(radius, 0.0), centerToVertexTemp;
+
+  Polygon polygon;
+  for (int j = 0; j < nVertices; j++) {
+    double theta = j * 2 * M_PI / (nVertices - 1);
+    Eigen::Rotation2D<double> rot2d(theta);
+    centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
+    polygon.addVertex(center + centerToVertexTemp);
+  }
+  return polygon;
+}
+
+Polygon Polygon::convexHullOfTwoCircles(const Position center1,
+                                   const Position center2, const double radius,
+                                   const int nVertices)
+{
+  if (center1 == center2) return fromCircle(center1, radius, nVertices);
+  Eigen::Vector2d centerToVertex, centerToVertexTemp;
+  centerToVertex = center2 - center1;
+  centerToVertex.normalize();
+  centerToVertex *= radius;
+
+  grid_map::Polygon polygon;
+  for (int j = 0; j < ceil(nVertices / 2.0); j++) {
+    double theta = M_PI_2 + j * M_PI / (ceil(nVertices / 2.0) - 1);
+    Eigen::Rotation2D<double> rot2d(theta);
+    centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
+    polygon.addVertex(center1 + centerToVertexTemp);
+  }
+  for (int j = 0; j < ceil(nVertices / 2.0); j++) {
+    double theta = 3 * M_PI_2 + j * M_PI / (ceil(nVertices / 2.0) - 1);
+    Eigen::Rotation2D<double> rot2d(theta);
+    centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
+    polygon.addVertex(center2 + centerToVertexTemp);
+  }
+  return polygon;
+}
+
+Polygon Polygon::convexHull(Polygon& polygon1, Polygon& polygon2)
+{
+  std::vector<Position> vertices;
+  vertices.reserve(polygon1.nVertices() + polygon2.nVertices());
+  vertices.insert(vertices.end(), polygon1.getVertices().begin(), polygon1.getVertices().end());
+  vertices.insert(vertices.end(), polygon2.getVertices().begin(), polygon2.getVertices().end());
+
+  return monotoneChainConvexHullOfPoints(vertices);
+}
+
+Polygon Polygon::monotoneChainConvexHullOfPoints(const std::vector<Position>& points)
+{
+  // Adapted from https://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain
+  if (points.size() <= 3) {
+    return Polygon(points);
+  }
+  std::vector<Position> pointsConvexHull(2 * points.size());
+
+  // Sort points lexicographically.
+  auto sortedPoints(points);
+  std::sort(sortedPoints.begin(), sortedPoints.end(), sortVertices);
+
+
+  int k = 0;
+  // Build lower hull
+  for (size_t i = 0; i < sortedPoints.size(); ++i) {
+    while (k >= 2 && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
+      k--;
+    }
+    pointsConvexHull.at(k++) = sortedPoints.at(i);
+  }
+
+  // Build upper hull.
+  for (int i = sortedPoints.size() - 2, t = k + 1; i >= 0; i--) {
+    while (k >= t && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
+      k--;
+    }
+    pointsConvexHull.at(k++) = sortedPoints.at(i);
+  }
+  pointsConvexHull.resize(k - 1);
+
+  Polygon polygon(pointsConvexHull);
+  return polygon;
+}
+
+bool Polygon::sortVertices(const Eigen::Vector2d& vector1,
+                           const Eigen::Vector2d& vector2)
+{
+  return (vector1.x() < vector2.x()
+      || (vector1.x() == vector2.x() && vector1.y() < vector2.y()));
+}
+
+double Polygon::computeCrossProduct2D(const Eigen::Vector2d& vector1,
+                                      const Eigen::Vector2d& vector2)
+{
+  return (vector1.x() * vector2.y() - vector1.y() * vector2.x());
+}
+
+double Polygon::vectorsMakeClockwiseTurn(const Eigen::Vector2d &pointOrigin,
+                                         const Eigen::Vector2d &pointA,
+                                         const Eigen::Vector2d &pointB)
+{
+  return computeCrossProduct2D(pointA - pointOrigin, pointB - pointOrigin) <= 0;
+}
+
+} /* namespace grid_map */

src/SubmapGeometry.cpp

@@ -0,0 +1,59 @@
+/*
+ * SubmapGeometry.cpp
+ *
+ *  Created on: Aug 18, 2015
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#include <grid_map_core/GridMapMath.hpp>
+#include <grid_map_core/SubmapGeometry.hpp>
+
+namespace grid_map {
+
+SubmapGeometry::SubmapGeometry(const GridMap& gridMap, const Position& position,
+                               const Length& length, bool& isSuccess)
+    : gridMap_(gridMap)
+{
+  isSuccess = getSubmapInformation(startIndex_, size_, position_, length_,
+                                   requestedIndexInSubmap_, position, length, gridMap_.getLength(),
+                                   gridMap_.getPosition(), gridMap_.getResolution(),
+                                   gridMap_.getSize(), gridMap_.getStartIndex());
+}
+
+const GridMap& SubmapGeometry::getGridMap() const
+{
+  return gridMap_;
+}
+
+const Length& SubmapGeometry::getLength() const
+{
+  return length_;
+}
+
+const Position& SubmapGeometry::getPosition() const
+{
+  return position_;
+}
+
+const Index& SubmapGeometry::getRequestedIndexInSubmap() const
+{
+  return requestedIndexInSubmap_;
+}
+
+const Size& SubmapGeometry::getSize() const
+{
+  return size_;
+}
+
+double SubmapGeometry::getResolution() const
+{
+  return gridMap_.getResolution();
+}
+
+const Index& SubmapGeometry::getStartIndex() const
+{
+  return startIndex_;
+}
+
+} /* namespace grid_map */

src/iterators/CircleIterator.cpp

@@ -0,0 +1,88 @@
+/*
+ * CircleIterator.hpp
+ *
+ *  Created on: Nov 13, 2014
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/iterators/CircleIterator.hpp"
+
+#include <memory>
+#include "grid_map_core/GridMapMath.hpp"
+
+namespace grid_map {
+
+CircleIterator::CircleIterator(const GridMap& gridMap, const Position& center, const double radius)
+    : center_(center),
+      radius_(radius)
+{
+  radiusSquare_ = pow(radius_, 2);
+  mapLength_ = gridMap.getLength();
+  mapPosition_ = gridMap.getPosition();
+  resolution_ = gridMap.getResolution();
+  bufferSize_ = gridMap.getSize();
+  bufferStartIndex_ = gridMap.getStartIndex();
+  Index submapStartIndex;
+  Index submapBufferSize;
+  findSubmapParameters(center, radius, submapStartIndex, submapBufferSize);
+  internalIterator_ = std::make_shared<SubmapIterator>(gridMap, submapStartIndex, submapBufferSize);
+  if(!isInside()) {
+    ++(*this);
+  }
+}
+
+bool CircleIterator::operator !=(const CircleIterator& other) const
+{
+  return (internalIterator_ != other.internalIterator_);
+}
+
+const Index& CircleIterator::operator *() const
+{
+  return *(*internalIterator_);
+}
+
+CircleIterator& CircleIterator::operator ++()
+{
+  ++(*internalIterator_);
+  if (internalIterator_->isPastEnd()) {
+    return *this;
+  }
+
+  for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
+    if (isInside()) {
+      break;
+    }
+  }
+
+  return *this;
+}
+
+bool CircleIterator::isPastEnd() const
+{
+  return internalIterator_->isPastEnd();
+}
+
+bool CircleIterator::isInside() const
+{
+  Position position;
+  getPositionFromIndex(position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
+  double squareNorm = (position - center_).array().square().sum();
+  return (squareNorm <= radiusSquare_);
+}
+
+void CircleIterator::findSubmapParameters(const Position& center, const double radius,
+                                          Index& startIndex, Size& bufferSize) const
+{
+  Position topLeft = center.array() + radius;
+  Position bottomRight = center.array() - radius;
+  boundPositionToRange(topLeft, mapLength_, mapPosition_);
+  boundPositionToRange(bottomRight, mapLength_, mapPosition_);
+  getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
+  Index endIndex;
+  getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
+  bufferSize = getSubmapSizeFromCornerIndices(startIndex, endIndex, bufferSize_, bufferStartIndex_);
+}
+
+} /* namespace grid_map */
+

src/iterators/EllipseIterator.cpp

@@ -0,0 +1,100 @@
+/*
+ * EllipseIterator.hpp
+ *
+ *  Created on: Dec 2, 2015
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/iterators/EllipseIterator.hpp"
+#include "grid_map_core/GridMapMath.hpp"
+
+#include <cmath>
+#include <Eigen/Geometry>
+
+namespace grid_map {
+
+EllipseIterator::EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation)
+    : center_(center)
+{
+  semiAxisSquare_ = (0.5 * length).square();
+  double sinRotation = std::sin(rotation);
+  double cosRotation = std::cos(rotation);
+  transformMatrix_ << cosRotation, sinRotation, sinRotation, -cosRotation;
+  mapLength_ = gridMap.getLength();
+  mapPosition_ = gridMap.getPosition();
+  resolution_ = gridMap.getResolution();
+  bufferSize_ = gridMap.getSize();
+  bufferStartIndex_ = gridMap.getStartIndex();
+  Index submapStartIndex;
+  Index submapBufferSize;
+  findSubmapParameters(center, length, rotation, submapStartIndex, submapBufferSize);
+  internalIterator_ = std::make_shared<SubmapIterator>(gridMap, submapStartIndex, submapBufferSize);
+  if (!isInside()) {
+    ++(*this);
+  }
+}
+
+bool EllipseIterator::operator !=(const EllipseIterator& other) const
+{
+  return (internalIterator_ != other.internalIterator_);
+}
+
+const Eigen::Array2i& EllipseIterator::operator *() const
+{
+  return *(*internalIterator_);
+}
+
+EllipseIterator& EllipseIterator::operator ++()
+{
+  ++(*internalIterator_);
+  if (internalIterator_->isPastEnd()) {
+    return *this;
+  }
+
+  for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
+    if (isInside()) {
+      break;
+    }
+  }
+
+  return *this;
+}
+
+bool EllipseIterator::isPastEnd() const
+{
+  return internalIterator_->isPastEnd();
+}
+
+const Size& EllipseIterator::getSubmapSize() const
+{
+  return internalIterator_->getSubmapSize();
+}
+
+bool EllipseIterator::isInside() const
+{
+  Position position;
+  getPositionFromIndex(position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
+  double value = ((transformMatrix_ * (position - center_)).array().square() / semiAxisSquare_).sum();
+  return (value <= 1);
+}
+
+void EllipseIterator::findSubmapParameters(const Position& center, const Length& length, const double rotation,
+                                           Index& startIndex, Size& bufferSize) const
+{
+  const Eigen::Rotation2Dd rotationMatrix(rotation);
+  Eigen::Vector2d u = rotationMatrix * Eigen::Vector2d(length(0), 0.0);
+  Eigen::Vector2d v = rotationMatrix * Eigen::Vector2d(0.0, length(1));
+  const Length boundingBoxHalfLength = (u.cwiseAbs2() + v.cwiseAbs2()).array().sqrt();
+  Position topLeft = center.array() + boundingBoxHalfLength;
+  Position bottomRight = center.array() - boundingBoxHalfLength;
+  boundPositionToRange(topLeft, mapLength_, mapPosition_);
+  boundPositionToRange(bottomRight, mapLength_, mapPosition_);
+  getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
+  Index endIndex;
+  getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
+  bufferSize = getSubmapSizeFromCornerIndices(startIndex, endIndex, bufferSize_, bufferStartIndex_);
+}
+
+} /* namespace grid_map */
+

src/iterators/GridMapIterator.cpp

@@ -0,0 +1,75 @@
+/*
+ * GridMapIterator.cpp
+ *
+ *  Created on: Sep 22, 2014
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/iterators/GridMapIterator.hpp"
+#include "grid_map_core/GridMapMath.hpp"
+
+namespace grid_map {
+
+GridMapIterator::GridMapIterator(const grid_map::GridMap& gridMap)
+{
+  size_ = gridMap.getSize();
+  startIndex_ = gridMap.getStartIndex();
+  linearSize_ = size_.prod();
+  linearIndex_ = 0;
+  isPastEnd_ = false;
+}
+
+GridMapIterator::GridMapIterator(const GridMapIterator* other)
+{
+  size_ = other->size_;
+  startIndex_ = other->startIndex_;
+  linearSize_ = other->linearSize_;
+  linearIndex_ = other->linearIndex_;
+  isPastEnd_ = other->isPastEnd_;
+}
+
+bool GridMapIterator::operator !=(const GridMapIterator& other) const
+{
+  return linearIndex_ != other.linearIndex_;
+}
+
+Index GridMapIterator::operator *() const
+{
+  return getIndexFromLinearIndex(linearIndex_, size_);
+}
+
+const size_t& GridMapIterator::getLinearIndex() const
+{
+  return linearIndex_;
+}
+
+Index GridMapIterator::getUnwrappedIndex() const
+{
+  return getIndexFromBufferIndex(*(*this), size_, startIndex_);
+}
+
+GridMapIterator& GridMapIterator::operator ++()
+{
+  size_t newIndex = linearIndex_ + 1;
+  if (newIndex < linearSize_) {
+    linearIndex_ = newIndex;
+  } else {
+    isPastEnd_ = true;
+  }
+  return *this;
+}
+
+GridMapIterator GridMapIterator::end() const
+{
+  GridMapIterator res(this);
+  res.linearIndex_ = linearSize_ - 1;
+  return res;
+}
+
+bool GridMapIterator::isPastEnd() const
+{
+  return isPastEnd_;
+}
+
+} /* namespace grid_map */

src/iterators/LineIterator.cpp

@@ -0,0 +1,138 @@
+/*
+ * LineIterator.hpp
+ *
+ *  Created on: Nov 13, 2014
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/iterators/LineIterator.hpp"
+#include "grid_map_core/GridMapMath.hpp"
+
+namespace grid_map {
+
+LineIterator::LineIterator(const grid_map::GridMap& gridMap, const Position& start,
+                           const Position& end)
+{
+  Index startIndex;
+  Index endIndex;
+  if (getIndexLimitedToMapRange(gridMap, start, end, startIndex)
+      && getIndexLimitedToMapRange(gridMap, end, start, endIndex)) {
+    initialize(gridMap, startIndex, endIndex);
+  }
+  else {
+    throw std::invalid_argument("Failed to construct LineIterator.");
+  }
+}
+
+LineIterator::LineIterator(const grid_map::GridMap& gridMap, const Index& start, const Index& end)
+{
+  initialize(gridMap, start, end);
+}
+
+bool LineIterator::operator !=(const LineIterator& other) const
+{
+  return (index_ != other.index_).any();
+}
+
+const Index& LineIterator::operator *() const
+{
+  return index_;
+}
+
+LineIterator& LineIterator::operator ++()
+{
+  numerator_ += numeratorAdd_;  // Increase the numerator by the top of the fraction.
+  if (numerator_ >= denominator_) {
+    numerator_ -= denominator_;
+    const Index unwrappedIndex = getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) + increment1_;
+    index_ = getBufferIndexFromIndex(unwrappedIndex, bufferSize_, bufferStartIndex_);
+  }
+  const Index unwrappedIndex = getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) + increment2_;
+  index_ = getBufferIndexFromIndex(unwrappedIndex, bufferSize_, bufferStartIndex_);
+  ++iCell_;
+  return *this;
+}
+
+bool LineIterator::isPastEnd() const
+{
+  return iCell_ >= nCells_;
+}
+
+bool LineIterator::initialize(const grid_map::GridMap& gridMap, const Index& start, const Index& end)
+{
+    start_ = start;
+    end_ = end;
+    mapLength_ = gridMap.getLength();
+    mapPosition_ = gridMap.getPosition();
+    resolution_ = gridMap.getResolution();
+    bufferSize_ = gridMap.getSize();
+    bufferStartIndex_ = gridMap.getStartIndex();
+    initializeIterationParameters();
+    return true;
+}
+
+bool LineIterator::getIndexLimitedToMapRange(const grid_map::GridMap& gridMap,
+                                             const Position& start, const Position& end,
+                                             Index& index)
+{
+  Position newStart = start;
+  Vector direction = (end - start).normalized();
+  while (!gridMap.getIndex(newStart, index)) {
+    newStart += (gridMap.getResolution() - std::numeric_limits<double>::epsilon()) * direction;
+    if ((end - newStart).norm() < gridMap.getResolution() - std::numeric_limits<double>::epsilon()) {
+      return false;
+    }
+  }
+  return true;
+}
+
+void LineIterator::initializeIterationParameters()
+{
+  iCell_ = 0;
+  index_ = start_;
+
+  const Index unwrappedStart = getIndexFromBufferIndex(start_, bufferSize_, bufferStartIndex_);
+  const Index unwrappedEnd = getIndexFromBufferIndex(end_, bufferSize_, bufferStartIndex_);
+  const Size delta = (unwrappedEnd - unwrappedStart).abs();
+
+  if (unwrappedEnd.x() >= unwrappedStart.x()) {
+    // x-values increasing.
+    increment1_.x() = 1;
+    increment2_.x() = 1;
+  } else {
+    // x-values decreasing.
+    increment1_.x() = -1;
+    increment2_.x() = -1;
+  }
+
+  if (unwrappedEnd.y() >= unwrappedStart.y()) {
+    // y-values increasing.
+    increment1_.y() = 1;
+    increment2_.y() = 1;
+  } else {
+    // y-values decreasing.
+    increment1_.y() = -1;
+    increment2_.y() = -1;
+  }
+
+  if (delta.x() >= delta.y()) {
+    // There is at least one x-value for every y-value.
+    increment1_.x() = 0; // Do not change the x when numerator >= denominator.
+    increment2_.y() = 0; // Do not change the y for every iteration.
+    denominator_ = delta.x();
+    numerator_ = delta.x() / 2;
+    numeratorAdd_ = delta.y();
+    nCells_ = delta.x() + 1; // There are more x-values than y-values.
+  } else {
+    // There is at least one y-value for every x-value
+    increment2_.x() = 0; // Do not change the x for every iteration.
+    increment1_.y() = 0; // Do not change the y when numerator >= denominator.
+    denominator_ = delta.y();
+    numerator_ = delta.y() / 2;
+    numeratorAdd_ = delta.x();
+    nCells_ = delta.y() + 1; // There are more y-values than x-values.
+  }
+}
+
+} /* namespace grid_map */

src/iterators/PolygonIterator.cpp

@@ -0,0 +1,88 @@
+/*
+ * PolygonIterator.hpp
+ *
+ *  Created on: Sep 19, 2014
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#include <memory>
+
+#include "grid_map_core/iterators/PolygonIterator.hpp"
+#include "grid_map_core/GridMapMath.hpp"
+
+namespace grid_map {
+
+PolygonIterator::PolygonIterator(const grid_map::GridMap& gridMap, const grid_map::Polygon& polygon)
+    : polygon_(polygon)
+{
+  mapLength_ = gridMap.getLength();
+  mapPosition_ = gridMap.getPosition();
+  resolution_ = gridMap.getResolution();
+  bufferSize_ = gridMap.getSize();
+  bufferStartIndex_ = gridMap.getStartIndex();
+  Index submapStartIndex;
+  Size submapBufferSize;
+  findSubmapParameters(polygon, submapStartIndex, submapBufferSize);
+  internalIterator_ = std::make_shared<SubmapIterator>(gridMap, submapStartIndex, submapBufferSize);
+  if (!isInside()) {
+    ++(*this);
+  }
+}
+
+bool PolygonIterator::operator !=(const PolygonIterator& other) const
+{
+  return (internalIterator_ != other.internalIterator_);
+}
+
+const Index& PolygonIterator::operator *() const
+{
+  return *(*internalIterator_);
+}
+
+PolygonIterator& PolygonIterator::operator ++()
+{
+  ++(*internalIterator_);
+  if (internalIterator_->isPastEnd()) {
+    return *this;
+  }
+
+  for (; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
+    if (isInside()) {
+      break;
+    }
+  }
+
+  return *this;
+}
+
+bool PolygonIterator::isPastEnd() const
+{
+  return internalIterator_->isPastEnd();
+}
+
+bool PolygonIterator::isInside() const
+{
+  Position position;
+  getPositionFromIndex(position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
+  return polygon_.isInside(position);
+}
+
+void PolygonIterator::findSubmapParameters(const grid_map::Polygon& /*polygon*/, Index& startIndex, Size& bufferSize) const
+{
+  Position topLeft = polygon_.getVertices()[0];
+  Position bottomRight = topLeft;
+  for (const auto& vertex : polygon_.getVertices()) {
+    topLeft = topLeft.array().max(vertex.array());
+    bottomRight = bottomRight.array().min(vertex.array());
+  }
+  boundPositionToRange(topLeft, mapLength_, mapPosition_);
+  boundPositionToRange(bottomRight, mapLength_, mapPosition_);
+  getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
+  Index endIndex;
+  getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
+  bufferSize = getSubmapSizeFromCornerIndices(startIndex, endIndex, bufferSize_, bufferStartIndex_);
+}
+
+} /* namespace grid_map */
+

src/iterators/SlidingWindowIterator.cpp

@@ -0,0 +1,117 @@
+/*
+ * SlidingWindowIterator.cpp
+ *
+ *  Created on: Aug 17, 2017
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/iterators/SlidingWindowIterator.hpp"
+#include "grid_map_core/GridMapMath.hpp"
+
+#include <iostream>
+
+namespace grid_map {
+
+SlidingWindowIterator::SlidingWindowIterator(const GridMap& gridMap, const std::string& layer,
+                                             const EdgeHandling& edgeHandling, const size_t windowSize)
+    : GridMapIterator(gridMap),
+      edgeHandling_(edgeHandling),
+      data_(gridMap[layer])
+{
+  windowSize_ = windowSize;
+  setup(gridMap);
+}
+
+SlidingWindowIterator::SlidingWindowIterator(const SlidingWindowIterator* other)
+    : GridMapIterator(other),
+      edgeHandling_(other->edgeHandling_),
+      data_(other->data_)
+{
+  windowSize_ = other->windowSize_;
+  windowMargin_ = other->windowMargin_;
+}
+
+void SlidingWindowIterator::setWindowLength(const GridMap& gridMap, const double windowLength)
+{
+  windowSize_ = static_cast<size_t>(std::round(windowLength / gridMap.getResolution()));
+  if (windowSize_ % 2 != 1) {
+    ++windowSize_;
+  }
+  setup(gridMap);
+}
+
+SlidingWindowIterator& SlidingWindowIterator::operator ++()
+{
+  if (edgeHandling_ == EdgeHandling::INSIDE) {
+    while (!isPastEnd()) {
+      GridMapIterator::operator++();
+      if (dataInsideMap()) {
+        break;
+      }
+    }
+  } else {
+    GridMapIterator::operator++();
+  }
+  return *this;
+}
+
+Matrix SlidingWindowIterator::getData() const
+{
+  const Index centerIndex(*(*this));
+  const Index windowMargin(Index::Constant(static_cast<int>(windowMargin_)));
+  const Index originalTopLeftIndex(centerIndex - windowMargin);
+  Index topLeftIndex(originalTopLeftIndex);
+  boundIndexToRange(topLeftIndex, size_);
+  Index bottomRightIndex(centerIndex + windowMargin);
+  boundIndexToRange(bottomRightIndex, size_);
+  const Size adjustedWindowSize(bottomRightIndex - topLeftIndex + Size::Ones());
+
+  switch (edgeHandling_) {
+    case EdgeHandling::INSIDE:
+    case EdgeHandling::CROP:
+      return data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1));
+    case EdgeHandling::EMPTY:
+    case EdgeHandling::MEAN:
+      const Matrix data = data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1));
+      Matrix returnData(windowSize_, windowSize_);
+      if (edgeHandling_ == EdgeHandling::EMPTY) {
+        returnData.setConstant(NAN);
+      } else if (edgeHandling_ == EdgeHandling::MEAN) {
+        returnData.setConstant(data.meanOfFinites());
+      }
+      const Index topLeftIndexShift(topLeftIndex - originalTopLeftIndex);
+      returnData.block(topLeftIndexShift(0), topLeftIndexShift(1), adjustedWindowSize(0), adjustedWindowSize(1)) =
+          data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1));
+      return returnData;
+  }
+  return Matrix::Zero(0, 0);
+}
+
+void SlidingWindowIterator::setup(const GridMap& gridMap)
+{
+  if (!gridMap.isDefaultStartIndex()) {
+    throw std::runtime_error("SlidingWindowIterator cannot be used with grid maps that don't have a default buffer start index.");
+  }
+  if (windowSize_ % 2 == 0) {
+    throw std::runtime_error("SlidingWindowIterator has a wrong window size!");
+  }
+  windowMargin_ = (windowSize_ - 1) / 2;
+
+  if (edgeHandling_ == EdgeHandling::INSIDE) {
+    if (!dataInsideMap()) {
+      operator++();
+    }
+  }
+}
+
+bool SlidingWindowIterator::dataInsideMap() const
+{
+  const Index centerIndex(*(*this));
+  const Index windowMargin(Index::Constant(static_cast<int>(windowMargin_)));
+  const Index topLeftIndex(centerIndex - windowMargin);
+  const Index bottomRightIndex(centerIndex + windowMargin);
+  return checkIfIndexInRange(topLeftIndex, size_) && checkIfIndexInRange(bottomRightIndex, size_);
+}
+
+} /* namespace grid_map */

src/iterators/SpiralIterator.cpp

@@ -0,0 +1,110 @@
+/*
+ * SpiralIterator.hpp
+ *
+ *  Created on: Jul 7, 2015
+ *      Author: Martin Wermelinger
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/iterators/SpiralIterator.hpp"
+#include "grid_map_core/GridMapMath.hpp"
+
+#include <cmath>
+#include <utility>
+
+
+namespace grid_map {
+
+SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, Eigen::Vector2d  center,
+                               const double radius)
+    : center_(std::move(center)),
+      radius_(radius),
+      distance_(0)
+{
+  radiusSquare_ = radius_ * radius_;
+  mapLength_ = gridMap.getLength();
+  mapPosition_ = gridMap.getPosition();
+  resolution_ = gridMap.getResolution();
+  bufferSize_ = gridMap.getSize();
+  gridMap.getIndex(center_, indexCenter_);
+  nRings_ = static_cast<uint>(std::ceil(radius_ / resolution_));
+  if (checkIfIndexInRange(indexCenter_, bufferSize_)) {
+    pointsRing_.push_back(indexCenter_);
+  } else {
+    while (pointsRing_.empty() && !isPastEnd()) {
+      generateRing();
+    }
+  }
+}
+
+bool SpiralIterator::operator !=(const SpiralIterator& /*other*/) const
+{
+  return (pointsRing_.back() != pointsRing_.back()).any();
+}
+
+const Eigen::Array2i& SpiralIterator::operator *() const
+{
+  return pointsRing_.back();
+}
+
+SpiralIterator& SpiralIterator::operator ++()
+{
+  pointsRing_.pop_back();
+  if (pointsRing_.empty() && !isPastEnd()) {
+    generateRing();
+  }
+  return *this;
+}
+
+bool SpiralIterator::isPastEnd() const
+{
+  return (distance_ == nRings_ && pointsRing_.empty());
+}
+
+bool SpiralIterator::isInside(const Index& index) const
+{
+  Eigen::Vector2d position;
+  getPositionFromIndex(position, index, mapLength_, mapPosition_, resolution_, bufferSize_);
+  double squareNorm = (position - center_).array().square().sum();
+  return (squareNorm <= radiusSquare_);
+}
+
+void SpiralIterator::generateRing()
+{
+  distance_++;
+  Index point(distance_, 0);
+  Index pointInMap;
+  Index normal;
+  do {
+    pointInMap.x() = point.x() + indexCenter_.x();
+    pointInMap.y() = point.y() + indexCenter_.y();
+    if (checkIfIndexInRange(pointInMap, bufferSize_)) {
+      if (distance_ == nRings_ || distance_ == nRings_ - 1) {
+        if (isInside(pointInMap)) {
+          pointsRing_.push_back(pointInMap);
+        }
+      } else {
+        pointsRing_.push_back(pointInMap);
+      }
+    }
+    normal.x() = -signum(point.y());
+    normal.y() = signum(point.x());
+    if (normal.x() != 0 && static_cast<unsigned int>(Vector(point.x() + normal.x(), point.y()).norm()) == distance_) {
+      point.x() += normal.x();
+    } else if (normal.y() != 0 && static_cast<unsigned int>(Vector(point.x(), point.y() + normal.y()).norm()) == distance_) {
+      point.y() += normal.y();
+    } else {
+      point.x() += normal.x();
+      point.y() += normal.y();
+    }
+  } while (static_cast<unsigned int>(point.x()) != distance_ || point.y() != 0);
+}
+
+double SpiralIterator::getCurrentRadius() const
+{
+  Index radius = *(*this) - indexCenter_;
+  return radius.matrix().norm() * resolution_;
+}
+
+} /* namespace grid_map */
+

src/iterators/SubmapIterator.cpp

@@ -0,0 +1,84 @@
+/*
+ * SubmapIterator.hpp
+ *
+ *  Created on: Sep 22, 2014
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/iterators/SubmapIterator.hpp"
+#include "grid_map_core/GridMapMath.hpp"
+
+
+
+namespace grid_map {
+
+SubmapIterator::SubmapIterator(const grid_map::SubmapGeometry& submap)
+    : SubmapIterator(submap.getGridMap(), submap.getStartIndex(), submap.getSize())
+{
+}
+
+SubmapIterator::SubmapIterator(const grid_map::GridMap& gridMap,
+                               const grid_map::BufferRegion& bufferRegion)
+    : SubmapIterator(gridMap, bufferRegion.getStartIndex(), bufferRegion.getSize())
+{
+}
+
+
+SubmapIterator::SubmapIterator(const grid_map::GridMap& gridMap, const Index& submapStartIndex,
+                               const Size& submapSize)
+{
+  size_ = gridMap.getSize();
+  startIndex_ = gridMap.getStartIndex();
+  index_ = submapStartIndex;
+  submapSize_ = submapSize;
+  submapStartIndex_ = submapStartIndex;
+  submapIndex_.setZero();
+  isPastEnd_ = false;
+}
+
+SubmapIterator::SubmapIterator(const SubmapIterator* other)
+{
+  size_ = other->size_;
+  startIndex_ = other->startIndex_;
+  submapSize_ = other->submapSize_;
+  submapStartIndex_ = other->submapStartIndex_;
+  index_ = other->index_;
+  submapIndex_ = other->submapIndex_;
+  isPastEnd_ = other->isPastEnd_;
+}
+
+bool SubmapIterator::operator !=(const SubmapIterator& other) const
+{
+  return (index_ != other.index_).any();
+}
+
+const Index& SubmapIterator::operator *() const
+{
+  return index_;
+}
+
+const Index& SubmapIterator::getSubmapIndex() const
+{
+  return submapIndex_;
+}
+
+SubmapIterator& SubmapIterator::operator ++()
+{
+  isPastEnd_ = !incrementIndexForSubmap(submapIndex_, index_, submapStartIndex_,
+                                        submapSize_, size_, startIndex_);
+  return *this;
+}
+
+bool SubmapIterator::isPastEnd() const
+{
+  return isPastEnd_;
+}
+
+const Size& SubmapIterator::getSubmapSize() const
+{
+  return submapSize_;
+}
+
+} /* namespace grid_map */
+

test/CubicConvolutionInterpolationTest.cpp

@@ -0,0 +1,130 @@
+/*
+ * CubicConvolutionInterpolationTest.cpp
+ *
+ *  Created on: Mar 3, 2020
+ *      Author: Edo Jelavic
+ *      Institute: ETH Zurich, Robotic Systems Lab
+ */
+
+#include "test_helpers.hpp"
+
+#include "grid_map_core/GridMap.hpp"
+
+// gtest
+#include <gtest/gtest.h>
+
+namespace gm = grid_map;
+namespace gmt = grid_map_test;
+
+TEST(CubicConvolutionInterpolation, FlatWorld)
+{
+  const int seed = rand();
+  gmt::rndGenerator.seed(seed);
+  auto map = gmt::createMap(gm::Length(2.0, 2.0), 0.1, gm::Position(0.0, 0.0));
+  auto trueValues = gmt::createFlatWorld(&map);
+  const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 100);
+
+  verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION);
+
+  if (::testing::Test::HasFailure()) {
+    std::cout << "\n Test CubicConvolutionInterpolation, FlatWorld failed with seed: " << seed
+              << std::endl;
+  }
+}
+
+TEST(CubicConvolutionInterpolation, RationalFunctionWorld)
+{
+  const int seed = rand();
+  gmt::rndGenerator.seed(seed);
+  auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.01, gm::Position(0.0, 0.0));
+  auto trueValues = gmt::createRationalFunctionWorld(&map);
+  const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
+
+  verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION);
+
+  if (::testing::Test::HasFailure()) {
+    std::cout << "\n Test CubicConvolutionInterpolation, RationalFunctionWorld failed with seed: "
+              << seed << std::endl;
+  }
+}
+
+TEST(CubicConvolutionInterpolation, SaddleWorld)
+{
+  const int seed = rand();
+  gmt::rndGenerator.seed(seed);
+  auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.1, gm::Position(0.0, 0.0));
+  auto trueValues = gmt::createSaddleWorld(&map);
+  const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
+
+  verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION);
+
+  if (::testing::Test::HasFailure()) {
+    std::cout << "\n Test CubicConvolutionInterpolation, SaddleWorld failed with seed: " << seed
+              << std::endl;
+  }
+}
+
+TEST(CubicConvolutionInterpolation, SecondOrderPolyWorld)
+{
+  const int seed = rand();
+  gmt::rndGenerator.seed(seed);
+  auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.1, gm::Position(0.0, 0.0));
+  auto trueValues = gmt::createSecondOrderPolyWorld(&map);
+  const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
+
+  verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION);
+
+  if (::testing::Test::HasFailure()) {
+    std::cout << "\n Test CubicConvolutionInterpolation, SecondOrderPolyWorld failed with seed: "
+              << seed << std::endl;
+  }
+}
+
+TEST(CubicConvolutionInterpolation, SineWorld)
+{
+  const int seed = rand();
+  gmt::rndGenerator.seed(seed);
+  auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.01, gm::Position(0.0, 0.0));
+  auto trueValues = gmt::createSineWorld(&map);
+  const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
+
+  verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION);
+
+  if (::testing::Test::HasFailure()) {
+    std::cout << "\n Test CubicConvolutionInterpolation, SineWorld failed with seed: " << seed
+              << std::endl;
+  }
+}
+
+TEST(CubicConvolutionInterpolation, TanhWorld)
+{
+  const int seed = rand();
+  gmt::rndGenerator.seed(seed);
+  auto map = gmt::createMap(gm::Length(2.5, 2.5), 0.02, gm::Position(0.0, 0.0));
+  auto trueValues = gmt::createTanhWorld(&map);
+  const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
+
+  verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION);
+
+  if (::testing::Test::HasFailure()) {
+    std::cout << "\n Test CubicConvolutionInterpolation, TanhWorld failed with seed: " << seed
+              << std::endl;
+  }
+}
+
+TEST(CubicConvolutionInterpolation, GaussianWorld)
+{
+  const int seed = rand();
+  gmt::rndGenerator.seed(seed);
+  auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.02, gm::Position(0.0, 0.0));
+  auto trueValues = gmt::createGaussianWorld(&map);
+  const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
+
+  verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION);
+
+  if (::testing::Test::HasFailure()) {
+    std::cout << "\n Test CubicConvolutionInterpolation, GaussianWorld failed with seed: " << seed
+              << std::endl;
+  }
+}
+

test/CubicInterpolationTest.cpp

@@ -0,0 +1,126 @@
+/*
+ * CubicInterpolationTest.cpp
+ *
+ *  Created on: Mar 12, 2020
+ *      Author: Edo Jelavic
+ *      Institute: ETH Zurich, Robotic Systems Lab
+ */
+
+#include "test_helpers.hpp"
+
+#include "grid_map_core/GridMap.hpp"
+
+// gtest
+#include <gtest/gtest.h>
+
+namespace gm = grid_map;
+namespace gmt = grid_map_test;
+
+TEST(CubicInterpolation, FlatWorld)
+{
+  const int seed = rand();
+  gmt::rndGenerator.seed(seed);
+  auto map = gmt::createMap(gm::Length(2.0, 2.0), 0.1, gm::Position(0.0, 0.0));
+  auto trueValues = gmt::createFlatWorld(&map);
+  const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 100);
+
+  verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC);
+
+  if (::testing::Test::HasFailure()) {
+    std::cout << "\n Test CubicInterpolation, FlatWorld failed with seed: " << seed << std::endl;
+  }
+}
+
+TEST(CubicInterpolation, RationalFunctionWorld)
+{
+  const int seed = rand();
+  gmt::rndGenerator.seed(seed);
+  auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.01, gm::Position(0.0, 0.0));
+  auto trueValues = gmt::createRationalFunctionWorld(&map);
+  const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
+
+  verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC);
+
+  if (::testing::Test::HasFailure()) {
+    std::cout << "\n Test CubicInterpolation, RationalFunctionWorld failed with seed: " << seed
+              << std::endl;
+  }
+}
+
+TEST(CubicInterpolation, SaddleWorld)
+{
+  const int seed = rand();
+  gmt::rndGenerator.seed(seed);
+  auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.1, gm::Position(0.0, 0.0));
+  auto trueValues = gmt::createSaddleWorld(&map);
+  const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
+
+  verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC);
+
+  if (::testing::Test::HasFailure()) {
+    std::cout << "\n Test CubicInterpolation, SaddleWorld failed with seed: " << seed << std::endl;
+  }
+}
+
+TEST(CubicInterpolation, SecondOrderPolyWorld)
+{
+  const int seed = rand();
+  gmt::rndGenerator.seed(seed);
+  auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.1, gm::Position(0.0, 0.0));
+  auto trueValues = gmt::createSecondOrderPolyWorld(&map);
+  const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
+
+  verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC);
+
+  if (::testing::Test::HasFailure()) {
+    std::cout << "\n Test CubicInterpolation, SecondOrderPolyWorld failed with seed: " << seed
+              << std::endl;
+  }
+}
+
+TEST(CubicInterpolation, SineWorld)
+{
+  const int seed = rand();
+  gmt::rndGenerator.seed(seed);
+  auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.01, gm::Position(0.0, 0.0));
+  auto trueValues = gmt::createSineWorld(&map);
+  const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
+
+  verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC);
+
+  if (::testing::Test::HasFailure()) {
+    std::cout << "\n Test CubicInterpolation, SineWorld failed with seed: " << seed << std::endl;
+  }
+}
+
+TEST(CubicInterpolation, TanhWorld)
+{
+  const int seed = rand();
+  gmt::rndGenerator.seed(seed);
+  auto map = gmt::createMap(gm::Length(2.5, 2.5), 0.02, gm::Position(0.0, 0.0));
+  auto trueValues = gmt::createTanhWorld(&map);
+  const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
+
+  verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC);
+
+  if (::testing::Test::HasFailure()) {
+    std::cout << "\n Test CubicInterpolation, TanhWorld failed with seed: " << seed << std::endl;
+  }
+}
+
+TEST(CubicInterpolation, GaussianWorld)
+{
+  const int seed = rand();
+  gmt::rndGenerator.seed(seed);
+  auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.02, gm::Position(0.0, 0.0));
+  auto trueValues = gmt::createGaussianWorld(&map);
+  const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
+
+  verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC);
+
+  if (::testing::Test::HasFailure()) {
+    std::cout << "\n Test CubicInterpolation, GaussianWorld failed with seed: " << seed
+              << std::endl;
+  }
+}
+

test/EigenPluginsTest.cpp

@@ -0,0 +1,115 @@
+/*
+ * EigenMatrixBaseAddonsTest.cpp
+ *
+ *  Created on: Sep 23, 2015
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/grid_map_core.hpp"
+
+// gtest
+#include <gtest/gtest.h>
+
+// Eigen
+#include <Eigen/Core>
+
+using Eigen::Matrix;
+
+TEST(EigenMatrixBaseAddons, numberOfFinites)
+{
+  Eigen::Matrix3f matrix(Eigen::Matrix3f::Ones());
+  matrix(0, 0) = NAN;
+  matrix(1, 0) = NAN;
+  EXPECT_EQ(7, matrix.numberOfFinites());
+
+  Matrix<double, 13, 10> matrix2;
+  matrix2.setOnes();
+  EXPECT_EQ(matrix2.rows() * matrix2.cols(), matrix2.numberOfFinites());
+
+  Matrix<double, 13, 10> matrix3;
+  matrix3.setConstant(NAN);
+  matrix3.col(3).setConstant(0.0);
+  EXPECT_EQ(matrix3.rows(), matrix3.numberOfFinites());
+}
+
+TEST(EigenMatrixBaseAddons, sumOfFinites)
+{
+  Matrix<double, 7, 18> matrix;
+  matrix.setRandom();
+  EXPECT_NEAR(matrix.sum(), matrix.sumOfFinites(), 1e-10);
+  double finiteSum = matrix.sum() - matrix(0, 0) - matrix(1, 2) - matrix(3, 6) - matrix(6, 12);
+  matrix(0, 0) = NAN;
+  matrix(1, 2) = NAN;
+  matrix(3, 6) = NAN;
+  matrix(6, 12) = NAN;
+  EXPECT_NEAR(finiteSum, matrix.sumOfFinites(), 1e-10);
+  matrix.setConstant(NAN);
+  EXPECT_TRUE(std::isnan(matrix.sumOfFinites()));
+  matrix(5, 7) = 1.0;
+  EXPECT_NEAR(1.0, matrix.sumOfFinites(), 1e-10);
+}
+
+TEST(EigenMatrixBaseAddons, meanOfFinites)
+{
+  Eigen::Matrix3f matrix(Eigen::Matrix3f::Ones());
+  matrix(0, 0) = NAN;
+  matrix(1, 1) = NAN;
+  EXPECT_DOUBLE_EQ(1.0, matrix.meanOfFinites());
+
+  Matrix<double, 13, 10> matrix2;
+  matrix2.setRandom();
+  EXPECT_NEAR(matrix2.mean(), matrix2.meanOfFinites(), 1e-10);
+}
+
+TEST(EigenMatrixBaseAddons, minCoeffOfFinites)
+{
+  Matrix<double, 7, 18> matrix;
+  matrix.setRandom();
+  double min = matrix.minCoeff();
+  EXPECT_NEAR(min, matrix.minCoeffOfFinites(), 1e-10);
+
+  int i;
+  int j;
+  matrix.maxCoeff(&i, &j);
+  matrix(i, j) = NAN;
+  EXPECT_NEAR(min, matrix.minCoeffOfFinites(), 1e-10);
+
+  matrix.setConstant(NAN);
+  EXPECT_TRUE(std::isnan(matrix.minCoeffOfFinites()));
+  matrix(i, j) = -1.0;
+  EXPECT_NEAR(-1.0, matrix.minCoeffOfFinites(), 1e-10);
+}
+
+TEST(EigenMatrixBaseAddons, maxCoeffOfFinites)
+{
+  Matrix<double, 7, 18> matrix;
+  matrix.setRandom();
+  double max = matrix.maxCoeff();
+  EXPECT_NEAR(max, matrix.maxCoeffOfFinites(), 1e-10);
+
+  int i;
+  int j;
+  matrix.minCoeff(&i, &j);
+  matrix(i, j) = NAN;
+  EXPECT_NEAR(max, matrix.maxCoeffOfFinites(), 1e-10);
+
+  matrix.setConstant(NAN);
+  EXPECT_TRUE(std::isnan(matrix.maxCoeffOfFinites()));
+  matrix(i, j) = -1.0;
+  EXPECT_NEAR(-1.0, matrix.maxCoeffOfFinites(), 1e-10);
+}
+
+TEST(EigenMatrixBaseAddons, clamp)
+{
+  Eigen::VectorXf vector(Eigen::VectorXf::LinSpaced(9, 1.0, 9.0));
+  Eigen::Matrix3f matrix;
+  matrix << vector.segment(0, 3), vector.segment(3, 3), vector.segment(6, 3);
+  matrix(1, 1) = NAN;
+  matrix = matrix.unaryExpr(grid_map::Clamp<float>(2.1, 7.0));
+  EXPECT_NEAR(2.1, matrix(0, 0), 1e-7);
+  EXPECT_NEAR(2.1, matrix(1, 0), 1e-7);
+  EXPECT_NEAR(3.0, matrix(2, 0), 1e-7);
+  EXPECT_TRUE(std::isnan(matrix(1, 1)));
+  EXPECT_NEAR(7.0, matrix(2, 2), 1e-7);
+}

test/EllipseIteratorTest.cpp

@@ -0,0 +1,55 @@
+/*
+ * EllipseIteratorTest.cpp
+ *
+ *  Created on: Dec 2, 2015
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/iterators/EllipseIterator.hpp"
+#include "grid_map_core/GridMap.hpp"
+
+// gtest
+#include <gtest/gtest.h>
+
+// Vector
+#include <vector>
+
+using grid_map::GridMap;
+using grid_map::Length;
+using grid_map::Position;
+using grid_map::EllipseIterator;
+
+TEST(EllipseIterator, OneCellWideEllipse)
+{
+  GridMap map( { "types" });
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
+
+  EllipseIterator iterator(map, Position(0.0, 0.0), Length(8.0, 1.0));
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(0, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(1, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(2, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  ++iterator;
+  ++iterator;
+  ++iterator;
+  ++iterator;
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(7, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}

test/GridMapIteratorTest.cpp

@@ -0,0 +1,59 @@
+/*
+ * GridMapDataIterator.cpp
+ *
+ *  Created on: Feb 16, 2016
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/iterators/GridMapIterator.hpp"
+#include "grid_map_core/GridMap.hpp"
+
+// gtest
+#include <gtest/gtest.h>
+
+// Vector
+#include <vector>
+
+using grid_map::GridMap;
+using grid_map::Length;
+using grid_map::Position;
+using grid_map::GridMapIterator;
+
+TEST(GridMapIterator, Simple)
+{
+  GridMap map;
+  map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
+  map.add("layer", 0.0);
+  GridMapIterator iterator(map);
+
+  unsigned int i = 0;
+  for (; !iterator.isPastEnd(); ++iterator, ++i) {
+    map.at("layer", *iterator) = 1.0;
+    EXPECT_FALSE(iterator.isPastEnd());
+  }
+
+  EXPECT_EQ(40, i);
+  EXPECT_TRUE(iterator.isPastEnd());
+  EXPECT_TRUE((map["layer"].array() == 1.0f).all());
+}
+
+TEST(GridMapIterator, LinearIndex)
+{
+  GridMap map;
+  map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
+  map.add("layer", 0.0);
+  GridMapIterator iterator(map);
+
+  auto& data = map["layer"];
+  unsigned int i = 0;
+  for (; !iterator.isPastEnd(); ++iterator, ++i) {
+    data(static_cast<long>(iterator.getLinearIndex())) = 1.0;
+    EXPECT_EQ(i, iterator.getLinearIndex());
+    EXPECT_FALSE(iterator.isPastEnd());
+  }
+
+  EXPECT_EQ(40, i);
+  EXPECT_TRUE(iterator.isPastEnd());
+  EXPECT_TRUE((map["layer"].array() == 1.0f).all());
+}

test/GridMapTest.cpp

@@ -0,0 +1,496 @@
+/*
+ * GridMapTest.cpp
+ *
+ *  Created on: Aug 26, 2015
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/GridMap.hpp"
+
+// gtest
+#include <gtest/gtest.h>
+
+namespace grid_map {
+
+TEST(GridMap, CopyConstructor) {
+  GridMap map({"layer_a", "layer_b"});
+  map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2));
+  map["layer_a"].setConstant(1.0);
+  map["layer_b"].setConstant(2.0);
+  GridMap mapCopy(map);
+  EXPECT_EQ(map.getSize()[0], mapCopy.getSize()[0]);
+  EXPECT_EQ(map.getSize()[1], mapCopy.getSize()[1]);
+  EXPECT_EQ(map.getLength().x(), mapCopy.getLength().x());
+  EXPECT_EQ(map.getLength().y(), mapCopy.getLength().y());
+  EXPECT_EQ(map.getPosition().x(), mapCopy.getPosition().x());
+  EXPECT_EQ(map.getPosition().y(), mapCopy.getPosition().y());
+  EXPECT_EQ(map.getLayers().size(), mapCopy.getLayers().size());
+  EXPECT_EQ(map["layer_a"](0, 0), mapCopy["layer_a"](0, 0));
+  EXPECT_EQ(map["layer_b"](0, 0), mapCopy["layer_b"](0, 0));
+}
+
+TEST(GridMap, CopyAssign)
+{
+  GridMap map({"layer_a", "layer_b"});
+  map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2));
+  map["layer_a"].setConstant(1.0);
+  map["layer_b"].setConstant(2.0);
+  GridMap mapCopy;
+  mapCopy = map;
+  EXPECT_EQ(map.getSize()[0], mapCopy.getSize()[0]);
+  EXPECT_EQ(map.getSize()[1], mapCopy.getSize()[1]);
+  EXPECT_EQ(map.getLength().x(), mapCopy.getLength().x());
+  EXPECT_EQ(map.getLength().y(), mapCopy.getLength().y());
+  EXPECT_EQ(map.getPosition().x(), mapCopy.getPosition().x());
+  EXPECT_EQ(map.getPosition().y(), mapCopy.getPosition().y());
+  EXPECT_EQ(map.getLayers().size(), mapCopy.getLayers().size());
+  EXPECT_EQ(map["layer_a"](0, 0), mapCopy["layer_a"](0, 0));
+  EXPECT_EQ(map["layer_b"](0, 0), mapCopy["layer_b"](0, 0));
+}
+
+TEST(GridMap, Move)
+{
+  GridMap map;
+  map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
+  map.add("layer", 0.0);
+  map.setBasicLayers(map.getLayers());
+  std::vector<BufferRegion> regions;
+  map.move(Position(-3.0, -2.0), regions);
+  Index startIndex = map.getStartIndex();
+
+  EXPECT_EQ(3, startIndex(0));
+  EXPECT_EQ(2, startIndex(1));
+  
+  Eigen::Matrix<bool, 8, 5> isValidExpected;
+  isValidExpected << false, false, false, false, false, // clang-format off
+                     false, false, false, false, false,
+                     false, false, false, false, false,
+                     false, false, true,  true,  true,
+                     false, false, true,  true,  true,
+                     false, false, true,  true,  true,
+                     false, false, true,  true,  true,
+                     false, false, true,  true,  true; // clang-format on
+  for(int row{0}; row < 8; row++){
+    for(int col{0}; col < 5; col++){
+      EXPECT_EQ(map.isValid(Index(row, col)), isValidExpected(row, col)) << "Value of map.isValid at ["<<row << ", " << col <<"] is unexpected!";
+    }
+  }
+
+  EXPECT_EQ(2, regions.size());
+  EXPECT_EQ(0, regions[0].getStartIndex()[0]);
+  EXPECT_EQ(0, regions[0].getStartIndex()[1]);
+  EXPECT_EQ(3, regions[0].getSize()[0]);
+  EXPECT_EQ(5, regions[0].getSize()[1]);
+  EXPECT_EQ(0, regions[1].getStartIndex()[0]);
+  EXPECT_EQ(0, regions[1].getStartIndex()[1]);
+  EXPECT_EQ(8, regions[1].getSize()[0]);
+  EXPECT_EQ(2, regions[1].getSize()[1]);
+}
+
+TEST(GridMap, Transform)
+{
+  // Initial map.
+  GridMap map;
+  constexpr auto heightLayerName = "height";
+
+  map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0));
+  map.add(heightLayerName, 0.0);
+  map.setBasicLayers(map.getLayers());
+  map.get(heightLayerName)(0,0) = 1.0;
+
+  // Transformation (90° rotation).
+  Eigen::Isometry3d transform;
+
+  transform.translation().x() = 0.0;
+  transform.translation().y() = 0.0;
+  transform.translation().z() = 0.0;
+
+  transform.linear()(0,0) =  0.0;
+  transform.linear()(0,1) = -1.0;
+  transform.linear()(0,2) =  0.0;
+
+  transform.linear()(1,0) =  1.0;
+  transform.linear()(1,1) =  0.0;
+  transform.linear()(1,2) =  0.0;
+
+  transform.linear()(2,0) =  0.0;
+  transform.linear()(2,1) =  0.0;
+  transform.linear()(2,2) =  1.0;
+
+  // Apply affine transformation.
+  const GridMap transformedMap = map.getTransformedMap(transform, heightLayerName, map.getFrameId(), 0.25);
+
+  // Check if map has been rotated by 90° about z
+  EXPECT_NEAR(map.getLength().x(), transformedMap.getLength().y(), 1e-6);
+  EXPECT_NEAR(map.getLength().y(), transformedMap.getLength().x(), 1e-6);
+  EXPECT_EQ(map.get(heightLayerName).size(), transformedMap.get(heightLayerName).size());
+  EXPECT_DOUBLE_EQ(map.get(heightLayerName)(0,0), transformedMap.get(heightLayerName)(19,0));
+}
+
+TEST(GridMap, ClipToMap)
+{
+  GridMap map({"layer_a", "layer_b"});
+  map.setGeometry(Length(1.0, 1.0), 0.1, Position(0.5, 0.5));
+  map["layer_a"].setConstant(1.0);
+  map["layer_b"].setConstant(2.0);
+
+  const Position positionInMap = Position(0.4, 0.3); // position located inside the map
+  const Position positionOutMap = Position(1.0, 2.0); // position located outside the map
+
+  const Position clippedPositionInMap = map.getClosestPositionInMap(positionInMap);
+  const Position clippedPositionOutMap = map.getClosestPositionInMap(positionOutMap);
+
+  // Check if position-in-map remains unchanged.
+  EXPECT_NEAR(clippedPositionInMap.x(),positionInMap.x(), 1e-6);
+  EXPECT_NEAR(clippedPositionInMap.y(), positionInMap.y(), 1e-6);
+
+  // Check if position-out-map is indeed outside the map.
+  EXPECT_TRUE(!map.isInside(positionOutMap));
+
+  // Check if position-out-map has been projected into the map.
+  EXPECT_TRUE(map.isInside(clippedPositionOutMap));
+}
+
+
+
+TEST(GridMap, ClipToMap2)
+{
+  GridMap map({"types"});
+  map.setGeometry(Length(1.0, 1.0), 0.05, Position(0.0, 0.0));
+
+  // Test 8 points outside of map.
+  /*
+   * A  B  C
+   *  +---+
+   *  |   |         X
+   * D|   |E        ^
+   *  |   |         |
+   *  +---+     Y<--+
+   * F  G  H
+   *
+   * Note: Position to index alignment is a half open interval.
+   *       An example position of 0.5 is assigned to the upper index.
+   *       The interval in the current example is: 
+   *       Position: [...)[0.485 ... 0.5)[0.5 ... 0.505)[...)
+   *       Index:      8          9           10          11
+   */
+
+  Index insideIndex;
+  Position outsidePosition;
+
+  // Point A
+  outsidePosition = Position(1.0, 1.0);
+  auto closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
+  bool isInside = map.getIndex(closestInsidePosition, insideIndex);
+
+  auto expectedPosition = Position(0.5, 0.5);
+  auto expectedIndex = Index(0, 0);
+
+  // Check position.
+  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
+  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
+  
+  // Check index.
+  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
+  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
+  
+  // Check if index is inside.
+  EXPECT_TRUE(isInside) << "position is: " << std::endl
+                        << closestInsidePosition << std::endl 
+                        << " index is: " << std::endl
+                        << insideIndex << std::endl;
+
+  // Point B
+  outsidePosition = Position(1.0, 0.0);
+  closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
+  isInside = map.getIndex(closestInsidePosition, insideIndex);
+
+  expectedPosition = Position(0.5, 0.0);
+  expectedIndex = Index(0, 10);
+
+  // Check position.
+  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
+  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
+  
+  // Check index.
+  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
+  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
+  
+  // Check if index is inside.
+  EXPECT_TRUE(isInside) << "position is: " << std::endl
+                        << closestInsidePosition << std::endl 
+                        << " index is: " << std::endl
+                        << insideIndex << std::endl;
+
+  // Point C
+  outsidePosition = Position(1.0, -1.0);
+  closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
+  isInside = map.getIndex(closestInsidePosition, insideIndex);
+
+  expectedPosition = Position(0.5, -0.5);
+  expectedIndex = Index(0, 19);
+
+  // Check position.
+  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
+  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
+  
+  // Check index.
+  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
+  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
+  
+  // Check if index is inside.
+  EXPECT_TRUE(isInside) << "position is: " << std::endl
+                        << closestInsidePosition << std::endl 
+                        << " index is: " << std::endl
+                        << insideIndex << std::endl;
+
+  // Point D
+  outsidePosition = Position(0.0, 1.0);
+  closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
+  isInside = map.getIndex(closestInsidePosition, insideIndex);
+
+  expectedPosition = Position(0.0, 0.5);
+  expectedIndex = Index(10, 0);
+
+  // Check position.
+  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
+  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
+  
+  // Check index.
+  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
+  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
+  
+  // Check if index is inside.
+  EXPECT_TRUE(isInside) << "position is: " << std::endl
+                        << closestInsidePosition << std::endl 
+                        << " index is: " << std::endl
+                        << insideIndex << std::endl;
+
+  // Point E
+  outsidePosition = Position(0.0, -1.0);
+  closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
+  isInside = map.getIndex(closestInsidePosition, insideIndex);
+
+  expectedPosition = Position(0.0, -0.5);
+  expectedIndex = Index(10, 19);
+
+  // Check position.
+  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
+  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
+  
+  // Check index.
+  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
+  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
+  
+  // Check if index is inside.
+  EXPECT_TRUE(isInside) << "position is: " << std::endl
+                        << closestInsidePosition << std::endl 
+                        << " index is: " << std::endl
+                        << insideIndex << std::endl;
+
+  // Point F
+  outsidePosition = Position(-1.0, 1.0);
+  closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
+  isInside = map.getIndex(closestInsidePosition, insideIndex);
+
+  expectedPosition = Position(-0.5, 0.5);
+  expectedIndex = Index(19, 0);
+
+  // Check position.
+  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
+  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
+  
+  // Check index.
+  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
+  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
+  
+  // Check if index is inside.
+  EXPECT_TRUE(isInside) << "position is: " << std::endl
+                        << closestInsidePosition << std::endl 
+                        << " index is: " << std::endl
+                        << insideIndex << std::endl;
+
+  // Point G
+  outsidePosition = Position(-1.0, 0.0);
+  closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
+  isInside = map.getIndex(closestInsidePosition, insideIndex);
+
+  expectedPosition = Position(-0.5, 0.0);
+  expectedIndex = Index(19, 10);
+
+  // Check position.
+  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
+  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
+  
+  // Check index.
+  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
+  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
+  
+  // Check if index is inside.
+  EXPECT_TRUE(isInside) << "position is: " << std::endl
+                        << closestInsidePosition << std::endl 
+                        << " index is: " << std::endl
+                        << insideIndex << std::endl;
+
+  // Point H
+  outsidePosition = Position(-1.0, -1.0);
+  closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
+  isInside = map.getIndex(closestInsidePosition, insideIndex);
+
+  expectedPosition = Position(-0.5, -0.5);
+  expectedIndex = Index(19, 19);
+
+  // Check position.
+  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
+  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
+  
+  // Check index.
+  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
+  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
+  
+  // Check if index is inside.
+  EXPECT_TRUE(isInside) << "position is: " << std::endl
+                        << closestInsidePosition << std::endl 
+                        << " index is: " << std::endl
+                        << insideIndex << std::endl;
+}
+
+TEST(AddDataFrom, ExtendMapAligned)
+{
+  GridMap map1;
+  GridMap map2;
+  map1.setGeometry(Length(5.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(5, 5)
+  map1.add("zero", 0.0);
+  map1.add("one", 1.0);
+  map1.setBasicLayers(map1.getLayers());
+
+  map2.setGeometry(Length(3.1, 3.1), 1.0, Position(2.0, 2.0));
+  map2.add("one", 1.1);
+  map2.add("two", 2.0);
+  map2.setBasicLayers(map1.getLayers());
+
+  map1.addDataFrom(map2, true, true, true);
+
+  EXPECT_TRUE(map1.exists("two"));
+  EXPECT_TRUE(map1.isInside(Position(3.0, 3.0)));
+  EXPECT_DOUBLE_EQ(6.0, map1.getLength().x());
+  EXPECT_DOUBLE_EQ(6.0, map1.getLength().y());
+  EXPECT_DOUBLE_EQ(0.5, map1.getPosition().x());
+  EXPECT_DOUBLE_EQ(0.5, map1.getPosition().y());
+  EXPECT_NEAR(1.1, map1.atPosition("one", Position(2, 2)), 1e-4);
+  EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(-2, -2)));
+  EXPECT_DOUBLE_EQ(0.0, map1.atPosition("zero", Position(0.0, 0.0)));
+}
+
+TEST(AddDataFrom, ExtendMapNotAligned)
+{
+  GridMap map1;
+  GridMap map2;
+  map1.setGeometry(Length(6.1, 6.1), 1.0, Position(0.0, 0.0)); // bufferSize(6, 6)
+  map1.add("nan");
+  map1.add("one", 1.0);
+  map1.add("zero", 0.0);
+  map1.setBasicLayers(map1.getLayers());
+
+  map2.setGeometry(Length(3.1, 3.1), 1.0, Position(3.2, 3.2));
+  map2.add("nan", 1.0);
+  map2.add("one", 1.1);
+  map2.add("two", 2.0);
+  map2.setBasicLayers(map1.getLayers());
+
+  std::vector<std::string> stringVector;
+  stringVector.emplace_back("nan");
+  map1.addDataFrom(map2, true, false, false, stringVector);
+  Index index;
+  map1.getIndex(Position(-2, -2), index);
+
+  EXPECT_FALSE(map1.exists("two"));
+  EXPECT_TRUE(map1.isInside(Position(4.0, 4.0)));
+  EXPECT_DOUBLE_EQ(8.0, map1.getLength().x());
+  EXPECT_DOUBLE_EQ(8.0, map1.getLength().y());
+  EXPECT_DOUBLE_EQ(1.0, map1.getPosition().x());
+  EXPECT_DOUBLE_EQ(1.0, map1.getPosition().y());
+  EXPECT_FALSE(map1.isValid(index, "nan"));
+  EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(0.0, 0.0)));
+  EXPECT_DOUBLE_EQ(1.0, map1.atPosition("nan", Position(3.0, 3.0)));
+}
+
+TEST(AddDataFrom, CopyData)
+{
+  GridMap map1;
+  GridMap map2;
+  map1.setGeometry(Length(5.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(5, 5)
+  map1.add("zero", 0.0);
+  map1.add("one");
+  map1.setBasicLayers(map1.getLayers());
+
+  map2.setGeometry(Length(3.1, 3.1), 1.0, Position(2.0, 2.0));
+  map2.add("one", 1.0);
+  map2.add("two", 2.0);
+  map2.setBasicLayers(map1.getLayers());
+
+  map1.addDataFrom(map2, false, false, true);
+  Index index;
+  map1.getIndex(Position(-2, -2), index);
+
+  EXPECT_TRUE(map1.exists("two"));
+  EXPECT_FALSE(map1.isInside(Position(3.0, 3.0)));
+  EXPECT_DOUBLE_EQ(5.0, map1.getLength().x());
+  EXPECT_DOUBLE_EQ(5.0, map1.getLength().y());
+  EXPECT_DOUBLE_EQ(0.0, map1.getPosition().x());
+  EXPECT_DOUBLE_EQ(0.0, map1.getPosition().y());
+  EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(2, 2)));
+  EXPECT_FALSE(map1.isValid(index, "one"));
+  EXPECT_DOUBLE_EQ(0.0, map1.atPosition("zero", Position(0.0, 0.0)));
+}
+
+TEST(ValueAtPosition, NearestNeighbor)
+{
+  GridMap map( { "types" });
+  map.setGeometry(Length(3.0, 3.0), 1.0, Position(0.0, 0.0));
+
+  map.at("types", Index(0,0)) = 0.5;
+  map.at("types", Index(0,1)) = 3.8;
+  map.at("types", Index(0,2)) = 2.0;
+  map.at("types", Index(1,0)) = 2.1;
+  map.at("types", Index(1,1)) = 1.0;
+  map.at("types", Index(1,2)) = 2.0;
+  map.at("types", Index(2,0)) = 1.0;
+  map.at("types", Index(2,1)) = 2.0;
+  map.at("types", Index(2,2)) = 2.0;
+
+  double value = map.atPosition("types", Position(1.35,-0.4));
+  EXPECT_DOUBLE_EQ((float)3.8, value);
+
+  value = map.atPosition("types", Position(-0.3,0.0));
+  EXPECT_DOUBLE_EQ(1.0, value);
+}
+
+TEST(ValueAtPosition, LinearInterpolated)
+{
+  GridMap map( { "types" });
+  map.setGeometry(Length(3.0, 3.0), 1.0, Position(0.0, 0.0));
+
+  map.at("types", Index(0,0)) = 0.5;
+  map.at("types", Index(0,1)) = 3.8;
+  map.at("types", Index(0,2)) = 2.0;
+  map.at("types", Index(1,0)) = 2.1;
+  map.at("types", Index(1,1)) = 1.0;
+  map.at("types", Index(1,2)) = 2.0;
+  map.at("types", Index(2,0)) = 1.0;
+  map.at("types", Index(2,1)) = 2.0;
+  map.at("types", Index(2,2)) = 2.0;
+
+  // Close to the border -> reverting to INTER_NEAREST.
+  double value = map.atPosition("types", Position(-0.5,-1.2), InterpolationMethods::INTER_LINEAR);
+  EXPECT_DOUBLE_EQ(2.0, value);
+  // In between 1.0 and 2.0 field.
+  value = map.atPosition("types", Position(-0.5,0.0), InterpolationMethods::INTER_LINEAR);
+  EXPECT_DOUBLE_EQ(1.5, value);
+  // Calculated "by Hand".
+  value = map.atPosition("types", Position(0.69,0.38), InterpolationMethods::INTER_LINEAR);
+  EXPECT_NEAR(2.1963200, value, 0.0000001);
+}
+
+}  // namespace grid_map

test/LineIteratorTest.cpp

@@ -0,0 +1,178 @@
+/*
+ * LineIteratorTest.cpp
+ *
+ *  Created on: Sep 14, 2016
+ *      Author: Dominic Jud
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/iterators/LineIterator.hpp"
+#include "grid_map_core/GridMap.hpp"
+
+// gtest
+#include <gtest/gtest.h>
+
+namespace grid_map {
+
+TEST(LineIterator, StartOutsideMap) {
+  GridMap map( { "types" });
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
+
+  EXPECT_NO_THROW(LineIterator iterator(map, Position(2.0, 2.0), Position(0.0, 0.0)));
+  LineIterator iterator(map, Position(2.0, 2.0), Position(0.0, 0.0));
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(2, (*iterator)(0));
+  EXPECT_EQ(0, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(3, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(4, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}
+
+TEST(LineIterator, EndOutsideMap)
+{
+  GridMap map( { "types" });
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
+
+  EXPECT_NO_THROW(LineIterator iterator(map, Position(0.0, 0.0), Position(9.0, 6.0)));
+  LineIterator iterator(map, Position(0.0, 0.0), Position(9.0, 6.0));
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(4, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(3, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(2, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}
+
+TEST(LineIterator, StartAndEndOutsideMap)
+{
+  GridMap map( { "types" });
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
+
+  EXPECT_NO_THROW(LineIterator iterator(map, Position(-7.0, -9.0), Position(8.0, 8.0)));
+  LineIterator iterator(map, Position(-7.0, -9.0), Position(8.0, 8.0));
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(5, (*iterator)(0));
+  EXPECT_EQ(4, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(4, (*iterator)(0));
+  EXPECT_EQ(3, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(3, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  ++iterator;
+  ++iterator;
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}
+
+TEST(LineIterator, StartAndEndOutsideMapWithoutIntersectingMap)
+{
+  GridMap map( { "types" });
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
+
+  EXPECT_THROW(LineIterator iterator(map, Position(-8.0, 8.0), Position(8.0, 8.0)), std::invalid_argument);
+}
+
+TEST(LineIterator, MovedMap)
+{
+  GridMap map( { "types" });
+  map.setGeometry(Length(7.0, 5.0), 1.0, Position(0.0, 0.0));
+  map.move(Position(2.0, 2.0));
+
+  EXPECT_NO_THROW(LineIterator iterator(map, Position(0.0, 0.0), Position(2.0, 2.0)));
+  LineIterator iterator(map, Position(0.0, 0.0), Position(2.0, 2.0));
+  Position point;
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  map.getPosition(*iterator, point);
+  EXPECT_EQ(0, point.x());
+  EXPECT_EQ(0, point.y());
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  map.getPosition(*iterator, point);
+  EXPECT_EQ(1, point.x());
+  EXPECT_EQ(1, point.y());
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  map.getPosition(*iterator, point);
+  EXPECT_EQ(2, point.x());
+  EXPECT_EQ(2, point.y());
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}
+
+TEST(LineIterator, StartAndEndOutsideMovedMap)
+{
+  GridMap map( { "types" });
+  map.setGeometry(Length(7.0, 5.0), 1.0, Position(0.0, 0.0));
+  map.move(Position(2.0, 2.0));
+
+  EXPECT_NO_THROW(LineIterator iterator(map, Position(0.0, 0.0), Position(8.0, 8.0)));
+  LineIterator iterator(map, Position(0.0, 0.0), Position(8.0, 8.0));
+  Position point;
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  map.getPosition(*iterator, point);
+  EXPECT_EQ(0, point.x());
+  EXPECT_EQ(0, point.y());
+
+  ++iterator;
+  map.getPosition(*iterator, point);
+  EXPECT_EQ(1, point.x());
+  EXPECT_EQ(1, point.y());
+  //
+  ++iterator;
+  map.getPosition(*iterator, point);
+  EXPECT_EQ(2, point.x());
+  EXPECT_EQ(2, point.y());
+  //
+  ++iterator;
+  map.getPosition(*iterator, point);
+  EXPECT_EQ(3, point.x());
+  EXPECT_EQ(3, point.y());
+
+  ++iterator;
+  map.getPosition(*iterator, point);
+  EXPECT_EQ(4, point.x());
+  EXPECT_EQ(4, point.y());
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}
+
+}  // namespace grid_map

test/PolygonIteratorTest.cpp

@@ -0,0 +1,196 @@
+/*
+ * PolygonIteratorTest.cpp
+ *
+ *  Created on: Sep 19, 2014
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/iterators/PolygonIterator.hpp"
+#include "grid_map_core/GridMap.hpp"
+#include "grid_map_core/Polygon.hpp"
+
+// gtest
+#include <gtest/gtest.h>
+
+// Vector
+#include <vector>
+
+using grid_map::GridMap;
+using grid_map::Length;
+using grid_map::Polygon;
+using grid_map::PolygonIterator;
+using grid_map::Position;
+
+TEST(PolygonIterator, FullCover) {
+  std::vector<std::string> types;
+  types.emplace_back("type");
+  GridMap map(types);
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
+
+  Polygon polygon;
+  polygon.addVertex(Position(-100.0, 100.0));
+  polygon.addVertex(Position(100.0, 100.0));
+  polygon.addVertex(Position(100.0, -100.0));
+  polygon.addVertex(Position(-100.0, -100.0));
+
+  PolygonIterator iterator(map, polygon);
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(0, (*iterator)(0));
+  EXPECT_EQ(0, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(0, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(0, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  for (int i = 0; i < 37; ++i) {
+    ++iterator;
+  }
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(7, (*iterator)(0));
+  EXPECT_EQ(4, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}
+
+TEST(PolygonIterator, Outside)
+{
+  GridMap map({"types"});
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
+
+  Polygon polygon;
+  polygon.addVertex(Position(99.0, 101.0));
+  polygon.addVertex(Position(101.0, 101.0));
+  polygon.addVertex(Position(101.0, 99.0));
+  polygon.addVertex(Position(99.0, 99.0));
+
+  PolygonIterator iterator(map, polygon);
+
+  EXPECT_TRUE(iterator.isPastEnd());
+}
+
+TEST(PolygonIterator, Square)
+{
+  GridMap map({"types"});
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
+
+  Polygon polygon;
+  polygon.addVertex(Position(-1.0, 1.5));
+  polygon.addVertex(Position(1.0, 1.5));
+  polygon.addVertex(Position(1.0, -1.5));
+  polygon.addVertex(Position(-1.0, -1.5));
+
+  PolygonIterator iterator(map, polygon);
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(3, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(3, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(3, (*iterator)(0));
+  EXPECT_EQ(3, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(4, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(4, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(4, (*iterator)(0));
+  EXPECT_EQ(3, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}
+
+TEST(PolygonIterator, TopLeftTriangle)
+{
+  GridMap map({"types"});
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
+
+  Polygon polygon;
+  polygon.addVertex(Position(-40.1, 20.6));
+  polygon.addVertex(Position(40.1, 20.4));
+  polygon.addVertex(Position(-40.1, -20.6));
+
+  PolygonIterator iterator(map, polygon);
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(0, (*iterator)(0));
+  EXPECT_EQ(0, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(1, (*iterator)(0));
+  EXPECT_EQ(0, (*iterator)(1));
+
+  // TODO Extend.
+}
+
+TEST(PolygonIterator, MoveMap)
+{
+  GridMap map({"layer"});
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
+  map.move(Position(2.0, 0.0));
+
+  Polygon polygon;
+  polygon.addVertex(Position(6.1, 1.6));
+  polygon.addVertex(Position(0.9, 1.6));
+  polygon.addVertex(Position(0.9, -1.6));
+  polygon.addVertex(Position(6.1, -1.6));
+  PolygonIterator iterator(map, polygon);
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(6, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(6, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  for (int i = 0; i < 4; ++i) {
+    ++iterator;
+  }
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(7, (*iterator)(0));
+  EXPECT_EQ(3, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(0, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+
+  for (int i = 0; i < 8; ++i) {
+    ++iterator;
+  }
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(2, (*iterator)(0));
+  EXPECT_EQ(3, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}

test/PolygonTest.cpp

@@ -0,0 +1,268 @@
+/*
+ * PolygonTest.cpp
+ *
+ *  Created on: Mar 24, 2015
+ *      Author: Martin Wermelinger, Péter Fankhauser
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/Polygon.hpp"
+
+// gtest
+#include <gtest/gtest.h>
+
+// Eigen
+#include <Eigen/Core>
+
+using Eigen::MatrixXd;
+using Eigen::Vector2d;
+using Eigen::VectorXd;
+
+using grid_map::Length;
+using grid_map::Polygon;
+using grid_map::Position;
+
+TEST(Polygon, getCentroidTriangle)
+{
+  Polygon triangle;
+  triangle.addVertex(Vector2d(0.0, 0.0));
+  triangle.addVertex(Vector2d(1.0, 0.0));
+  triangle.addVertex(Vector2d(0.5, 1.0));
+
+  Position expectedCentroid;
+  expectedCentroid.x() = 1.0 / 3.0 * (1.0 + 0.5);
+  expectedCentroid.y() = 1.0 / 3.0;
+  Position centroid = triangle.getCentroid();
+  EXPECT_DOUBLE_EQ(expectedCentroid.x(), centroid.x());
+  EXPECT_DOUBLE_EQ(expectedCentroid.y(), centroid.y());
+}
+
+TEST(Polygon, getCentroidRectangle)
+{
+  Polygon rectangle;
+  rectangle.addVertex(Vector2d(-2.0, -1.0));
+  rectangle.addVertex(Vector2d(-2.0, 2.0));
+  rectangle.addVertex(Vector2d(1.0, 2.0));
+  rectangle.addVertex(Vector2d(1.0, -1.0));
+
+  Position expectedCentroid(-0.5, 0.5);
+  Position centroid = rectangle.getCentroid();
+  EXPECT_DOUBLE_EQ(expectedCentroid.x(), centroid.x());
+  EXPECT_DOUBLE_EQ(expectedCentroid.y(), centroid.y());
+}
+
+TEST(Polygon, getBoundingBox)
+{
+  Polygon triangle;
+  triangle.addVertex(Vector2d(0.0, 0.0));
+  triangle.addVertex(Vector2d(0.5, -1.2));
+  triangle.addVertex(Vector2d(1.0, 0.0));
+
+  Position expectedCenter(0.5, -0.6);
+  Length expectedLength(1.0, 1.2);
+  Position center;
+  Length length;
+  triangle.getBoundingBox(center, length);
+
+  EXPECT_DOUBLE_EQ(expectedCenter.x(), center.x());
+  EXPECT_DOUBLE_EQ(expectedCenter.y(), center.y());
+  EXPECT_DOUBLE_EQ(expectedLength.x(), length.x());
+  EXPECT_DOUBLE_EQ(expectedLength.y(), length.y());
+}
+
+TEST(Polygon, convexHullPoints)
+{
+  // Test that points which already create a convex shape (square) can be used to create a convex polygon.
+  std::vector<Position> points1;
+  points1.emplace_back(0.0, 0.0);
+  points1.emplace_back(1.0, 0.0);
+  points1.emplace_back(1.0, 1.0);
+  points1.emplace_back(0.0, 1.0);
+  Polygon polygon1 = Polygon::monotoneChainConvexHullOfPoints(points1);
+  EXPECT_EQ(4, polygon1.nVertices());
+  EXPECT_TRUE(polygon1.isInside(Vector2d(0.5, 0.5)));
+  EXPECT_FALSE(polygon1.isInside(Vector2d(-0.01, 0.5)));
+
+  // Test that a random set of points can be used to create a convex polygon.
+  std::vector<Position> points2;
+  points2.emplace_back(0.0, 0.0);
+  points2.emplace_back(1.0, 0.0);
+  points2.emplace_back(2.0, 1.0);
+  points2.emplace_back(1.0, 2.0);
+  points2.emplace_back(-1.0, 2.0);
+  points2.emplace_back(-1.0, -2.0);
+  points2.emplace_back(0.0, 1.0);
+  points2.emplace_back(1.0, 1.0);
+  Polygon polygon2 = Polygon::monotoneChainConvexHullOfPoints(points2);
+  EXPECT_EQ(4, polygon2.nVertices());
+  EXPECT_TRUE(polygon2.isInside(Vector2d(0.5, 0.5)));
+  EXPECT_TRUE(polygon2.isInside(Vector2d(0.0, 1.0)));
+  EXPECT_TRUE(polygon2.isInside(Vector2d(-0.5, -0.5)));
+  EXPECT_FALSE(polygon2.isInside(Vector2d(2.0, 0.0)));
+  EXPECT_FALSE(polygon2.isInside(Vector2d(-0.5, -2)));
+  EXPECT_FALSE(polygon2.isInside(Vector2d(1.75, 1.75)));
+}
+
+TEST(Polygon, convexHullPolygon)
+{
+  Polygon polygon1;
+  polygon1.addVertex(Vector2d(0.0, 0.0));
+  polygon1.addVertex(Vector2d(1.0, 1.0));
+  polygon1.addVertex(Vector2d(0.0, 1.0));
+  polygon1.addVertex(Vector2d(1.0, 0.0));
+
+  Polygon polygon2;
+  polygon2.addVertex(Vector2d(0.5, 0.5));
+  polygon2.addVertex(Vector2d(0.5, 1.5));
+  polygon2.addVertex(Vector2d(1.5, 0.5));
+  polygon2.addVertex(Vector2d(1.5, 1.5));
+
+  Polygon hull = Polygon::convexHull(polygon1, polygon2);
+
+  EXPECT_EQ(6, hull.nVertices());
+  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.5)));
+  EXPECT_FALSE(hull.isInside(Vector2d(0.01, 1.49)));
+}
+
+TEST(Polygon, convexHullCircles)
+{
+  Position center1(0.0, 0.0);
+  Position center2(1.0, 0.0);
+  double radius = 0.5;
+  const int nVertices = 15;
+
+  Polygon hull = Polygon::convexHullOfTwoCircles(center1, center2, radius);
+  EXPECT_EQ(20, hull.nVertices());
+  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
+  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.0)));
+  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.4)));
+  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.6)));
+  EXPECT_FALSE(hull.isInside(Vector2d(1.5, 0.2)));
+
+  hull = Polygon::convexHullOfTwoCircles(center1, center2, radius, nVertices);
+  EXPECT_EQ(nVertices + 1, hull.nVertices());
+  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
+  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.0)));
+  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.4)));
+  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.6)));
+  EXPECT_FALSE(hull.isInside(Vector2d(1.5, 0.2)));
+
+  hull = Polygon::convexHullOfTwoCircles(center1, center1, radius);
+  EXPECT_EQ(20, hull.nVertices());
+  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
+  EXPECT_TRUE(hull.isInside(Vector2d(0.25, 0.0)));
+  EXPECT_TRUE(hull.isInside(Vector2d(0.0, 0.25)));
+  EXPECT_TRUE(hull.isInside(Vector2d(0.0, -0.25)));
+  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.5)));
+  EXPECT_FALSE(hull.isInside(Vector2d(0.6, 0.0)));
+  EXPECT_FALSE(hull.isInside(Vector2d(-0.6, 0.0)));
+  EXPECT_FALSE(hull.isInside(Vector2d(0.0, 0.6)));
+  EXPECT_FALSE(hull.isInside(Vector2d(0.0, -0.6)));
+
+  hull = Polygon::convexHullOfTwoCircles(center1, center1, radius, nVertices);
+  EXPECT_EQ(nVertices, hull.nVertices());
+  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
+  EXPECT_TRUE(hull.isInside(Vector2d(0.25, 0.0)));
+  EXPECT_TRUE(hull.isInside(Vector2d(0.0, 0.25)));
+  EXPECT_TRUE(hull.isInside(Vector2d(0.0, -0.25)));
+  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.5)));
+  EXPECT_FALSE(hull.isInside(Vector2d(0.6, 0.0)));
+  EXPECT_FALSE(hull.isInside(Vector2d(-0.6, 0.0)));
+  EXPECT_FALSE(hull.isInside(Vector2d(0.0, 0.6)));
+  EXPECT_FALSE(hull.isInside(Vector2d(0.0, -0.6)));
+}
+
+TEST(Polygon, convexHullCircle)
+{
+  Position center(0.0, 0.0);
+  double radius = 0.5;
+  const int nVertices = 15;
+
+  Polygon hull = Polygon::fromCircle(center, radius);
+
+  EXPECT_EQ(20, hull.nVertices());
+  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
+  EXPECT_TRUE(hull.isInside(Vector2d(0.49, 0.0)));
+  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.4)));
+  EXPECT_FALSE(hull.isInside(Vector2d(1.0, 0.0)));
+
+  hull = Polygon::fromCircle(center, radius, nVertices);
+  EXPECT_EQ(nVertices, hull.nVertices());
+  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
+  EXPECT_TRUE(hull.isInside(Vector2d(0.49, 0.0)));
+  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.4)));
+  EXPECT_FALSE(hull.isInside(Vector2d(1.0, 0.0)));
+}
+
+TEST(convertToInequalityConstraints, triangle1)
+{
+  Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.1, -1.1)});
+  MatrixXd A;
+  VectorXd b;
+  ASSERT_TRUE(polygon.convertToInequalityConstraints(A, b));
+  EXPECT_NEAR(-1.3636, A(0, 0), 1e-4);
+  EXPECT_NEAR( 1.3636, A(0, 1), 1e-4);
+  EXPECT_NEAR(-1.5000, A(1, 0), 1e-4);
+  EXPECT_NEAR(-1.5000, A(1, 1), 1e-4);
+  EXPECT_NEAR( 2.8636, A(2, 0), 1e-4);
+  EXPECT_NEAR( 0.1364, A(2, 1), 1e-4);
+  EXPECT_NEAR( 0.0000, b(0), 1e-4);
+  EXPECT_NEAR( 0.0000, b(1), 1e-4);
+  EXPECT_NEAR( 3.0000, b(2), 1e-4);
+}
+
+TEST(convertToInequalityConstraints, triangle2)
+{
+  Polygon polygon({Position(-1.0, 0.5), Position(-1.0, -0.5), Position(1.0, -0.5)});
+  MatrixXd A;
+  VectorXd b;
+  ASSERT_TRUE(polygon.convertToInequalityConstraints(A, b));
+  EXPECT_NEAR(-1.5000, A(0, 0), 1e-4);
+  EXPECT_NEAR( 0.0000, A(0, 1), 1e-4);
+  EXPECT_NEAR( 0.0000, A(1, 0), 1e-4);
+  EXPECT_NEAR(-3.0000, A(1, 1), 1e-4);
+  EXPECT_NEAR( 1.5000, A(2, 0), 1e-4);
+  EXPECT_NEAR( 3.0000, A(2, 1), 1e-4);
+  EXPECT_NEAR( 1.5000, b(0), 1e-4);
+  EXPECT_NEAR( 1.5000, b(1), 1e-4);
+  EXPECT_NEAR( 0.0000, b(2), 1e-4);
+}
+
+TEST(offsetInward, triangle)
+{
+  Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.0, -1.0)});
+  polygon.offsetInward(0.1);
+  EXPECT_NEAR(0.9, polygon.getVertex(0)(0), 1e-4);
+  EXPECT_NEAR(0.758579, polygon.getVertex(0)(1), 1e-4);
+  EXPECT_NEAR(0.141421, polygon.getVertex(1)(0), 1e-4);
+  EXPECT_NEAR(0.0, polygon.getVertex(1)(1), 1e-4);
+  EXPECT_NEAR(0.9, polygon.getVertex(2)(0), 1e-4);
+  EXPECT_NEAR(-0.758579, polygon.getVertex(2)(1), 1e-4);
+}
+
+TEST(triangulation, triangle)
+{
+  Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.0, -1.0)});
+  std::vector<Polygon> polygons;
+  polygons = polygon.triangulate();
+  ASSERT_EQ(1, polygons.size());
+  EXPECT_EQ(polygon.getVertex(0).x(), polygons[0].getVertex(0).x());
+  EXPECT_EQ(polygon.getVertex(0).y(), polygons[0].getVertex(0).y());
+  EXPECT_EQ(polygon.getVertex(1).x(), polygons[0].getVertex(1).x());
+  EXPECT_EQ(polygon.getVertex(1).y(), polygons[0].getVertex(1).y());
+  EXPECT_EQ(polygon.getVertex(2).x(), polygons[0].getVertex(2).x());
+  EXPECT_EQ(polygon.getVertex(2).y(), polygons[0].getVertex(2).y());
+}
+
+TEST(triangulation, rectangle)
+{
+  Polygon rectangle;
+  rectangle.addVertex(Vector2d(-2.0, -1.0));
+  rectangle.addVertex(Vector2d(-2.0, 2.0));
+  rectangle.addVertex(Vector2d(1.0, 2.0));
+  rectangle.addVertex(Vector2d(1.0, -1.0));
+  std::vector<Polygon> polygons;
+  polygons = rectangle.triangulate();
+  ASSERT_EQ(2, polygons.size());
+  // TODO Extend.
+}

test/SlidingWindowIteratorTest.cpp

@@ -0,0 +1,153 @@
+/*
+ * SlidingWindowIteratorTest.cpp
+ *
+ *  Created on: Aug 16, 2017
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich
+ */
+
+#include "grid_map_core/iterators/SlidingWindowIterator.hpp"
+#include "grid_map_core/GridMap.hpp"
+
+#include <gtest/gtest.h>
+#include <vector>
+
+using grid_map::GridMap;
+using grid_map::Index;
+using grid_map::Length;
+using grid_map::Position;
+using grid_map::SlidingWindowIterator;
+
+TEST(SlidingWindowIterator, WindowSize3Cutoff)
+{
+  GridMap map;
+  map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
+  map.add("layer");
+  map["layer"].setRandom();
+
+  SlidingWindowIterator iterator(map, "layer", SlidingWindowIterator::EdgeHandling::CROP, 3);
+  EXPECT_EQ(iterator.getData().rows(), 2);
+  EXPECT_EQ(iterator.getData().cols(), 2);
+  EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 2, 2)));
+
+  ++iterator;
+  EXPECT_EQ(iterator.getData().rows(), 3);
+  EXPECT_EQ(iterator.getData().cols(), 2);
+  EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 3, 2)));
+
+  ++iterator;
+  EXPECT_EQ(iterator.getData().rows(), 3);
+  EXPECT_EQ(iterator.getData().cols(), 2);
+  EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(1, 0, 3, 2)));
+
+  for (; !iterator.isPastEnd(); ++iterator) {
+    EXPECT_FALSE(iterator.isPastEnd());
+    if ((*iterator == Index(3, 2)).all()) {
+      break;
+    }
+  }
+
+  EXPECT_EQ(iterator.getData().rows(), 3);
+  EXPECT_EQ(iterator.getData().cols(), 3);
+  EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(2, 1, 3, 3)));
+
+  for (; !iterator.isPastEnd(); ++iterator) {
+    EXPECT_FALSE(iterator.isPastEnd());
+    if ((*iterator == Index(7, 4)).all()) {
+      break;
+    }
+  }
+
+  EXPECT_EQ(iterator.getData().rows(), 2);
+  EXPECT_EQ(iterator.getData().cols(), 2);
+  EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(6, 3, 2, 2)));
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}
+
+TEST(SlidingWindowIterator, WindowSize5)
+{
+  GridMap map;
+  map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
+  map.add("layer");
+  map["layer"].setRandom();
+
+  SlidingWindowIterator iterator(map, "layer", SlidingWindowIterator::EdgeHandling::CROP, 5);
+  EXPECT_EQ(iterator.getData().rows(), 3);
+  EXPECT_EQ(iterator.getData().cols(), 3);
+  EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 3, 3)));
+
+  ++iterator;
+  EXPECT_EQ(iterator.getData().rows(), 4);
+  EXPECT_EQ(iterator.getData().cols(), 3);
+  EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 4, 3)));
+
+  ++iterator;
+  EXPECT_EQ(iterator.getData().rows(), 5);
+  EXPECT_EQ(iterator.getData().cols(), 3);
+  EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 5, 3)));
+
+  for (; !iterator.isPastEnd(); ++iterator) {
+    EXPECT_FALSE(iterator.isPastEnd());
+    if ((*iterator == Index(3, 2)).all()) {
+      break;
+    }
+  }
+
+  EXPECT_EQ(iterator.getData().rows(), 5);
+  EXPECT_EQ(iterator.getData().cols(), 5);
+  EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(1, 0, 5, 5)));
+
+  for (; !iterator.isPastEnd(); ++iterator) {
+    EXPECT_FALSE(iterator.isPastEnd());
+    if ((*iterator == Index(7, 4)).all()) {
+      break;
+    }
+  }
+
+  EXPECT_EQ(iterator.getData().rows(), 3);
+  EXPECT_EQ(iterator.getData().cols(), 3);
+  EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(5, 2, 3, 3)));
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}
+
+TEST(SlidingWindowIterator, WindowSize3Inside)
+{
+  GridMap map;
+  map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
+  map.add("layer");
+  map["layer"].setRandom();
+
+  SlidingWindowIterator iterator(map, "layer", SlidingWindowIterator::EdgeHandling::INSIDE, 3);
+  EXPECT_EQ(iterator.getData().rows(), 3);
+  EXPECT_EQ(iterator.getData().cols(), 3);
+  EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 3, 3)));
+
+  for (; !iterator.isPastEnd(); ++iterator) {
+    EXPECT_FALSE(iterator.isPastEnd());
+    if ((*iterator == Index(3, 2)).all()) {
+      break;
+    }
+  }
+
+  EXPECT_EQ(iterator.getData().rows(), 3);
+  EXPECT_EQ(iterator.getData().cols(), 3);
+  EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(2, 1, 3, 3)));
+
+  for (; !iterator.isPastEnd(); ++iterator) {
+    EXPECT_FALSE(iterator.isPastEnd());
+    if ((*iterator == Index(6, 3)).all()) {
+      break;
+    }
+  }
+
+  EXPECT_EQ(iterator.getData().rows(), 3);
+  EXPECT_EQ(iterator.getData().cols(), 3);
+  EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(5, 2, 3, 3)));
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}

test/SpiralIteratorTest.cpp

@@ -0,0 +1,36 @@
+/*
+ * SpiralIteratorTest.cpp
+ *
+ *  Created on: Jul 26, 2017
+ *      Author: Benjamin Scholz
+ *	 Institute: University of Hamburg, TAMS
+ */
+
+#include "grid_map_core/iterators/SpiralIterator.hpp"
+#include "grid_map_core/GridMap.hpp"
+
+// gtest
+#include <gtest/gtest.h>
+
+// Vector
+#include <vector>
+
+using grid_map::GridMap;
+using grid_map::Length;
+using grid_map::Position;
+using grid_map::SpiralIterator;
+
+TEST(SpiralIterator, CenterOutOfMap)
+{
+  GridMap map( { "types" });
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
+  Position center(8.0, 0.0);
+  double radius = 5.0;
+
+  SpiralIterator iterator(map, center, radius);
+
+  Position iterator_position;
+  map.getPosition(*iterator, iterator_position);
+
+  EXPECT_TRUE(map.isInside(iterator_position));
+}

test/SubmapIteratorTest.cpp

@@ -0,0 +1,267 @@
+/*
+ * SubmapIteratorTest.cpp
+ *
+ *  Created on: Sep 15, 2014
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_core/GridMap.hpp"
+#include "grid_map_core/iterators/SubmapIterator.hpp"
+
+// Eigen
+#include <Eigen/Core>
+
+// gtest
+#include <gtest/gtest.h>
+
+// Vector
+#include <vector>
+
+using std::vector;
+using std::string;
+
+namespace grid_map{
+
+TEST(SubmapIterator, Simple) {
+  Eigen::Array2i submapTopLeftIndex(3, 1);
+  Eigen::Array2i submapBufferSize(3, 2);
+  Eigen::Array2i index;
+  Eigen::Array2i submapIndex;
+
+  vector<string> types;
+  types.emplace_back("type");
+  GridMap map(types);
+  map.setGeometry(Eigen::Array2d(8.1, 5.1), 1.0, Eigen::Vector2d(0.0, 0.0));  // bufferSize(8, 5)
+
+  SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize);
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(submapTopLeftIndex(0), (*iterator)(0));
+  EXPECT_EQ(submapTopLeftIndex(1), (*iterator)(1));
+  EXPECT_EQ(0, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(0, iterator.getSubmapIndex()(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(3, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+  EXPECT_EQ(0, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(1, iterator.getSubmapIndex()(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(4, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+  EXPECT_EQ(1, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(0, iterator.getSubmapIndex()(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(4, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+  EXPECT_EQ(1, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(1, iterator.getSubmapIndex()(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(5, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+  EXPECT_EQ(2, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(0, iterator.getSubmapIndex()(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(5, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+  EXPECT_EQ(2, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(1, iterator.getSubmapIndex()(1));
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+  EXPECT_EQ(5, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+  EXPECT_EQ(2, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(1, iterator.getSubmapIndex()(1));
+}
+
+TEST(SubmapIterator, CircularBuffer) {
+  Eigen::Array2i submapTopLeftIndex(6, 3);
+  Eigen::Array2i submapBufferSize(2, 4);
+  Eigen::Array2i index;
+  Eigen::Array2i submapIndex;
+
+  vector<string> types;
+  types.emplace_back("type");
+  GridMap map(types);
+  map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0));  // bufferSize(8, 5)
+  map.move(Position(-3.0, -2.0));                              // bufferStartIndex(3, 2)
+
+  SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize);
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(submapTopLeftIndex(0), (*iterator)(0));
+  EXPECT_EQ(submapTopLeftIndex(1), (*iterator)(1));
+  EXPECT_EQ(0, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(0, iterator.getSubmapIndex()(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(6, (*iterator)(0));
+  EXPECT_EQ(4, (*iterator)(1));
+  EXPECT_EQ(0, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(1, iterator.getSubmapIndex()(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(6, (*iterator)(0));
+  EXPECT_EQ(0, (*iterator)(1));
+  EXPECT_EQ(0, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(2, iterator.getSubmapIndex()(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(6, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+  EXPECT_EQ(0, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(3, iterator.getSubmapIndex()(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(7, (*iterator)(0));
+  EXPECT_EQ(3, (*iterator)(1));
+  EXPECT_EQ(1, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(0, iterator.getSubmapIndex()(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(7, (*iterator)(0));
+  EXPECT_EQ(4, (*iterator)(1));
+  EXPECT_EQ(1, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(1, iterator.getSubmapIndex()(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(7, (*iterator)(0));
+  EXPECT_EQ(0, (*iterator)(1));
+  EXPECT_EQ(1, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(2, iterator.getSubmapIndex()(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(7, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+  EXPECT_EQ(1, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(3, iterator.getSubmapIndex()(1));
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+  EXPECT_EQ(7, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+  EXPECT_EQ(1, iterator.getSubmapIndex()(0));
+  EXPECT_EQ(3, iterator.getSubmapIndex()(1));
+}
+
+/**
+ * The submap should contain the same elements as before even after moving the underlying map.
+ *
+ *                                                          +----------------------------+
+ *                                                          |                            |
+ *                                                          |                            |
+ *              +----------------------------+              |                            |
+ *              |0  0  0  0  0  0  0  0  0  0|              |      0  0  0  0  0  0  0  0|
+ *              |     +----+                 |              |           +----+           |
+ * Submap       |1  1 |1  1| 1  1  1  1  1  1|              |      1  1 |1  1| 1  1  1  1|
+ *           +------> |    |                 |              |           |    |           |
+ *              |2  2 |2  2| 2  2  2  2  2  2|              |      2  2 |2  2| 2  2  2  2|
+ *              |     +----+                 |              |           +----+           |
+ *              |3  3  3  3  3  3  3  3  3  3|   Move       |      3  3  3  3  3  3  3  3|
+ *              |                            |              |                            |
+ *              |4  4  4  4  4  4  4  4  4  4| +--------->  |      4  4  4  4  4  4  4  4|
+ *              |                            |              |                            |
+ *              |5  5  5  5  5  5  5  5  5  5|              |      5  5  5  5  5  5  5  5|
+ *              |                            |              |                            |
+ *              |6  6  6  6  6  6  6  6  6  6|              |      6  6  6  6  6  6  6  6|
+ *              |                            |              |                            |
+ *              |7  7  7  7  7  7  7  7  7  7|              |      7  7  7  7  7  7  7  7|
+ *              |                            |              +----------------------------+
+ *              |8  8  8  8  8  8  8  8  8  8|
+ *              |                            |
+ *              |9  9  9  9  9  9  9  9  9  9|
+ *              +----------------------------+
+ */
+TEST(SubmapIterator, InterleavedExecutionWithMove) {
+  grid_map::Index submapTopLeftIndex(3, 1);
+  grid_map::Size submapSize(2, 2);
+
+  GridMap map({"layer"});
+
+  map.setGeometry(Length(10, 10), 1.0, Position(0.0, 0.0));  // bufferSize(8, 5)
+
+  auto& layer = map.get("layer");
+
+  // Initialize the layer as sketched.
+  for (long colIndex = 0; colIndex < layer.cols(); colIndex++) {
+    layer.col(colIndex).setConstant(static_cast<DataType>(colIndex));
+  }
+
+  std::cout << "(4,7) contains " << map.at("layer", {4, 7}) << std::endl;
+  // Instantiate the submap iterator as sketched.
+  SubmapIterator iterator(map, submapTopLeftIndex, submapSize);
+
+  // check that the submap iterator returns {1,1,2,2}
+  auto checkCorrectValues = [](std::array<double, 4> given) {
+    int countOnes = 0;
+    int countTwos = 0;
+    for (auto& value : given) {
+      if (std::abs(value - 1.0) < 1e-6) {
+        countOnes++;
+      } else if (std::abs(value - 2.0) < 1e-6) {
+        countTwos++;
+      } else {
+        FAIL() << "Submap iterator returned unexpected value.";
+      }
+    }
+    EXPECT_EQ(countOnes, 2);
+    EXPECT_EQ(countTwos, 2);
+  };
+
+  std::array<double, 4> returnedSequence{};
+  returnedSequence.fill(0);
+
+  for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
+    returnedSequence.at(submapIndex) = map.at("layer", *iterator);
+    ++iterator;
+  }
+
+  checkCorrectValues(returnedSequence);
+
+  // Reset the iterator and now check that it still returns the same sequence when we move the map interleaved with iterating.
+  iterator = SubmapIterator(map, submapTopLeftIndex, submapSize);
+  returnedSequence.fill(0);
+  for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
+    if (submapIndex == 2) {
+      // Now move the map as depicted.
+      map.move(Position(2.0, 2.0));
+    }
+    returnedSequence.at(submapIndex) = map.at("layer", *iterator);
+    ++iterator;
+  }
+  checkCorrectValues(returnedSequence);
+
+  // TODO (mwulf, mgaertner): This behavior is not yet implemented:
+  //
+  //  // Reset the iterator and now check that the iterator throws? if the submap moved out of range.
+  //  iterator = SubmapIterator(map, submapTopLeftIndex, submapSize);
+  //
+  //  EXPECT_ANY_THROW(for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
+  //    if (submapIndex == 2) {
+  //      // Now move the map so that the submap gets out of range.
+  //      map.move(Position(20.0, 20.0));
+  //    }
+  //    returnedSequence.at(submapIndex) = map.at("layer", *iterator);
+  //    ++iterator;
+  //  });
+}
+
+}  // namespace grid_map

test/test_grid_map_core.cpp

@@ -0,0 +1,18 @@
+/*
+ * test_grid_map.cpp
+ *
+ *  Created on: Feb 10, 2014
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, ANYbotics
+ */
+
+// gtest
+#include <gtest/gtest.h>
+
+// Run all the tests that were declared with TEST()
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  srand((int)time(nullptr));
+  return RUN_ALL_TESTS();
+}

test/test_helpers.cpp

@@ -0,0 +1,232 @@
+/*
+ * test_helpers.cpp
+ *
+ *  Created on: Mar 3, 2020
+ *      Author: Edo Jelavic
+ *      Institute: ETH Zurich, Robotic Systems Lab
+ */
+
+#include "test_helpers.hpp"
+
+#include "grid_map_core/GridMap.hpp"
+#include "grid_map_core/iterators/GridMapIterator.hpp"
+
+// gtest
+#include <gtest/gtest.h>
+
+namespace grid_map_test {
+
+std::mt19937 rndGenerator;
+
+AnalyticalFunctions createFlatWorld(grid_map::GridMap *map)
+{
+
+  AnalyticalFunctions func;
+
+  func.f_ = [](double  /*x*/, double  /*y*/) {
+    return 0.0;
+  };
+
+  fillGridMap(map, func);
+
+  return func;
+
+}
+
+AnalyticalFunctions createRationalFunctionWorld(grid_map::GridMap *map)
+{
+
+  AnalyticalFunctions func;
+
+  std::uniform_real_distribution<double> shift(-3.0, 3.0);
+  std::uniform_real_distribution<double> scale(1.0, 20.0);
+  const double x0 = shift(rndGenerator);
+  const double y0 = shift(rndGenerator);
+  const double s = scale(rndGenerator);
+
+  func.f_ = [x0, y0,s](double x, double y) {
+    return s / (1 + std::pow(x-x0, 2.0) + std::pow(y-y0, 2.0));
+  };
+
+  fillGridMap(map, func);
+
+  return func;
+
+}
+
+AnalyticalFunctions createSecondOrderPolyWorld(grid_map::GridMap *map)
+{
+
+  AnalyticalFunctions func;
+
+  func.f_ = [](double x,double y) {
+    return (-x*x -y*y +2.0*x*y +x*x*y*y);
+  };
+
+  fillGridMap(map, func);
+
+  return func;
+
+}
+
+AnalyticalFunctions createSaddleWorld(grid_map::GridMap *map)
+{
+  AnalyticalFunctions func;
+
+  func.f_ = [](double x,double y) {
+    return (x*x-y*y);
+  };
+
+  fillGridMap(map, func);
+
+  return func;
+
+}
+
+AnalyticalFunctions createSineWorld(grid_map::GridMap *map)
+{
+
+  AnalyticalFunctions func;
+
+  std::uniform_real_distribution<double> Uw(0.1, 4.0);
+  const double w1 = Uw(rndGenerator);
+  const double w2 = Uw(rndGenerator);
+  const double w3 = Uw(rndGenerator);
+  const double w4 = Uw(rndGenerator);
+
+  func.f_ = [w1,w2,w3,w4](double x,double y) {
+    return std::cos(w1*x) + std::sin(w2*y) + std::cos(w3*x) + std::sin(w4*y);
+  };
+
+  fillGridMap(map, func);
+
+  return func;
+
+}
+
+AnalyticalFunctions createTanhWorld(grid_map::GridMap *map)
+{
+
+  AnalyticalFunctions func;
+
+  std::uniform_real_distribution<double> scaling(0.1, 2.0);
+  const double s = scaling(rndGenerator);
+  func.f_ = [s](double x,double  /*y*/) {
+    const double expZ = std::exp(2 *s* x);
+    return (expZ - 1) / (expZ + 1);
+  };
+
+  fillGridMap(map, func);
+
+  return func;
+}
+
+AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map)
+{
+
+  struct Gaussian
+  {
+    double x0, y0;
+    double varX, varY;
+    double s;
+  };
+
+  AnalyticalFunctions func;
+
+  std::uniform_real_distribution<double> var(0.1, 3.0);
+  std::uniform_real_distribution<double> mean(-4.0, 4.0);
+  std::uniform_real_distribution<double> scale(-3.0, 3.0);
+  constexpr int numGaussians = 3;
+  std::array<Gaussian, numGaussians> g;
+
+  for (int i = 0; i < numGaussians; ++i) {
+    g.at(i).x0 = mean(rndGenerator);
+    g.at(i).y0 = mean(rndGenerator);
+    g.at(i).varX = var(rndGenerator);
+    g.at(i).varY = var(rndGenerator);
+    g.at(i).s = scale(rndGenerator);
+  }
+
+  func.f_ = [g](double x,double y) {
+    double value = 0.0;
+    for (const auto & i : g) {
+      const double x0 = i.x0;
+      const double y0 = i.y0;
+      const double varX = i.varX;
+      const double varY = i.varY;
+      const double s = i.s;
+        value += s * std::exp(-(x-x0)*(x-x0) / (2.0*varX) - (y-y0)*(y-y0) / (2.0 * varY));
+    }
+
+    return value;
+  };
+
+  fillGridMap(map, func);
+
+  return func;
+}
+
+void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions)
+{
+  using grid_map::DataType;
+  using grid_map::GridMapIterator;
+  using grid_map::Index;
+  using grid_map::Matrix;
+  using grid_map::Position;
+
+  Matrix& data = (*map)[testLayer];
+  for (GridMapIterator iterator(*map); !iterator.isPastEnd(); ++iterator) {
+    const Index index(*iterator);
+    Position pos;
+    map->getPosition(index, pos);
+    data(index(0), index(1)) = static_cast<DataType>(functions.f_(pos.x(), pos.y()));
+  }
+}
+
+grid_map::GridMap createMap(const grid_map::Length &length, double resolution,
+                            const grid_map::Position &pos)
+{
+  grid_map::GridMap map;
+
+  map.setGeometry(length, resolution, pos);
+  map.add(testLayer, 0.0);
+  map.setFrameId("map");
+
+  return map;
+}
+
+std::vector<Point2D> uniformlyDitributedPointsWithinMap(const grid_map::GridMap &map,
+                                                       unsigned int numPoints)
+{
+
+  // stay away from the edges
+  // on the edges the cubic interp is invalid. Not enough points.
+  const double dimX = map.getLength().x() / 2.0 - 3.0 * map.getResolution();
+  const double dimY = map.getLength().y() / 2.0 - 3.0 * map.getResolution();
+  std::uniform_real_distribution<double> Ux(-dimX, dimX);
+  std::uniform_real_distribution<double> Uy(-dimY, dimY);
+
+  std::vector<Point2D> points(numPoints);
+  for (auto &point : points) {
+    point.x_ = Ux(rndGenerator);
+    point.y_ = Uy(rndGenerator);
+  }
+
+  return points;
+}
+
+void verifyValuesAtQueryPointsAreClose(const grid_map::GridMap &map, const AnalyticalFunctions &trueValues,
+                               const std::vector<Point2D> &queryPoints,
+                               grid_map::InterpolationMethods interpolationMethod){
+  for (const auto point : queryPoints) {
+    const grid_map::Position p(point.x_, point.y_);
+    const double trueValue = trueValues.f_(p.x(), p.y());
+    const double interpolatedValue = map.atPosition(
+        grid_map_test::testLayer, p, interpolationMethod);
+    EXPECT_NEAR(trueValue, interpolatedValue, grid_map_test::maxAbsErrorValue);
+  }
+}
+
+
+} /* namespace grid_map_test */
+

test/test_helpers.hpp

@@ -0,0 +1,96 @@
+/*
+ * test_helpers.hpp
+ *
+ *  Created on: Mar 3, 2020
+ *      Author: Edo Jelavic
+ *      Institute: ETH Zurich, Robotic Systems Lab
+ */
+
+#pragma once
+#include "grid_map_core/TypeDefs.hpp"
+#include <functional>
+#include <vector>
+#include <random>
+
+namespace grid_map {
+  class GridMap;
+}
+
+namespace grid_map_test {
+
+/*
+ * Name of the layer that is used in all tests.
+ * It has no special meaning.
+ */
+static const std::string testLayer = "test";
+
+/*
+ * Class that holds a function pointer to analytical
+ * functions used for evaluation. Analytical functions
+ * are in the form of f = f(x,y). One could also add
+ * derivatives to this class, e.g. for testing the
+ * accuracy of the normal estimation.
+ */
+struct AnalyticalFunctions
+{
+  std::function<double(double, double)> f_;
+};
+
+struct Point2D
+{
+  double x_ = 0.0;
+  double y_ = 0.0;
+};
+
+// Random generator engine.
+extern std::mt19937 rndGenerator;
+
+// Maximal tolerance when comparing doubles in tests.
+const double maxAbsErrorValue = 1e-3;
+
+grid_map::GridMap createMap(const grid_map::Length &length, double resolution,
+                            const grid_map::Position &pos);
+
+/*
+ * Collections of methods that modify the grid map.
+ * All these methods create an analytical function that
+ * describes the value of the layer "testLayer" as a function
+ * of coordinates.  That can be any mathematical function. Inside the test,
+ * sinusoidal, polynomial functions and exponential functions are used.
+ * e.g. f(x,y) = sin(x) + cos(y), f(x,y) = exp(-x*x - y*y)
+ * Grid map is then filled by evaluating
+ * that analytical function over the entire length and width of the
+ * grid map. Grid map thus contains spatially sampled mathematical
+ * function.
+ * Each method returns a structure containing the analytical function.
+ */
+AnalyticalFunctions createFlatWorld(grid_map::GridMap *map);
+AnalyticalFunctions createRationalFunctionWorld(grid_map::GridMap *map);
+AnalyticalFunctions createSaddleWorld(grid_map::GridMap *map);
+AnalyticalFunctions createSecondOrderPolyWorld(grid_map::GridMap *map);
+AnalyticalFunctions createSineWorld(grid_map::GridMap *map);
+AnalyticalFunctions createTanhWorld(grid_map::GridMap *map);
+AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map);
+
+/*
+ * Iterates over the grid map and fill it with values.
+ * values are calculated by evaluating analytical function.
+ */
+void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions);
+
+/*
+ * Create numPoints uniformly distributed random points that lie within the grid map.
+ */
+std::vector<Point2D> uniformlyDitributedPointsWithinMap(const grid_map::GridMap &map,
+                                                        unsigned int numPoints);
+
+/*
+ * For each point in queryPoints, verify that the interpolated value of the grid map
+ * is close to the ground truth which is contained in Analytical functions structure.
+ * Called inside the tests. Calls macros from gtest.
+ */
+void verifyValuesAtQueryPointsAreClose(const grid_map::GridMap &map, const AnalyticalFunctions &trueValues,
+                               const std::vector<Point2D> &queryPoints,
+                               grid_map::InterpolationMethods interpolationMethod);
+
+}  // namespace grid_map_test