commit 99662d6d47a5be42494772b856751e24a3d84fec Author: duongtd Date: Wed Jan 14 15:04:54 2026 +0700 first commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..64c5965 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +# Bỏ qua thư mục build/ +build/ + +CODE_REVIEW.md \ No newline at end of file diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 0000000..7a4acae --- /dev/null +++ b/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 +* 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 `_). +* Fixed error in SpiralIterator when center is outside the map (`#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 `_). +* 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 `_). +* 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 `_). +* New iterator_benchmark demo to exemplify the usage of the iterators and their computational performance (`#45 `_). +* Added new method to set the position of a grid map (`#42 `_). +* Added new move_demo to illustrate the difference between the `move` and `setPosition` method. +* Fixed behavior of checkIfPositionWithinMap() in edge cases (`#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 diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..238f7ba --- /dev/null +++ b/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 + $ + $ + ${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() diff --git a/cmake/grid_map_core-extras.cmake b/cmake/grid_map_core-extras.cmake new file mode 100644 index 0000000..e63eae4 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/doc/grid_map_conventions.pdf b/doc/grid_map_conventions.pdf new file mode 100644 index 0000000..405588f Binary files /dev/null and b/doc/grid_map_conventions.pdf differ diff --git a/doc/grid_map_conventions.png b/doc/grid_map_conventions.png new file mode 100644 index 0000000..d755376 Binary files /dev/null and b/doc/grid_map_conventions.png differ diff --git a/doc/grid_map_layers.pdf b/doc/grid_map_layers.pdf new file mode 100644 index 0000000..bfc4830 Binary files /dev/null and b/doc/grid_map_layers.pdf differ diff --git a/doc/grid_map_layers.png b/doc/grid_map_layers.png new file mode 100644 index 0000000..ef56e52 Binary files /dev/null and b/doc/grid_map_layers.png differ diff --git a/doc/interpolationGaussWorld.gif b/doc/interpolationGaussWorld.gif new file mode 100644 index 0000000..74e35e6 Binary files /dev/null and b/doc/interpolationGaussWorld.gif differ diff --git a/doc/interpolationSineWorld.gif b/doc/interpolationSineWorld.gif new file mode 100644 index 0000000..f8c420f Binary files /dev/null and b/doc/interpolationSineWorld.gif differ diff --git a/doc/iterators/circle_iterator.gif b/doc/iterators/circle_iterator.gif new file mode 100644 index 0000000..d1bc507 Binary files /dev/null and b/doc/iterators/circle_iterator.gif differ diff --git a/doc/iterators/circle_iterator_preview.gif b/doc/iterators/circle_iterator_preview.gif new file mode 100644 index 0000000..0b447d2 Binary files /dev/null and b/doc/iterators/circle_iterator_preview.gif differ diff --git a/doc/iterators/ellipse_iterator.gif b/doc/iterators/ellipse_iterator.gif new file mode 100644 index 0000000..a401b30 Binary files /dev/null and b/doc/iterators/ellipse_iterator.gif differ diff --git a/doc/iterators/ellipse_iterator_preview.gif b/doc/iterators/ellipse_iterator_preview.gif new file mode 100644 index 0000000..18908ac Binary files /dev/null and b/doc/iterators/ellipse_iterator_preview.gif differ diff --git a/doc/iterators/grid_map_iterator.gif b/doc/iterators/grid_map_iterator.gif new file mode 100644 index 0000000..b89b64b Binary files /dev/null and b/doc/iterators/grid_map_iterator.gif differ diff --git a/doc/iterators/grid_map_iterator_preview.gif b/doc/iterators/grid_map_iterator_preview.gif new file mode 100644 index 0000000..b76f5d3 Binary files /dev/null and b/doc/iterators/grid_map_iterator_preview.gif differ diff --git a/doc/iterators/line_iterator.gif b/doc/iterators/line_iterator.gif new file mode 100644 index 0000000..1a664ab Binary files /dev/null and b/doc/iterators/line_iterator.gif differ diff --git a/doc/iterators/line_iterator_preview.gif b/doc/iterators/line_iterator_preview.gif new file mode 100644 index 0000000..18e70a8 Binary files /dev/null and b/doc/iterators/line_iterator_preview.gif differ diff --git a/doc/iterators/polygon_iterator.gif b/doc/iterators/polygon_iterator.gif new file mode 100644 index 0000000..96f20c4 Binary files /dev/null and b/doc/iterators/polygon_iterator.gif differ diff --git a/doc/iterators/polygon_iterator_preview.gif b/doc/iterators/polygon_iterator_preview.gif new file mode 100644 index 0000000..33e8d37 Binary files /dev/null and b/doc/iterators/polygon_iterator_preview.gif differ diff --git a/doc/iterators/spiral_iterator.gif b/doc/iterators/spiral_iterator.gif new file mode 100644 index 0000000..ff287dc Binary files /dev/null and b/doc/iterators/spiral_iterator.gif differ diff --git a/doc/iterators/spiral_iterator_preview.gif b/doc/iterators/spiral_iterator_preview.gif new file mode 100644 index 0000000..8699b3c Binary files /dev/null and b/doc/iterators/spiral_iterator_preview.gif differ diff --git a/doc/iterators/submap_iterator.gif b/doc/iterators/submap_iterator.gif new file mode 100644 index 0000000..c9038bf Binary files /dev/null and b/doc/iterators/submap_iterator.gif differ diff --git a/doc/iterators/submap_iterator_preview.gif b/doc/iterators/submap_iterator_preview.gif new file mode 100644 index 0000000..49784d3 Binary files /dev/null and b/doc/iterators/submap_iterator_preview.gif differ diff --git a/doc/move_method.gif b/doc/move_method.gif new file mode 100644 index 0000000..4be9d7f Binary files /dev/null and b/doc/move_method.gif differ diff --git a/doc/setposition_method.gif b/doc/setposition_method.gif new file mode 100644 index 0000000..21b87ba Binary files /dev/null and b/doc/setposition_method.gif differ diff --git a/include/grid_map_core/BufferRegion.hpp b/include/grid_map_core/BufferRegion.hpp new file mode 100644 index 0000000..7821c2d --- /dev/null +++ b/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 */ diff --git a/include/grid_map_core/CubicInterpolation.hpp b/include/grid_map_core/CubicInterpolation.hpp new file mode 100644 index 0000000..360e384 --- /dev/null +++ b/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 +#include +#include +#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*/ diff --git a/include/grid_map_core/GridMap.hpp b/include/grid_map_core/GridMap.hpp new file mode 100644 index 0000000..74a62b7 --- /dev/null +++ b/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 +#include + +// Eigen +#include +#include + +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& 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& 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& basicLayers); + + /*! + * Gets the names of the basic layers. + * @return the names of the basic layers. + */ + const std::vector& 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& 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& 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 layers = std::vector()); + + /*! + * 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 data_; + + //! Names of the data layers. + std::vector 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 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 diff --git a/include/grid_map_core/GridMapMath.hpp b/include/grid_map_core/GridMapMath.hpp new file mode 100644 index 0000000..fb1a38e --- /dev/null +++ b/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 +#include +#include + +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& 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 diff --git a/include/grid_map_core/Polygon.hpp b/include/grid_map_core/Polygon.hpp new file mode 100644 index 0000000..d7720fb --- /dev/null +++ b/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 + +// STD +#include + +// Eigen +#include + +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 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& 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 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& 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 vertices_; + // NOLINTEND(misc-non-private-member-variables-in-classes) + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} /* namespace grid_map */ diff --git a/include/grid_map_core/SubmapGeometry.hpp b/include/grid_map_core/SubmapGeometry.hpp new file mode 100644 index 0000000..3e7b048 --- /dev/null +++ b/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 + +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 */ diff --git a/include/grid_map_core/TypeDefs.hpp b/include/grid_map_core/TypeDefs.hpp new file mode 100644 index 0000000..04a7a36 --- /dev/null +++ b/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 + +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 + diff --git a/include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp b/include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp new file mode 100644 index 0000000..118a6e3 --- /dev/null +++ b/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 meanOfFinites() const +{ + return Scalar(this->redux(Eigen::internal::scalar_sum_of_finites_op())) / this->numberOfFinites(); +} + +Scalar minCoeffOfFinites() const +{ + return Scalar(this->redux(Eigen::internal::scalar_min_of_finites_op())); +} + +Scalar maxCoeffOfFinites() const +{ + return Scalar(this->redux(Eigen::internal::scalar_max_of_finites_op())); +} diff --git a/include/grid_map_core/eigen_plugins/Functors.hpp b/include/grid_map_core/eigen_plugins/Functors.hpp new file mode 100644 index 0000000..1f9f665 --- /dev/null +++ b/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 +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 diff --git a/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp b/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp new file mode 100644 index 0000000..77aa039 --- /dev/null +++ b/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp @@ -0,0 +1,59 @@ +#include + +template 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 +struct functor_traits > { + enum { + Cost = 2 * NumTraits::ReadCost + NumTraits::AddCost, + PacketAccess = false + }; +}; + +template +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 +struct functor_traits > { + enum { + Cost = NumTraits::AddCost, + PacketAccess = false + }; +}; + +template +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 +struct functor_traits > { + enum { + Cost = NumTraits::AddCost, + PacketAccess = false + }; +}; diff --git a/include/grid_map_core/grid_map_core.hpp b/include/grid_map_core/grid_map_core.hpp new file mode 100644 index 0000000..d714311 --- /dev/null +++ b/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" diff --git a/include/grid_map_core/gtest_eigen.hpp b/include/grid_map_core/gtest_eigen.hpp new file mode 100644 index 0000000..8872ab8 --- /dev/null +++ b/include/grid_map_core/gtest_eigen.hpp @@ -0,0 +1,165 @@ +/** + * @file gtest.hpp + * @author Paul Furgale + * @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 +#include +#include +#include + +namespace grid_map { + + template + Eigen::Matrix randomCovariance() + { + Eigen::Matrix U; + U.setRandom(); + return U.transpose() * U + 5.0 * Eigen::Matrix::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 + 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 + 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 + 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 + 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 diff --git a/include/grid_map_core/iterators/CircleIterator.hpp b/include/grid_map_core/iterators/CircleIterator.hpp new file mode 100644 index 0000000..14f2713 --- /dev/null +++ b/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 + +#include + +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 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 + diff --git a/include/grid_map_core/iterators/EllipseIterator.hpp b/include/grid_map_core/iterators/EllipseIterator.hpp new file mode 100644 index 0000000..06be4d4 --- /dev/null +++ b/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 + +#include + +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 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 + diff --git a/include/grid_map_core/iterators/GridMapIterator.hpp b/include/grid_map_core/iterators/GridMapIterator.hpp new file mode 100644 index 0000000..3a3443b --- /dev/null +++ b/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 + +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 + diff --git a/include/grid_map_core/iterators/LineIterator.hpp b/include/grid_map_core/iterators/LineIterator.hpp new file mode 100644 index 0000000..ffbdc9c --- /dev/null +++ b/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 + +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 + diff --git a/include/grid_map_core/iterators/PolygonIterator.hpp b/include/grid_map_core/iterators/PolygonIterator.hpp new file mode 100644 index 0000000..6027670 --- /dev/null +++ b/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 + +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 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 + diff --git a/include/grid_map_core/iterators/SlidingWindowIterator.hpp b/include/grid_map_core/iterators/SlidingWindowIterator.hpp new file mode 100644 index 0000000..42b2a95 --- /dev/null +++ b/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 + +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 + diff --git a/include/grid_map_core/iterators/SpiralIterator.hpp b/include/grid_map_core/iterators/SpiralIterator.hpp new file mode 100644 index 0000000..f1c3966 --- /dev/null +++ b/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 +#include +#include + +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(0 < val) - static_cast(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 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 + diff --git a/include/grid_map_core/iterators/SubmapIterator.hpp b/include/grid_map_core/iterators/SubmapIterator.hpp new file mode 100644 index 0000000..3cf1a1a --- /dev/null +++ b/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 + +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 + diff --git a/include/grid_map_core/iterators/iterators.hpp b/include/grid_map_core/iterators/iterators.hpp new file mode 100644 index 0000000..1d919aa --- /dev/null +++ b/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" diff --git a/include/grid_map_core/utils/testing.hpp b/include/grid_map_core/utils/testing.hpp new file mode 100644 index 0000000..e997c2f --- /dev/null +++ b/include/grid_map_core/utils/testing.hpp @@ -0,0 +1,37 @@ +/** + * Copied from ANYbotics/eigen_utils, to avoid testing dependency. + */ + +#pragma once + +#include +#include + +#define ASSERT_MATRICES_EQ_WITH_NAN(first, second) assertMatrixesEqualWithNan((first), #first, (second), #second, __LINE__) +static void assertMatrixesEqualWithNan(Eigen::Ref first, std::string firstName, + Eigen::Ref 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 +} diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..ac020f6 --- /dev/null +++ b/package.xml @@ -0,0 +1,17 @@ + + + grid_map_core + 1.7.18 + Universal grid map library to manage two-dimensional grid maps with multiple data layers. + Maximilian Wulf + Yoshua Nava + BSD + http://github.com/anybotics/grid_map + http://github.com/anybotics/grid_map/issues + Péter Fankhauser + catkin + + eigen + + gtest + diff --git a/src/BufferRegion.cpp b/src/BufferRegion.cpp new file mode 100644 index 0000000..4c1e8e6 --- /dev/null +++ b/src/BufferRegion.cpp @@ -0,0 +1,56 @@ +/* + * BufferRegion.cpp + * + * Created on: Aug 19, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, ANYbotics + */ +#include + +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 */ + + diff --git a/src/CubicInterpolation.cpp b/src/CubicInterpolation.cpp new file mode 100644 index 0000000..d9eb2fc --- /dev/null +++ b/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(layerMat.cols()); + const auto numRow = static_cast(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(layerData.cols())}; + const auto numRow{static_cast(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(layerData.cols())}; + const auto numRow{static_cast(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 */ diff --git a/src/GridMap.cpp b/src/GridMap.cpp new file mode 100644 index 0000000..b72ea4c --- /dev/null +++ b/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 +#include +#include +#include +#include + +using std::cout; +using std::endl; +using std::isfinite; + +namespace grid_map { + +GridMap::GridMap(const std::vector& layers) { + position_.setZero(); + length_.setZero(); + resolution_ = 0.0; + size_.setZero(); + startIndex_.setZero(); + timestamp_ = 0; + layers_ = layers; + + for (auto& layer : layers_) { + data_.insert(std::pair(layer, Matrix())); + } +} + +GridMap::GridMap() : GridMap(std::vector()) {} + +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(round(length(0) / resolution)); // There is no round() function in Eigen. + size(1) = static_cast(round(length(1) / resolution)); + resize(size); + clearAll(); + + resolution_ = resolution; + length_ = (size_.cast() * 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& basicLayers) { + basicLayers_ = basicLayers; +} + +const std::vector& 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(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& 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& 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 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 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 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(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& 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 newRegions; + return move(position, newRegions); +} + +bool GridMap::addDataFrom(const GridMap& other, bool extendMap, bool overwriteData, bool copyAllLayers, std::vector 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 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::epsilon()); + positionInMap.y() = std::fmin(positionInMap.y(), maxY - std::numeric_limits::epsilon()); + + positionInMap.x() = std::fmax(positionInMap.x(), minX + std::numeric_limits::epsilon()); + positionInMap.y() = std::fmax(positionInMap.y(), minY + std::numeric_limits::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(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(interpolatedValue); + + return true; + +} + + +} // namespace grid_map diff --git a/src/GridMapMath.cpp b/src/GridMapMath.cpp new file mode 100644 index 0000000..b56d618 --- /dev/null +++ b/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 + +// Limits +#include + +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() * (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(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::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(); + + // 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() * 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& 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()).array() / 255.0).matrix(); + return true; +} + +bool colorValueToVector(const float& colorValue, Eigen::Vector3f& colorVector) +{ + // cppcheck-suppress invalidPointerCast + const unsigned long tempColorValue = *reinterpret_cast(&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(); + colorVectorToValue(tempColorVector, colorValue); +} + +} // namespace grid_map + diff --git a/src/Polygon.cpp b/src/Polygon.cpp new file mode 100644 index 0000000..3ab234c --- /dev/null +++ b/src/Polygon.cpp @@ -0,0 +1,352 @@ +/* + * Polygon.cpp + * + * Created on: Nov 7, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, ANYbotics + */ + +#include + +#include +#include + +#include +#include + +namespace grid_map { + +Polygon::Polygon() + : timestamp_(0) +{ +} + +Polygon::Polygon(std::vector 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& 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 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::infinity(); + double maxX = -std::numeric_limits::infinity(); + double minY = std::numeric_limits::infinity(); + double maxY = -std::numeric_limits::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 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 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 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 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::triangulate(const TriangulationMethods& /*method*/) const +{ + // TODO Add more triangulation methods. + // https://en.wikipedia.org/wiki/Polygon_triangulation + std::vector 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 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 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 rot2d(theta); + centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex; + polygon.addVertex(center2 + centerToVertexTemp); + } + return polygon; +} + +Polygon Polygon::convexHull(Polygon& polygon1, Polygon& polygon2) +{ + std::vector 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& points) +{ + // Adapted from https://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain + if (points.size() <= 3) { + return Polygon(points); + } + std::vector 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 */ diff --git a/src/SubmapGeometry.cpp b/src/SubmapGeometry.cpp new file mode 100644 index 0000000..065adac --- /dev/null +++ b/src/SubmapGeometry.cpp @@ -0,0 +1,59 @@ +/* + * SubmapGeometry.cpp + * + * Created on: Aug 18, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, ANYbotics + */ + +#include +#include + +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 */ diff --git a/src/iterators/CircleIterator.cpp b/src/iterators/CircleIterator.cpp new file mode 100644 index 0000000..b681b33 --- /dev/null +++ b/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 +#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(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 */ + diff --git a/src/iterators/EllipseIterator.cpp b/src/iterators/EllipseIterator.cpp new file mode 100644 index 0000000..7fe7632 --- /dev/null +++ b/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 +#include + +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(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 */ + diff --git a/src/iterators/GridMapIterator.cpp b/src/iterators/GridMapIterator.cpp new file mode 100644 index 0000000..756c103 --- /dev/null +++ b/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 */ diff --git a/src/iterators/LineIterator.cpp b/src/iterators/LineIterator.cpp new file mode 100644 index 0000000..2c6337d --- /dev/null +++ b/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::epsilon()) * direction; + if ((end - newStart).norm() < gridMap.getResolution() - std::numeric_limits::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 */ diff --git a/src/iterators/PolygonIterator.cpp b/src/iterators/PolygonIterator.cpp new file mode 100644 index 0000000..733a842 --- /dev/null +++ b/src/iterators/PolygonIterator.cpp @@ -0,0 +1,88 @@ +/* + * PolygonIterator.hpp + * + * Created on: Sep 19, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, ANYbotics + */ + +#include + +#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(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 */ + diff --git a/src/iterators/SlidingWindowIterator.cpp b/src/iterators/SlidingWindowIterator.cpp new file mode 100644 index 0000000..c1ba2a4 --- /dev/null +++ b/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 + +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(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(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(windowMargin_))); + const Index topLeftIndex(centerIndex - windowMargin); + const Index bottomRightIndex(centerIndex + windowMargin); + return checkIfIndexInRange(topLeftIndex, size_) && checkIfIndexInRange(bottomRightIndex, size_); +} + +} /* namespace grid_map */ diff --git a/src/iterators/SpiralIterator.cpp b/src/iterators/SpiralIterator.cpp new file mode 100644 index 0000000..9ee5b3c --- /dev/null +++ b/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 +#include + + +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(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(Vector(point.x() + normal.x(), point.y()).norm()) == distance_) { + point.x() += normal.x(); + } else if (normal.y() != 0 && static_cast(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(point.x()) != distance_ || point.y() != 0); +} + +double SpiralIterator::getCurrentRadius() const +{ + Index radius = *(*this) - indexCenter_; + return radius.matrix().norm() * resolution_; +} + +} /* namespace grid_map */ + diff --git a/src/iterators/SubmapIterator.cpp b/src/iterators/SubmapIterator.cpp new file mode 100644 index 0000000..12fb120 --- /dev/null +++ b/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 */ + diff --git a/test/CubicConvolutionInterpolationTest.cpp b/test/CubicConvolutionInterpolationTest.cpp new file mode 100644 index 0000000..4e884f2 --- /dev/null +++ b/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 + +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; + } +} + diff --git a/test/CubicInterpolationTest.cpp b/test/CubicInterpolationTest.cpp new file mode 100644 index 0000000..eb4a9cf --- /dev/null +++ b/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 + +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; + } +} + diff --git a/test/EigenPluginsTest.cpp b/test/EigenPluginsTest.cpp new file mode 100644 index 0000000..cdbd1e8 --- /dev/null +++ b/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 + +// Eigen +#include + +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 matrix2; + matrix2.setOnes(); + EXPECT_EQ(matrix2.rows() * matrix2.cols(), matrix2.numberOfFinites()); + + Matrix matrix3; + matrix3.setConstant(NAN); + matrix3.col(3).setConstant(0.0); + EXPECT_EQ(matrix3.rows(), matrix3.numberOfFinites()); +} + +TEST(EigenMatrixBaseAddons, sumOfFinites) +{ + Matrix 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 matrix2; + matrix2.setRandom(); + EXPECT_NEAR(matrix2.mean(), matrix2.meanOfFinites(), 1e-10); +} + +TEST(EigenMatrixBaseAddons, minCoeffOfFinites) +{ + Matrix 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 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(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); +} diff --git a/test/EllipseIteratorTest.cpp b/test/EllipseIteratorTest.cpp new file mode 100644 index 0000000..f77fee0 --- /dev/null +++ b/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 + +// Vector +#include + +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()); +} diff --git a/test/GridMapIteratorTest.cpp b/test/GridMapIteratorTest.cpp new file mode 100644 index 0000000..33c1ac4 --- /dev/null +++ b/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 + +// Vector +#include + +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(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()); +} diff --git a/test/GridMapMathTest.cpp b/test/GridMapMathTest.cpp new file mode 100644 index 0000000..1382f76 --- /dev/null +++ b/test/GridMapMathTest.cpp @@ -0,0 +1,1039 @@ +/* + * GridMapMathTest.cpp + * + * Created on: Feb 10, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, ANYbotics + */ + +#include "grid_map_core/GridMapMath.hpp" + +// gtest +#include + +// Limits +#include + +// Vector +#include + +using std::numeric_limits; + +namespace grid_map{ + +TEST(PositionFromIndex, Simple) +{ + Length mapLength(3.0, 2.0); + Position mapPosition(-1.0, 2.0); + double resolution = 1.0; + Size bufferSize(3, 2); + Position position; + + EXPECT_TRUE(getPositionFromIndex(position, Index(0, 0), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_DOUBLE_EQ(1.0 + mapPosition.x(), position.x()); + EXPECT_DOUBLE_EQ(0.5 + mapPosition.y(), position.y()); + + EXPECT_TRUE(getPositionFromIndex(position, Index(1, 0), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x()); + EXPECT_DOUBLE_EQ(0.5 + mapPosition.y(), position.y()); + + EXPECT_TRUE(getPositionFromIndex(position, Index(1, 1), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x()); + EXPECT_DOUBLE_EQ(-0.5 + mapPosition.y(), position.y()); + + EXPECT_TRUE(getPositionFromIndex(position, Index(2, 1), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_DOUBLE_EQ(-1.0 + mapPosition.x(), position.x()); + EXPECT_DOUBLE_EQ(-0.5 + mapPosition.y(), position.y()); + + EXPECT_FALSE(getPositionFromIndex(position, Index(3, 1), mapLength, mapPosition, resolution, bufferSize)); +} + +TEST(PositionFromIndex, CircularBuffer) +{ + Length mapLength(0.5, 0.4); + Position mapPosition(-0.1, 13.4); + double resolution = 0.1; + Size bufferSize(5, 4); + Index bufferStartIndex(3, 1); + Position position; + + EXPECT_TRUE(getPositionFromIndex(position, Index(3, 1), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_DOUBLE_EQ(0.2 + mapPosition.x(), position.x()); + EXPECT_DOUBLE_EQ(0.15 + mapPosition.y(), position.y()); + + EXPECT_TRUE(getPositionFromIndex(position, Index(4, 2), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_DOUBLE_EQ(0.1 + mapPosition.x(), position.x()); + EXPECT_DOUBLE_EQ(0.05 + mapPosition.y(), position.y()); + + EXPECT_TRUE(getPositionFromIndex(position, Index(2, 0), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_DOUBLE_EQ(-0.2 + mapPosition.x(), position.x()); + EXPECT_DOUBLE_EQ(-0.15 + mapPosition.y(), position.y()); + + EXPECT_TRUE(getPositionFromIndex(position, Index(0, 0), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x()); + EXPECT_DOUBLE_EQ(-0.15 + mapPosition.y(), position.y()); + + EXPECT_TRUE(getPositionFromIndex(position, Index(4, 3), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_DOUBLE_EQ(0.1 + mapPosition.x(), position.x()); + EXPECT_DOUBLE_EQ(-0.05 + mapPosition.y(), position.y()); + + EXPECT_FALSE(getPositionFromIndex(position, Index(5, 3), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); +} + +TEST(IndexFromPosition, Simple) +{ + Length mapLength(3.0, 2.0); + Position mapPosition(-12.4, -7.1); + double resolution = 1.0; + Index bufferSize(3, 2); + Index index; + + EXPECT_TRUE(getIndexFromPosition(index, Position(1.0, 0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize)); + EXPECT_EQ(0, index(0)); + EXPECT_EQ(0, index(1)); + + EXPECT_TRUE(getIndexFromPosition(index, Position(-1.0, -0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize)); + EXPECT_EQ(2, index(0)); + EXPECT_EQ(1, index(1)); + + EXPECT_TRUE(getIndexFromPosition(index, Position(0.6, 0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize)); + EXPECT_EQ(0, index(0)); + EXPECT_EQ(0, index(1)); + + EXPECT_TRUE(getIndexFromPosition(index, Position(0.4, -0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize)); + EXPECT_EQ(1, index(0)); + EXPECT_EQ(1, index(1)); + + EXPECT_TRUE(getIndexFromPosition(index, Position(0.4, 0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize)); + EXPECT_EQ(1, index(0)); + EXPECT_EQ(0, index(1)); + + EXPECT_FALSE(getIndexFromPosition(index, Position(4.0, 0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize)); +} + +TEST(IndexFromPosition, EdgeCases) +{ + Length mapLength(3.0, 2.0); + Position mapPosition(0.0, 0.0); + double resolution = 1.0; + Size bufferSize(3, 2); + Index index; + + EXPECT_TRUE(getIndexFromPosition(index, Position(0.0, DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_EQ(1, index(0)); + EXPECT_EQ(0, index(1)); + + EXPECT_TRUE(getIndexFromPosition(index, Position(0.5 - DBL_EPSILON, -DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_EQ(1, index(0)); + EXPECT_EQ(1, index(1)); + + EXPECT_TRUE(getIndexFromPosition(index, Position(-0.5 - DBL_EPSILON, -DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_EQ(2, index(0)); + EXPECT_EQ(1, index(1)); + + EXPECT_FALSE(getIndexFromPosition(index, Position(-1.5, 1.0), mapLength, mapPosition, resolution, bufferSize)); +} + +TEST(IndexFromPosition, CircularBuffer) +{ + Length mapLength(0.5, 0.4); + Position mapPosition(0.4, -0.9); + double resolution = 0.1; + Size bufferSize(5, 4); + Index bufferStartIndex(3, 1); + Index index; + + EXPECT_TRUE(getIndexFromPosition(index, Position(0.2, 0.15) + mapPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_EQ(3, index(0)); + EXPECT_EQ(1, index(1)); + + EXPECT_TRUE(getIndexFromPosition(index, Position(0.03, -0.17) + mapPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_EQ(0, index(0)); + EXPECT_EQ(0, index(1)); +} + +TEST(checkIfPositionWithinMap, Inside) +{ + Length mapLength(50.0, 25.0); + Position mapPosition(11.4, 0.0); + + EXPECT_TRUE(checkIfPositionWithinMap(Position(0.0, 0.0) + mapPosition, mapLength, mapPosition)); + EXPECT_TRUE(checkIfPositionWithinMap(Position(5.0, 5.0) + mapPosition, mapLength, mapPosition)); + EXPECT_TRUE(checkIfPositionWithinMap(Position(20.0, 10.0) + mapPosition, mapLength, mapPosition)); + EXPECT_TRUE(checkIfPositionWithinMap(Position(20.0, -10.0) + mapPosition, mapLength, mapPosition)); + EXPECT_TRUE(checkIfPositionWithinMap(Position(-20.0, 10.0) + mapPosition, mapLength, mapPosition)); + EXPECT_TRUE(checkIfPositionWithinMap(Position(-20.0, -10.0) + mapPosition, mapLength, mapPosition)); +} + +TEST(checkIfPositionWithinMap, Outside) +{ + Length mapLength(10.0, 5.0); + Position mapPosition(-3.0, 145.2); + + EXPECT_FALSE(checkIfPositionWithinMap(Position(5.5, 0.0) + mapPosition, mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, 0.0) + mapPosition, mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, 3.0) + mapPosition, mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, -3.0) + mapPosition, mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(3.0, 3.0) + mapPosition, mapLength, mapPosition)); +} + +TEST(checkIfPositionWithinMap, EdgeCases) +{ + Length mapLength(2.0, 3.0); + Position mapPosition(0.0, 0.0); + + /* + * + * A (is inside) B (is not inside) + * +-----------------------+ + * | | + * | | + * | X | + * | ^ | + * | | | + * | | | + * | <-----+ | + * | Y | + * | | + * | | + * | | + * +-----------------------+ + * C (is not inside) D (is not inside) + * + * Resulting coordinates are: + * A: (1.0, 1.5) + * B: (1.0, -1.5) + * C: (-1.0, 1.5) + * D: (-1.0, -1.5) + * + */ + + // Noise around A. + EXPECT_TRUE(checkIfPositionWithinMap(Position(1.0, 1.5), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, 1.5), mapLength, mapPosition)); + EXPECT_TRUE(checkIfPositionWithinMap(Position(1.0 - DBL_EPSILON, 1.5), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, 1.5 + DBL_EPSILON), mapLength, mapPosition)); + EXPECT_TRUE(checkIfPositionWithinMap(Position(1.0, 1.5 - DBL_EPSILON), mapLength, mapPosition)); + + // Noise around B. + EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, -1.5), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, - 1.5), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 - DBL_EPSILON, - 1.5), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, - 1.5 + DBL_EPSILON), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, - 1.5 - DBL_EPSILON), mapLength, mapPosition)); + + // Noise around C. + EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5), mapLength, mapPosition)); + EXPECT_TRUE(checkIfPositionWithinMap(Position(-1.0 + DBL_EPSILON, 1.5), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0 - DBL_EPSILON, 1.5), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5 + DBL_EPSILON), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5 - DBL_EPSILON), mapLength, mapPosition)); + + // Noise around D. + EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, -1.5), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0 + DBL_EPSILON, -1.5), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0 - DBL_EPSILON, -1.5), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, -1.5 + DBL_EPSILON), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, -1.5 - DBL_EPSILON), mapLength, mapPosition)); + + // Extra tests. + EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, 1.0), mapLength, mapPosition)); + EXPECT_TRUE(checkIfPositionWithinMap(Position((2.0 + DBL_EPSILON) / 2.0, 1.0), mapLength, mapPosition)); + EXPECT_FALSE(checkIfPositionWithinMap(Position(0.5, -1.5 - (2.0 * DBL_EPSILON)), mapLength, mapPosition)); + EXPECT_TRUE(checkIfPositionWithinMap(Position(-0.5, (3.0 + DBL_EPSILON) / 2.0), mapLength, mapPosition)); +} + +TEST(getIndexShiftFromPositionShift, All) +{ + double resolution = 1.0; + Index indexShift; + + EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.0, 0.0), resolution)); + EXPECT_EQ(0, indexShift(0)); + EXPECT_EQ(0, indexShift(1)); + + EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.35, -0.45), resolution)); + EXPECT_EQ(0, indexShift(0)); + EXPECT_EQ(0, indexShift(1)); + + EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.55, -0.45), resolution)); + EXPECT_EQ(-1, indexShift(0)); + EXPECT_EQ(0, indexShift(1)); + + EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(-1.3, -2.65), resolution)); + EXPECT_EQ(1, indexShift(0)); + EXPECT_EQ(3, indexShift(1)); + + EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(-0.4, 0.09), 0.2)); + EXPECT_EQ(2, indexShift(0)); + EXPECT_EQ(0, indexShift(1)); +} + +TEST(getPositionShiftFromIndexShift, All) +{ + double resolution = 0.3; + Vector positionShift; + + EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(0, 0), resolution)); + EXPECT_DOUBLE_EQ(0.0, positionShift.x()); + EXPECT_DOUBLE_EQ(0.0, positionShift.y()); + + EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(1, -1), resolution)); + EXPECT_DOUBLE_EQ(-0.3, positionShift.x()); + EXPECT_DOUBLE_EQ(0.3, positionShift.y()); + + EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(2, 1), resolution)); + EXPECT_DOUBLE_EQ(-0.6, positionShift.x()); + EXPECT_DOUBLE_EQ(-0.3, positionShift.y()); +} + +TEST(checkIfIndexInRange, All) +{ + Size bufferSize(10, 15); + EXPECT_TRUE(checkIfIndexInRange(Index(0, 0), bufferSize)); + EXPECT_TRUE(checkIfIndexInRange(Index(9, 14), bufferSize)); + EXPECT_FALSE(checkIfIndexInRange(Index(10, 5), bufferSize)); + EXPECT_FALSE(checkIfIndexInRange(Index(5, 300), bufferSize)); + EXPECT_FALSE(checkIfIndexInRange(Index(-1, 0), bufferSize)); + EXPECT_FALSE(checkIfIndexInRange(Index(0, -300), bufferSize)); +} + +TEST(boundIndexToRange, All) +{ + int bufferSize = 10; + + int index = 0; + boundIndexToRange(index, bufferSize); + EXPECT_EQ(0, index); + + index = 1; + boundIndexToRange(index, bufferSize); + EXPECT_EQ(1, index); + + index = -1; + boundIndexToRange(index, bufferSize); + EXPECT_EQ(0, index); + + index = 9; + boundIndexToRange(index, bufferSize); + EXPECT_EQ(9, index); + + index = 10; + boundIndexToRange(index, bufferSize); + EXPECT_EQ(9, index); + + index = 35; + boundIndexToRange(index, bufferSize); + EXPECT_EQ(9, index); + + index = -19; + boundIndexToRange(index, bufferSize); + EXPECT_EQ(0, index); +} + +TEST(wrapIndexToRange, All) +{ + int bufferSize = 10; + + int index = 0; + wrapIndexToRange(index, bufferSize); + EXPECT_EQ(0, index); + + index = 1; + wrapIndexToRange(index, bufferSize); + EXPECT_EQ(1, index); + + index = -1; + wrapIndexToRange(index, bufferSize); + EXPECT_EQ(9, index); + + index = 9; + wrapIndexToRange(index, bufferSize); + EXPECT_EQ(9, index); + + index = 10; + wrapIndexToRange(index, bufferSize); + EXPECT_EQ(0, index); + + index = 11; + wrapIndexToRange(index, bufferSize); + EXPECT_EQ(1, index); + + index = 35; + wrapIndexToRange(index, bufferSize); + EXPECT_EQ(5, index); + + index = -9; + wrapIndexToRange(index, bufferSize); + EXPECT_EQ(1, index); + + index = -19; + wrapIndexToRange(index, bufferSize); + EXPECT_EQ(1, index); +} + +TEST(boundPositionToRange, Simple) +{ + double epsilon = 11.0 * numeric_limits::epsilon(); + + Length mapLength(30.0, 10.0); + Position mapPosition(0.0, 0.0); + Position position; + + position << 0.0, 0.0; + boundPositionToRange(position, mapLength, mapPosition); + EXPECT_DOUBLE_EQ(0.0, position.x()); + EXPECT_DOUBLE_EQ(0.0, position.y()); + + position << 15.0, 5.0; + boundPositionToRange(position, mapLength, mapPosition); + EXPECT_NEAR(15.0, position.x(), 15.0 * epsilon); + EXPECT_GE(15.0, position.x()); + EXPECT_NEAR(5.0, position.y(), 5.0 * epsilon); + EXPECT_GE(5.0, position.y()); + + position << -15.0, -5.0; + boundPositionToRange(position, mapLength, mapPosition); + EXPECT_NEAR(-15.0, position.x(), 15.0 * epsilon); + EXPECT_LE(-15.0, position.x()); + EXPECT_NEAR(-5.0, position.y(), 5.0 * epsilon); + EXPECT_LE(-5.0, position.y()); + + position << 16.0, 6.0; + boundPositionToRange(position, mapLength, mapPosition); + EXPECT_NEAR(15.0, position.x(), 16.0 * epsilon); + EXPECT_GE(15.0, position.x()); + EXPECT_NEAR(5.0, position.y(), 6.0 * epsilon); + EXPECT_GE(5.0, position.y()); + + position << -16.0, -6.0; + boundPositionToRange(position, mapLength, mapPosition); + EXPECT_NEAR(-15.0, position.x(), 16.0 * epsilon); + EXPECT_LE(-15.0, position.x()); + EXPECT_NEAR(-5.0, position.y(), 6.0 * epsilon); + EXPECT_LE(-5.0, position.y()); + + position << 1e6, 1e6; + boundPositionToRange(position, mapLength, mapPosition); + EXPECT_NEAR(15.0, position.x(), 1e6 * epsilon); + EXPECT_GE(15.0, position.x()); + EXPECT_NEAR(5.0, position.y(), 1e6 * epsilon); + EXPECT_GE(5.0, position.y()); + + position << -1e6, -1e6; + boundPositionToRange(position, mapLength, mapPosition); + EXPECT_NEAR(-15.0, position.x(), 1e6 * epsilon); + EXPECT_LE(-15.0, position.x()); + EXPECT_NEAR(-5.0, position.y(), 1e6 * epsilon); + EXPECT_LE(-5.0, position.y()); +} + +TEST(boundPositionToRange, Position) +{ + double epsilon = 11.0 * numeric_limits::epsilon(); + + Length mapLength(30.0, 10.0); + Position mapPosition(1.0, 2.0); + Position position; + + position << 0.0, 0.0; + boundPositionToRange(position, mapLength, mapPosition); + EXPECT_DOUBLE_EQ(0.0, position.x()); + EXPECT_DOUBLE_EQ(0.0, position.y()); + + position << 16.0, 7.0; + boundPositionToRange(position, mapLength, mapPosition); + EXPECT_NEAR(16.0, position.x(), 16.0 * epsilon); + EXPECT_GE(16.0, position.x()); + EXPECT_NEAR(7.0, position.y(), 7.0 * epsilon); + EXPECT_GE(7.0, position.y()); + + position << -14.0, -3.0; + boundPositionToRange(position, mapLength, mapPosition); + EXPECT_NEAR(-14.0, position.x(), 14.0 * epsilon); + EXPECT_LE(-14.0, position.x()); + EXPECT_NEAR(-3.0, position.y(), 3.0 * epsilon); + EXPECT_LE(-3.0, position.y()); + + position << 17.0, 8.0; + boundPositionToRange(position, mapLength, mapPosition); + EXPECT_NEAR(16.0, position.x(), 17.0 * epsilon); + EXPECT_GE(16.0, position.x()); + EXPECT_NEAR(7.0, position.y(), 8.0 * epsilon); + EXPECT_GE(7.0, position.y()); + + position << -15.0, -4.0; + boundPositionToRange(position, mapLength, mapPosition); + EXPECT_NEAR(-14.0, position.x(), 15.0 * epsilon); + EXPECT_LE(-14.0, position.x()); + EXPECT_NEAR(-3.0, position.y(), 4.0 * epsilon); + EXPECT_LE(-3.0, position.y()); + + position << 1e6, 1e6; + boundPositionToRange(position, mapLength, mapPosition); + EXPECT_NEAR(16.0, position.x(), 1e6 * epsilon); + EXPECT_GE(16.0, position.x()); + EXPECT_NEAR(7.0, position.y(), 1e6 * epsilon); + EXPECT_GE(7.0, position.y()); + + position << -1e6, -1e6; + boundPositionToRange(position, mapLength, mapPosition); + EXPECT_NEAR(-14.0, position.x(), 1e6 * epsilon); + EXPECT_LE(-14.0, position.x()); + EXPECT_NEAR(-3.0, position.y(), 1e6 * epsilon); + EXPECT_LE(-3.0, position.y()); +} + +TEST(getSubmapInformation, Simple) +{ + // Map + Length mapLength(5.0, 4.0); + Position mapPosition(0.0, 0.0); + double resolution = 1.0; + Size bufferSize(5, 4); + + // Requested submap + Position requestedSubmapPosition; + Position requestedSubmapLength; + + // The returned submap indices + Index submapTopLeftIndex; + Index submapSize; + Position submapPosition; + Length submapLength; + Index requestedIndexInSubmap; + + requestedSubmapPosition << 0.0, 0.5; + requestedSubmapLength << 0.9, 2.9; + EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, mapLength, mapPosition, resolution, bufferSize)); + EXPECT_EQ(2, submapTopLeftIndex(0)); + EXPECT_EQ(0, submapTopLeftIndex(1)); + EXPECT_EQ(1, submapSize(0)); + EXPECT_EQ(3, submapSize(1)); + EXPECT_DOUBLE_EQ(0.0, submapPosition.x()); + EXPECT_DOUBLE_EQ(0.5, submapPosition.y()); + EXPECT_DOUBLE_EQ(1.0, submapLength(0)); + EXPECT_DOUBLE_EQ(3.0, submapLength(1)); + EXPECT_EQ(0, requestedIndexInSubmap(0)); + EXPECT_EQ(1, requestedIndexInSubmap(1)); +} + +TEST(getSubmapInformation, Zero) +{ + // Map + Length mapLength(5.0, 4.0); + Position mapPosition(0.0, 0.0); + double resolution = 1.0; + Size bufferSize(5, 4); + + // Requested submap + Position requestedSubmapPosition; + Length requestedSubmapLength; + + // The returned submap indices + Index submapTopLeftIndex; + Index submapSize; + Position submapPosition; + Length submapLength; + Index requestedIndexInSubmap; + + requestedSubmapPosition << -1.0, -0.5; + requestedSubmapLength << 0.0, 0.0; + EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize)); + EXPECT_EQ(3, submapTopLeftIndex(0)); + EXPECT_EQ(2, submapTopLeftIndex(1)); + EXPECT_EQ(1, submapSize(0)); + EXPECT_EQ(1, submapSize(1)); + EXPECT_DOUBLE_EQ(requestedSubmapPosition.x(), submapPosition.x()); + EXPECT_DOUBLE_EQ(requestedSubmapPosition.y(), submapPosition.y()); + EXPECT_DOUBLE_EQ(resolution, submapLength(0)); + EXPECT_DOUBLE_EQ(resolution, submapLength(1)); + EXPECT_EQ(0, requestedIndexInSubmap(0)); + EXPECT_EQ(0, requestedIndexInSubmap(1)); +} + +TEST(getSubmapInformation, ExceedingBoundaries) +{ + // Map + Length mapLength(5.0, 4.0); + Position mapPosition(0.0, 0.0); + double resolution = 1.0; + Size bufferSize(5, 4); + + // Requested submap + Position requestedSubmapPosition; + Length requestedSubmapLength; + + // The returned submap indices + Index submapTopLeftIndex; + Size submapSize; + Position submapPosition; + Length submapLength; + Index requestedIndexInSubmap; + + requestedSubmapPosition << 2.0, 1.5; + requestedSubmapLength << 2.9, 2.9; + EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize)); + EXPECT_EQ(0, submapTopLeftIndex(0)); + EXPECT_EQ(0, submapTopLeftIndex(1)); + EXPECT_EQ(2, submapSize(0)); + EXPECT_EQ(2, submapSize(1)); + EXPECT_DOUBLE_EQ(1.5, submapPosition.x()); + EXPECT_DOUBLE_EQ(1.0, submapPosition.y()); + EXPECT_DOUBLE_EQ(2.0, submapLength(0)); + EXPECT_DOUBLE_EQ(2.0, submapLength(1)); + EXPECT_EQ(0, requestedIndexInSubmap(0)); + EXPECT_EQ(0, requestedIndexInSubmap(1)); + + requestedSubmapPosition << 0.0, 0.0; + requestedSubmapLength << 1e6, 1e6; + EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize)); + EXPECT_EQ(0, submapTopLeftIndex(0)); + EXPECT_EQ(0, submapTopLeftIndex(1)); + EXPECT_EQ(bufferSize(0), submapSize(0)); + EXPECT_EQ(bufferSize(1), submapSize(1)); + EXPECT_DOUBLE_EQ(0.0, submapPosition.x()); + EXPECT_DOUBLE_EQ(0.0, submapPosition.y()); + EXPECT_DOUBLE_EQ(mapLength(0), submapLength(0)); + EXPECT_DOUBLE_EQ(mapLength(1), submapLength(1)); + EXPECT_EQ(2, requestedIndexInSubmap(0)); + EXPECT_LE(1, requestedIndexInSubmap(1)); + EXPECT_GE(2, requestedIndexInSubmap(1)); +} + +TEST(getSubmapInformation, CircularBuffer) +{ + // Map + Length mapLength(5.0, 4.0); + Position mapPosition(0.0, 0.0); + double resolution = 1.0; + Size bufferSize(5, 4); + Index bufferStartIndex(2, 1); + + // Requested submap + Position requestedSubmapPosition; + Length requestedSubmapLength; + + // The returned submap indices + Index submapTopLeftIndex; + Size submapSize; + Position submapPosition; + Length submapLength; + Index requestedIndexInSubmap; + + requestedSubmapPosition << 0.0, 0.5; + requestedSubmapLength << 0.9, 2.9; + EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_EQ(4, submapTopLeftIndex(0)); + EXPECT_EQ(1, submapTopLeftIndex(1)); + EXPECT_EQ(1, submapSize(0)); + EXPECT_EQ(3, submapSize(1)); + EXPECT_DOUBLE_EQ(0.0, submapPosition.x()); + EXPECT_DOUBLE_EQ(0.5, submapPosition.y()); + EXPECT_DOUBLE_EQ(1.0, submapLength(0)); + EXPECT_DOUBLE_EQ(3.0, submapLength(1)); + EXPECT_EQ(0, requestedIndexInSubmap(0)); + EXPECT_EQ(1, requestedIndexInSubmap(1)); + + requestedSubmapPosition << 2.0, 1.5; + requestedSubmapLength << 2.9, 2.9; + EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_EQ(2, submapTopLeftIndex(0)); + EXPECT_EQ(1, submapTopLeftIndex(1)); + EXPECT_EQ(2, submapSize(0)); + EXPECT_EQ(2, submapSize(1)); + EXPECT_DOUBLE_EQ(1.5, submapPosition.x()); + EXPECT_DOUBLE_EQ(1.0, submapPosition.y()); + EXPECT_DOUBLE_EQ(2.0, submapLength(0)); + EXPECT_DOUBLE_EQ(2.0, submapLength(1)); + EXPECT_EQ(0, requestedIndexInSubmap(0)); + EXPECT_EQ(0, requestedIndexInSubmap(1)); + + requestedSubmapPosition << 0.0, 0.0; + requestedSubmapLength << 1e6, 1e6; + EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_EQ(2, submapTopLeftIndex(0)); + EXPECT_EQ(1, submapTopLeftIndex(1)); + EXPECT_EQ(bufferSize(0), submapSize(0)); + EXPECT_EQ(bufferSize(1), submapSize(1)); + EXPECT_DOUBLE_EQ(0.0, submapPosition.x()); + EXPECT_DOUBLE_EQ(0.0, submapPosition.y()); + EXPECT_DOUBLE_EQ(mapLength(0), submapLength(0)); + EXPECT_DOUBLE_EQ(mapLength(1), submapLength(1)); + EXPECT_EQ(2, requestedIndexInSubmap(0)); + EXPECT_LE(1, requestedIndexInSubmap(1)); + EXPECT_GE(2, requestedIndexInSubmap(1)); +} + +TEST(getSubmapInformation, Debug1) +{ + // Map + Length mapLength(4.98, 4.98); + Position mapPosition(-4.98, -5.76); + double resolution = 0.06; + Size bufferSize(83, 83); + Index bufferStartIndex(0, 13); + + // Requested submap + Position requestedSubmapPosition(-7.44, -3.42); + Length requestedSubmapLength(0.12, 0.12); + + // The returned submap indices + Index submapTopLeftIndex; + Size submapSize; + Position submapPosition; + Length submapLength; + Index requestedIndexInSubmap; + + EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_EQ(2, submapSize(0)); + EXPECT_EQ(3, submapSize(1)); + EXPECT_DOUBLE_EQ(0.12, submapLength(0)); + EXPECT_DOUBLE_EQ(0.18, submapLength(1)); +} + +TEST(getSubmapInformation, Debug2) +{ + // Map + Length mapLength(4.98, 4.98); + Position mapPosition(2.46, -25.26); + double resolution = 0.06; + Size bufferSize(83, 83); + Index bufferStartIndex(42, 6); + + // Requested submap + Position requestedSubmapPosition(0.24, -26.82); + Length requestedSubmapLength(0.624614, 0.462276); + + // The returned submap indices + Index submapTopLeftIndex; + Size submapSize; + Position submapPosition; + Length submapLength; + Index requestedIndexInSubmap; + + EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_LT(0, submapSize(0)); + EXPECT_LT(0, submapSize(1)); + EXPECT_LT(0.0, submapLength(0)); + EXPECT_LT(0.0, submapLength(1)); +} + +TEST(getBufferRegionsForSubmap, Trivial) +{ + Size bufferSize(5, 4); + Index submapIndex(0, 0); + Size submapSize(0, 0); + std::vector regions; + + EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize)); + EXPECT_EQ(1, regions.size()); + EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); + EXPECT_EQ(0, regions[0].getStartIndex()[0]); + EXPECT_EQ(0, regions[0].getStartIndex()[1]); + EXPECT_EQ(0, regions[0].getSize()[0]); + EXPECT_EQ(0, regions[0].getSize()[1]); + + submapSize << 0, 7; + EXPECT_FALSE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize)); + + submapSize << 6, 7; + EXPECT_FALSE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize)); +} + +TEST(getBufferRegionsForSubmap, Simple) +{ + Size bufferSize(5, 4); + Index submapIndex(1, 2); + Size submapSize(3, 2); + std::vector regions; + + EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize)); + EXPECT_EQ(1, regions.size()); + EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); + EXPECT_EQ(1, regions[0].getStartIndex()[0]); + EXPECT_EQ(2, regions[0].getStartIndex()[1]); + EXPECT_EQ(3, regions[0].getSize()[0]); + EXPECT_EQ(2, regions[0].getSize()[1]); +} + +TEST(getBufferRegionsForSubmap, CircularBuffer) +{ + Size bufferSize(5, 4); + Index submapIndex; + Size submapSize; + Index bufferStartIndex(3, 1); + std::vector regions; + + submapIndex << 3, 1; + submapSize << 2, 3; + EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex)); + EXPECT_EQ(1, regions.size()); + EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); + EXPECT_EQ(3, regions[0].getStartIndex()[0]); + EXPECT_EQ(1, regions[0].getStartIndex()[1]); + EXPECT_EQ(2, regions[0].getSize()[0]); + EXPECT_EQ(3, regions[0].getSize()[1]); + + submapIndex << 4, 1; + submapSize << 2, 3; + EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex)); + EXPECT_EQ(2, regions.size()); + EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); + EXPECT_EQ(4, regions[0].getStartIndex()[0]); + EXPECT_EQ(1, regions[0].getStartIndex()[1]); + EXPECT_EQ(1, regions[0].getSize()[0]); + EXPECT_EQ(3, regions[0].getSize()[1]); + EXPECT_EQ(BufferRegion::Quadrant::BottomLeft, regions[1].getQuadrant()); + EXPECT_EQ(0, regions[1].getStartIndex()[0]); + EXPECT_EQ(1, regions[1].getStartIndex()[1]); + EXPECT_EQ(1, regions[1].getSize()[0]); + EXPECT_EQ(3, regions[1].getSize()[1]); + + submapIndex << 1, 0; + submapSize << 2, 1; + EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex)); + EXPECT_EQ(1, regions.size()); + EXPECT_EQ(BufferRegion::Quadrant::BottomRight, regions[0].getQuadrant()); + EXPECT_EQ(1, regions[0].getStartIndex()[0]); + EXPECT_EQ(0, regions[0].getStartIndex()[1]); + EXPECT_EQ(2, regions[0].getSize()[0]); + EXPECT_EQ(1, regions[0].getSize()[1]); + + submapIndex << 3, 1; + submapSize << 5, 4; + EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));\ + EXPECT_EQ(4, regions.size()); + EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); + EXPECT_EQ(3, regions[0].getStartIndex()[0]); + EXPECT_EQ(1, regions[0].getStartIndex()[1]); + EXPECT_EQ(2, regions[0].getSize()[0]); + EXPECT_EQ(3, regions[0].getSize()[1]); + EXPECT_EQ(BufferRegion::Quadrant::TopRight, regions[1].getQuadrant()); + EXPECT_EQ(3, regions[1].getStartIndex()[0]); + EXPECT_EQ(0, regions[1].getStartIndex()[1]); + EXPECT_EQ(2, regions[1].getSize()[0]); + EXPECT_EQ(1, regions[1].getSize()[1]); + EXPECT_EQ(BufferRegion::Quadrant::BottomLeft, regions[2].getQuadrant()); + EXPECT_EQ(0, regions[2].getStartIndex()[0]); + EXPECT_EQ(1, regions[2].getStartIndex()[1]); + EXPECT_EQ(3, regions[2].getSize()[0]); + EXPECT_EQ(3, regions[2].getSize()[1]); + EXPECT_EQ(BufferRegion::Quadrant::BottomRight, regions[3].getQuadrant()); + EXPECT_EQ(0, regions[3].getStartIndex()[0]); + EXPECT_EQ(0, regions[3].getStartIndex()[1]); + EXPECT_EQ(3, regions[3].getSize()[0]); + EXPECT_EQ(1, regions[3].getSize()[1]); +} + +TEST(checkIncrementIndex, Simple) +{ + Index index(0, 0); + Size bufferSize(4, 3); + + EXPECT_TRUE(incrementIndex(index, bufferSize)); + EXPECT_EQ(0, index[0]); + EXPECT_EQ(1, index[1]); + + EXPECT_TRUE(incrementIndex(index, bufferSize)); + EXPECT_EQ(0, index[0]); + EXPECT_EQ(2, index[1]); + + EXPECT_TRUE(incrementIndex(index, bufferSize)); + EXPECT_EQ(1, index[0]); + EXPECT_EQ(0, index[1]); + + EXPECT_TRUE(incrementIndex(index, bufferSize)); + EXPECT_EQ(1, index[0]); + EXPECT_EQ(1, index[1]); + + for (int i = 0; i < 6; i++) { + EXPECT_TRUE(incrementIndex(index, bufferSize)); + } + EXPECT_EQ(3, index[0]); + EXPECT_EQ(1, index[1]); + + EXPECT_TRUE(incrementIndex(index, bufferSize)); + EXPECT_EQ(3, index[0]); + EXPECT_EQ(2, index[1]); + + EXPECT_FALSE(incrementIndex(index, bufferSize)); + EXPECT_EQ(index[0], index[0]); + EXPECT_EQ(index[1], index[1]); +} + +TEST(checkIncrementIndex, CircularBuffer) +{ + Size bufferSize(4, 3); + Index bufferStartIndex(2, 1); + Index index(bufferStartIndex); + + EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_EQ(2, index[0]); + EXPECT_EQ(2, index[1]); + + EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_EQ(2, index[0]); + EXPECT_EQ(0, index[1]); + + EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_EQ(3, index[0]); + EXPECT_EQ(1, index[1]); + + EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_EQ(3, index[0]); + EXPECT_EQ(2, index[1]); + + EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_EQ(3, index[0]); + EXPECT_EQ(0, index[1]); + + EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_EQ(0, index[0]); + EXPECT_EQ(1, index[1]); + + EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_EQ(0, index[0]); + EXPECT_EQ(2, index[1]); + + EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_EQ(0, index[0]); + EXPECT_EQ(0, index[1]); + + EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_EQ(1, index[0]); + EXPECT_EQ(1, index[1]); + + EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_EQ(1, index[0]); + EXPECT_EQ(2, index[1]); + + EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_EQ(1, index[0]); + EXPECT_EQ(0, index[1]); + + EXPECT_FALSE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_EQ(index[0], index[0]); + EXPECT_EQ(index[1], index[1]); +} + +TEST(checkIncrementIndexForSubmap, Simple) +{ + Index submapIndex(0, 0); + Index index; + Index submapTopLeftIndex(3, 1); + Size submapBufferSize(2, 4); + Size bufferSize(8, 5); + + EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize)); + EXPECT_EQ(0, submapIndex[0]); + EXPECT_EQ(1, submapIndex[1]); + EXPECT_EQ(3, index[0]); + EXPECT_EQ(2, index[1]); + + EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize)); + EXPECT_EQ(0, submapIndex[0]); + EXPECT_EQ(2, submapIndex[1]); + EXPECT_EQ(3, index[0]); + EXPECT_EQ(3, index[1]); + + EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize)); + EXPECT_EQ(0, submapIndex[0]); + EXPECT_EQ(3, submapIndex[1]); + EXPECT_EQ(3, index[0]); + EXPECT_EQ(4, index[1]); + + EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize)); + EXPECT_EQ(1, submapIndex[0]); + EXPECT_EQ(0, submapIndex[1]); + EXPECT_EQ(4, index[0]); + EXPECT_EQ(1, index[1]); + + submapIndex << 1, 2; + EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize)); + EXPECT_EQ(1, submapIndex[0]); + EXPECT_EQ(3, submapIndex[1]); + EXPECT_EQ(4, index[0]); + EXPECT_EQ(4, index[1]); + + EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize)); + + submapIndex << 2, 0; + EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize)); +} + +TEST(checkIncrementIndexForSubmap, CircularBuffer) +{ + Index submapIndex(0, 0); + Index index; + Index submapTopLeftIndex(6, 3); + Size submapBufferSize(2, 4); + Size bufferSize(8, 5); + Index bufferStartIndex(3, 2); + + EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex)); + EXPECT_EQ(0, submapIndex[0]); + EXPECT_EQ(1, submapIndex[1]); + EXPECT_EQ(6, index[0]); + EXPECT_EQ(4, index[1]); + + EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex)); + EXPECT_EQ(0, submapIndex[0]); + EXPECT_EQ(2, submapIndex[1]); + EXPECT_EQ(6, index[0]); + EXPECT_EQ(0, index[1]); + + EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex)); + EXPECT_EQ(0, submapIndex[0]); + EXPECT_EQ(3, submapIndex[1]); + EXPECT_EQ(6, index[0]); + EXPECT_EQ(1, index[1]); + + EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex)); + EXPECT_EQ(1, submapIndex[0]); + EXPECT_EQ(0, submapIndex[1]); + EXPECT_EQ(7, index[0]); + EXPECT_EQ(3, index[1]); + + submapIndex << 1, 2; + EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex)); + EXPECT_EQ(1, submapIndex[0]); + EXPECT_EQ(3, submapIndex[1]); + EXPECT_EQ(7, index[0]); + EXPECT_EQ(1, index[1]); + + EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex)); + + submapIndex << 2, 0; + EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex)); +} + +TEST(getIndexFromLinearIndex, Simple) +{ + EXPECT_TRUE((Index(0, 0) == getIndexFromLinearIndex(0, Size(8, 5), false)).all()); + EXPECT_TRUE((Index(1, 0) == getIndexFromLinearIndex(1, Size(8, 5), false)).all()); + EXPECT_TRUE((Index(0, 1) == getIndexFromLinearIndex(1, Size(8, 5), true)).all()); + EXPECT_TRUE((Index(2, 0) == getIndexFromLinearIndex(2, Size(8, 5), false)).all()); + EXPECT_TRUE((Index(0, 1) == getIndexFromLinearIndex(8, Size(8, 5), false)).all()); + EXPECT_TRUE((Index(7, 4) == getIndexFromLinearIndex(39, Size(8, 5), false)).all()); +} + +} // namespace grid_map \ No newline at end of file diff --git a/test/GridMapTest.cpp b/test/GridMapTest.cpp new file mode 100644 index 0000000..64b15f8 --- /dev/null +++ b/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 + +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 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 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 ["< 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 \ No newline at end of file diff --git a/test/LineIteratorTest.cpp b/test/LineIteratorTest.cpp new file mode 100644 index 0000000..2086fc1 --- /dev/null +++ b/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 + +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 \ No newline at end of file diff --git a/test/PolygonIteratorTest.cpp b/test/PolygonIteratorTest.cpp new file mode 100644 index 0000000..ef73ae6 --- /dev/null +++ b/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 + +// Vector +#include + +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 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()); +} diff --git a/test/PolygonTest.cpp b/test/PolygonTest.cpp new file mode 100644 index 0000000..0974a05 --- /dev/null +++ b/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 + +// Eigen +#include + +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 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 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 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 polygons; + polygons = rectangle.triangulate(); + ASSERT_EQ(2, polygons.size()); + // TODO Extend. +} diff --git a/test/SlidingWindowIteratorTest.cpp b/test/SlidingWindowIteratorTest.cpp new file mode 100644 index 0000000..902144a --- /dev/null +++ b/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 +#include + +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()); +} diff --git a/test/SpiralIteratorTest.cpp b/test/SpiralIteratorTest.cpp new file mode 100644 index 0000000..f206dfc --- /dev/null +++ b/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 + +// Vector +#include + +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)); +} diff --git a/test/SubmapIteratorTest.cpp b/test/SubmapIteratorTest.cpp new file mode 100644 index 0000000..3c838ea --- /dev/null +++ b/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 + +// gtest +#include + +// Vector +#include + +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 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 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(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 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 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 diff --git a/test/test_grid_map_core.cpp b/test/test_grid_map_core.cpp new file mode 100644 index 0000000..78b7680 --- /dev/null +++ b/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 + +// 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(); +} diff --git a/test/test_helpers.cpp b/test/test_helpers.cpp new file mode 100644 index 0000000..96d78fa --- /dev/null +++ b/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 + +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 shift(-3.0, 3.0); + std::uniform_real_distribution 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 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 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 var(0.1, 3.0); + std::uniform_real_distribution mean(-4.0, 4.0); + std::uniform_real_distribution scale(-3.0, 3.0); + constexpr int numGaussians = 3; + std::array 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(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 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 Ux(-dimX, dimX); + std::uniform_real_distribution Uy(-dimY, dimY); + + std::vector 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 &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 */ + diff --git a/test/test_helpers.hpp b/test/test_helpers.hpp new file mode 100644 index 0000000..075963c --- /dev/null +++ b/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 +#include +#include + +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 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 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 &queryPoints, + grid_map::InterpolationMethods interpolationMethod); + +} // namespace grid_map_test