diff --git a/ros/src/vendor/grid_map/grid_map_core/CHANGELOG.rst b/ros/src/vendor/grid_map/grid_map_core/CHANGELOG.rst new file mode 100644 index 00000000000..ee7000212e6 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/CHANGELOG.rst @@ -0,0 +1,105 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package grid_map_core +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +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/ros/src/vendor/grid_map/grid_map_core/CMakeLists.txt b/ros/src/vendor/grid_map/grid_map_core/CMakeLists.txt new file mode 100644 index 00000000000..265d16deb60 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/CMakeLists.txt @@ -0,0 +1,126 @@ +cmake_minimum_required(VERSION 2.8.3) +project(grid_map_core) + +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS) + +## Define Eigen addons. +include(cmake/${PROJECT_NAME}-extras.cmake) + +## System dependencies are found with CMake's conventions +#find_package(Eigen3 REQUIRED) +# Solution to find Eigen3 with Saucy. +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() + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS + include + ${EIGEN3_INCLUDE_DIR} + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + DEPENDS + #Eigen3 + CFG_EXTRAS + ${PROJECT_NAME}-extras.cmake +) + +########### +## Build ## +########### + +## Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +## Declare a cpp library +add_library(${PROJECT_NAME} + src/GridMap.cpp + src/GridMapMath.cpp + src/SubmapGeometry.cpp + src/BufferRegion.cpp + src/Polygon.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 +) + +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# Mark executables and/or libraries for installation +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.hpp" +) + +# Mark other files for installation +install( + DIRECTORY doc + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +if(CATKIN_ENABLE_TESTING) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") +## Add gtest based cpp test target and link libraries +catkin_add_gtest(${PROJECT_NAME}-test + test/test_grid_map_core.cpp + test/GridMapMathTest.cpp + test/GridMapTest.cpp + test/GridMapIteratorTest.cpp + test/LineIteratorTest.cpp + test/EllipseIteratorTest.cpp + test/SubmapIteratorTest.cpp + test/PolygonIteratorTest.cpp + test/PolygonTest.cpp + test/EigenPluginsTest.cpp + test/SpiralIteratorTest.cpp + test/SlidingWindowIteratorTest.cpp + ) +endif() + +if(TARGET ${PROJECT_NAME}-test) + target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +endif() diff --git a/ros/src/vendor/grid_map/grid_map_core/cmake/grid_map_core-extras.cmake b/ros/src/vendor/grid_map/grid_map_core/cmake/grid_map_core-extras.cmake new file mode 100644 index 00000000000..e63eae44387 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/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/ros/src/vendor/grid_map/grid_map_core/doc/grid_map_conventions.pdf b/ros/src/vendor/grid_map/grid_map_core/doc/grid_map_conventions.pdf new file mode 100644 index 00000000000..405588f3d34 Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/grid_map_conventions.pdf differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/grid_map_conventions.png b/ros/src/vendor/grid_map/grid_map_core/doc/grid_map_conventions.png new file mode 100644 index 00000000000..d75537693ff Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/grid_map_conventions.png differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/grid_map_layers.pdf b/ros/src/vendor/grid_map/grid_map_core/doc/grid_map_layers.pdf new file mode 100644 index 00000000000..bfc4830b6a0 Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/grid_map_layers.pdf differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/grid_map_layers.png b/ros/src/vendor/grid_map/grid_map_core/doc/grid_map_layers.png new file mode 100644 index 00000000000..ef56e52287b Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/grid_map_layers.png differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/iterators/circle_iterator.gif b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/circle_iterator.gif new file mode 100644 index 00000000000..d1bc507174e Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/circle_iterator.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/iterators/circle_iterator_preview.gif b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/circle_iterator_preview.gif new file mode 100644 index 00000000000..0b447d2c6cf Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/circle_iterator_preview.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/iterators/ellipse_iterator.gif b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/ellipse_iterator.gif new file mode 100644 index 00000000000..a401b30818b Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/ellipse_iterator.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/iterators/ellipse_iterator_preview.gif b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/ellipse_iterator_preview.gif new file mode 100644 index 00000000000..18908ac7b3a Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/ellipse_iterator_preview.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/iterators/grid_map_iterator.gif b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/grid_map_iterator.gif new file mode 100644 index 00000000000..b89b64bd92c Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/grid_map_iterator.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/iterators/grid_map_iterator_preview.gif b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/grid_map_iterator_preview.gif new file mode 100644 index 00000000000..b76f5d328db Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/grid_map_iterator_preview.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/iterators/line_iterator.gif b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/line_iterator.gif new file mode 100644 index 00000000000..1a664ab5d9f Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/line_iterator.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/iterators/line_iterator_preview.gif b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/line_iterator_preview.gif new file mode 100644 index 00000000000..18e70a8208e Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/line_iterator_preview.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/iterators/polygon_iterator.gif b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/polygon_iterator.gif new file mode 100644 index 00000000000..96f20c409af Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/polygon_iterator.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/iterators/polygon_iterator_preview.gif b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/polygon_iterator_preview.gif new file mode 100644 index 00000000000..33e8d3764ca Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/polygon_iterator_preview.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/iterators/spiral_iterator.gif b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/spiral_iterator.gif new file mode 100644 index 00000000000..ff287dc499a Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/spiral_iterator.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/iterators/spiral_iterator_preview.gif b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/spiral_iterator_preview.gif new file mode 100644 index 00000000000..8699b3c1f79 Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/spiral_iterator_preview.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/iterators/submap_iterator.gif b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/submap_iterator.gif new file mode 100644 index 00000000000..c9038bfd4fe Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/submap_iterator.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/iterators/submap_iterator_preview.gif b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/submap_iterator_preview.gif new file mode 100644 index 00000000000..49784d3d6fa Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/iterators/submap_iterator_preview.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/move_method.gif b/ros/src/vendor/grid_map/grid_map_core/doc/move_method.gif new file mode 100644 index 00000000000..4be9d7f6a78 Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/move_method.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/doc/setposition_method.gif b/ros/src/vendor/grid_map/grid_map_core/doc/setposition_method.gif new file mode 100644 index 00000000000..21b87ba906b Binary files /dev/null and b/ros/src/vendor/grid_map/grid_map_core/doc/setposition_method.gif differ diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/BufferRegion.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/BufferRegion.hpp new file mode 100644 index 00000000000..9a2dce8f5a9 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/BufferRegion.hpp @@ -0,0 +1,63 @@ +/* + * BufferRegion.hpp + * + * Created on: Aug 19, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#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(const Index& startIndex, const Size& size, const BufferRegion::Quadrant& quadrant); + virtual ~BufferRegion(); + + 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 staretIndex_; + + //! 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/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/GridMap.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/GridMap.hpp new file mode 100644 index 00000000000..85010951576 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/GridMap.hpp @@ -0,0 +1,528 @@ +/* + * GridMap.hpp + * + * Created on: Jul 14, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#pragma once + +#include "grid_map_core/TypeDefs.hpp" +#include "grid_map_core/SubmapGeometry.hpp" +#include "grid_map_core/BufferRegion.hpp" + +// STL +#include +#include + +// Eigen +#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. + */ + 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(); + + /*! + * 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 double 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; + + /*! + * 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); + + /*! + * 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. + * 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(); + + private: + + /*! + * 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; + + /*! + * 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 indeces. + Index startIndex_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} /* namespace */ diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/GridMapMath.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/GridMapMath.hpp new file mode 100644 index 00000000000..cb31118cbc3 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/GridMapMath.hpp @@ -0,0 +1,357 @@ +/* + * GridMapMath.hpp + * + * Created on: Dec 2, 2013 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#pragma once + +#include "grid_map_core/TypeDefs.hpp" +#include "grid_map_core/BufferRegion.hpp" + +#include +#include +#include + +namespace grid_map { + +/*! + * 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/indeces 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 indeces. + * @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 indeces 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 indeces 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, const 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. + */ +const 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 getSubmapSizeFromCornerIndeces(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 indeces, 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 indeces, 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, const 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(const size_t linearIndex, const Size& bufferSize, const bool rowMajor = false); + +/*! + * Generates a list of indices for a region in the map. + * @param regionIndex the region top-left index. + * @param regionSize the region size. + * @param indices the list of indices of the region. + */ +void getIndicesForRegion(const Index& regionIndex, const Size& regionSize, + std::vector indices); + +/*! + * Generates a list of indices for multiple regions in the map. + * This method makes sure every index is only once contained in the list. + * @param regionIndeces the regions' top-left index. + * @param regionSizes the regions' sizes. + * @param indices the list of indices of the regions. + */ +void getIndicesForRegions(const std::vector& regionIndeces, const Size& regionSizes, + std::vector indices); + +/*! + * 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 diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/Polygon.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/Polygon.hpp new file mode 100644 index 00000000000..9fa28949f8f --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/Polygon.hpp @@ -0,0 +1,237 @@ +/* + * Polygon.hpp + * + * Created on: Nov 7, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#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); + + /*! + * Destructor. + */ + virtual ~Polygon(); + + /*! + * 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(const 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 [](const 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. + */ + const size_t nVertices() const; + + /*! + * Set the timestamp of the polygon. + * @param timestamp the timestamp to set (in nanoseconds). + */ + void setTimestamp(const 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. + */ + const 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(const double margin); + + /*! + * If only two verices 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(const 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(const Position center, const double radius, + const 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(const Position center1, + const Position center2, + const double radius, + const 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); + + 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); + + //! Frame id of the polygon. + std::string frameId_; + + //! Timestamp of the polygon (nanoseconds). + uint64_t timestamp_; + + //! Vertices of the polygon. + std::vector vertices_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} /* namespace grid_map */ diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/SubmapGeometry.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/SubmapGeometry.hpp new file mode 100644 index 00000000000..5ae25394b04 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/SubmapGeometry.hpp @@ -0,0 +1,73 @@ +/* + * SubmapGeometry.hpp + * + * Created on: Aug 18, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#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(); + + 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/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/TypeDefs.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/TypeDefs.hpp new file mode 100644 index 00000000000..6c325af01af --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/TypeDefs.hpp @@ -0,0 +1,33 @@ +/* + * TypeDefs.hpp + * + * Created on: March 18, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +// Eigen +#include + +#pragma once + +namespace grid_map { + + typedef Eigen::MatrixXf Matrix; + typedef Matrix::Scalar DataType; + typedef Eigen::Vector2d Position; + typedef Eigen::Vector2d Vector; + typedef Eigen::Vector3d Position3; + typedef Eigen::Vector3d Vector3; + typedef Eigen::Array2i Index; + typedef Eigen::Array2i Size; + typedef Eigen::Array2d Length; + typedef uint64_t Time; + + enum class InterpolationMethods{ + INTER_NEAREST, // nearest neighbor interpolation + INTER_LINEAR // bilinear interpolation + // ToDo: INTER_CUBIC + }; + +} /* namespace */ diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp new file mode 100644 index 00000000000..8b3dd6f9b3d --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp @@ -0,0 +1,26 @@ +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/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/eigen_plugins/Functors.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/eigen_plugins/Functors.hpp new file mode 100644 index 00000000000..604bf8163ff --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/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, Autonomous Systems Lab + */ + +#pragma once + +namespace grid_map { + +template +struct Clamp +{ + Clamp(const Scalar& min, const Scalar& max) + : min_(min), + max_(max) + { + } + const Scalar operator()(const Scalar& x) const + { + return x < min_ ? min_ : (x > max_ ? max_ : x); + } + Scalar min_, max_; +}; + +} diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp new file mode 100644 index 00000000000..79dcdd665c8 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/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/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/grid_map_core.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/grid_map_core.hpp new file mode 100644 index 00000000000..648d58c5e4f --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/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, Autonomous Systems Lab + */ + +#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/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/gtest_eigen.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/gtest_eigen.hpp new file mode 100644 index 00000000000..18c55b63ada --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/gtest_eigen.hpp @@ -0,0 +1,161 @@ +/** + * @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))); + } 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))); + } 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))); + } 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 = "") + { + 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 = NULL) + { + // \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) + *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 diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/CircleIterator.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/CircleIterator.hpp new file mode 100644 index 00000000000..b66e2361202 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/CircleIterator.hpp @@ -0,0 +1,107 @@ +/* + * CircleIterator.hpp + * + * Created on: Nov 13, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#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, const double radius); + + /*! + * Assignment operator. + * @param iterator the iterator to copy data from. + * @return a reference to *this. + */ + CircleIterator& operator =(const CircleIterator& other); + + /*! + * 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, const 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 */ diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/EllipseIterator.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/EllipseIterator.hpp new file mode 100644 index 00000000000..026b0c68f2f --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/EllipseIterator.hpp @@ -0,0 +1,116 @@ +/* + * EllipseIterator.hpp + * + * Created on: Dec 2, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#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, const double rotation = 0.0); + + /*! + * Assignment operator. + * @param iterator the iterator to copy data from. + * @return a reference to *this. + */ + EllipseIterator& operator =(const EllipseIterator& other); + + /*! + * 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, const 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 */ diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/GridMapIterator.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/GridMapIterator.hpp new file mode 100644 index 00000000000..65c7c135adf --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/GridMapIterator.hpp @@ -0,0 +1,110 @@ +/* + * GridMapIterator.hpp + * + * Created on: Sep 22, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#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); + + /*! + * Assignment operator. + * @param iterator the iterator to copy data from. + * @return a reference to *this. + */ + GridMapIterator& operator =(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. + */ + const 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. + */ + const 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: + + //! 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_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} /* namespace */ diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/LineIterator.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/LineIterator.hpp new file mode 100644 index 00000000000..d758a8b7ace --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/LineIterator.hpp @@ -0,0 +1,131 @@ +/* + * LineIterator.hpp + * + * Created on: Nov 13, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#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. + */ + 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); + + /*! + * Assignment operator. + * @param iterator the iterator to copy data from. + * @return a reference to *this. + */ + LineIterator& operator =(const LineIterator& other); + + /*! + * 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. + */ + 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_; + + //! Number of cells in the line. + unsigned int nCells_; + + //! Helper variables for Bresenham Line Drawing algorithm. + Size increment1_, increment2_; + int denominator_, numerator_, numeratorAdd_; + + //! 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 */ diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/PolygonIterator.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/PolygonIterator.hpp new file mode 100644 index 00000000000..221040ca87e --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/PolygonIterator.hpp @@ -0,0 +1,97 @@ +/* + * PolygonIterator.hpp + * + * Created on: Sep 19, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#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); + + /*! + * Assignment operator. + * @param iterator the iterator to copy data from. + * @return a reference to *this. + */ + PolygonIterator& operator =(const PolygonIterator& other); + + /*! + * 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 */ diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/SlidingWindowIterator.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/SlidingWindowIterator.hpp new file mode 100644 index 00000000000..68afe036cda --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/SlidingWindowIterator.hpp @@ -0,0 +1,94 @@ +/* + * SlidingWindowIterator.hpp + * + * Created on: Aug 17, 2017 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#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, + const size_t windowSize = 3); + + /*! + * Copy constructor. + * @param other the object to copy. + */ + 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, const double windowLength); + + /*! + * Increase the iterator to the next element. + * @return a reference to the updated iterator. + */ + SlidingWindowIterator& operator ++(); + + /*! + * Return the data of the sliding window. + * @return the data of the sliding window. + */ + const 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_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} /* namespace */ diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/SpiralIterator.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/SpiralIterator.hpp new file mode 100644 index 00000000000..73933ff660f --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/SpiralIterator.hpp @@ -0,0 +1,115 @@ +/* + * SpiralIterator.hpp + * + * Created on: Jul 7, 2015 + * Author: Martin Wermelinger + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#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, const Eigen::Vector2d& center, const double radius); + + /*! + * Assignment operator. + * @param iterator the iterator to copy data from. + * @return a reference to *this. + */ + SpiralIterator& operator =(const SpiralIterator& other); + + /*! + * 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(); + + int signum(const int val) { + return (0 < val) - (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 */ diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/SubmapIterator.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/SubmapIterator.hpp new file mode 100644 index 00000000000..51ae1493db2 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/SubmapIterator.hpp @@ -0,0 +1,125 @@ +/* + * SubmapIterator.hpp + * + * Created on: Sep 12, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#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); + + /*! + * Assignment operator. + * @param iterator the iterator to copy data from. + * @return a reference to *this. + */ + SubmapIterator& operator =(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 */ diff --git a/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/iterators.hpp b/ros/src/vendor/grid_map/grid_map_core/include/grid_map_core/iterators/iterators.hpp new file mode 100644 index 00000000000..e9144834178 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/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, Autonomous Systems Lab + */ + +#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/ros/src/vendor/grid_map/grid_map_core/package.xml b/ros/src/vendor/grid_map/grid_map_core/package.xml new file mode 100644 index 00000000000..541c98436ed --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/package.xml @@ -0,0 +1,13 @@ + + + grid_map_core + 1.6.0 + Universal grid map library to manage two-dimensional grid maps with multiple data layers. + Péter Fankhauser + BSD + http://github.com/ethz-asl/grid_map + http://github.com/ethz-asl/grid_map/issues + Péter Fankhauser + catkin + eigen + diff --git a/ros/src/vendor/grid_map/grid_map_core/src/BufferRegion.cpp b/ros/src/vendor/grid_map/grid_map_core/src/BufferRegion.cpp new file mode 100644 index 00000000000..411d8805173 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/src/BufferRegion.cpp @@ -0,0 +1,62 @@ +/* + * BufferRegion.cpp + * + * Created on: Aug 19, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ +#include + +namespace grid_map { + +BufferRegion::BufferRegion() : + staretIndex_(Index::Zero()), + size_(Size::Zero()), + quadrant_(BufferRegion::Quadrant::Undefined) +{ +} + +BufferRegion::BufferRegion(const Index& index, const Size& size, const BufferRegion::Quadrant& quadrant) : + staretIndex_(index), + size_(size), + quadrant_(quadrant) +{ +} + +BufferRegion::~BufferRegion() +{ +} + +const Index& BufferRegion::getStartIndex() const +{ + return staretIndex_; +} + +void BufferRegion::setStartIndex(const Index& staretIndex) +{ + staretIndex_ = staretIndex; +} + +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/ros/src/vendor/grid_map/grid_map_core/src/GridMap.cpp b/ros/src/vendor/grid_map/grid_map_core/src/GridMap.cpp new file mode 100644 index 00000000000..a5f4e3594f4 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/src/GridMap.cpp @@ -0,0 +1,721 @@ +/* + * GridMap.cpp + * + * Created on: Jul 14, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/GridMap.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 +#include + +using namespace std; +using namespace grid_map; + +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()) +{ +} + +GridMap::~GridMap() +{ +} + +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(); + + return; +} + +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_.size() > 0; +} + +bool GridMap::hasSameLayers(const GridMap& other) const +{ + for (const auto& layer : layers_) { + if (!other.exists(layer)) return false; + } + return true; +} + +void GridMap::add(const std::string& layer, const double 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 +{ + switch (interpolationMethod) { + case InterpolationMethods::INTER_LINEAR: + { + float value; + if (atPositionLinearInterpolated(layer, position, value)) + return value; + else + interpolationMethod = InterpolationMethods::INTER_NEAREST; + } + 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(const Index& index) const +{ + return isValid(index, basicLayers_); +} + +bool GridMap::isValid(const Index& index, const std::string& layer) const +{ + if (!isfinite(at(layer, index))) return false; + return true; +} + +bool GridMap::isValid(const Index& index, const std::vector& layers) const +{ + if (layers.empty()) return false; + for (auto& layer : layers) { + if (!isfinite(at(layer, index))) return false; + } + return true; +} + +bool GridMap::getPosition3(const std::string& layer, const Index& index, + Position3& position) const +{ + if (!isValid(index, layer)) return false; + Position position2d; + getPosition(index, position2d); + position.head(2) = position2d; + position.z() = at(layer, index); + return true; +} + +bool GridMap::getVector(const std::string& layerPrefix, const Index& index, + Eigen::Vector3d& vector) const +{ + std::vector layers; + layers.push_back(layerPrefix + "x"); + layers.push_back(layerPrefix + "y"); + layers.push_back(layerPrefix + "z"); + if (!isValid(index, layers)) return false; + for (size_t i = 0; i < 3; ++i) { + vector(i) = at(layers[i], index); + } + 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 the 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 == false) 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; +} + +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.push_back(BufferRegion(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.push_back(BufferRegion(Index(index, 0), Size(nCells, getSize()(1)), BufferRegion::Quadrant::Undefined)); + } else if (i == 1) { + clearCols(index, nCells); + newRegions.push_back(BufferRegion(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.push_back(BufferRegion(Index(firstIndex, 0), Size(firstNCells, getSize()(1)), BufferRegion::Quadrant::Undefined)); + } else if (i == 1) { + clearCols(firstIndex, firstNCells); + newRegions.push_back(BufferRegion(Index(0, firstIndex), Size(getSize()(0), firstNCells), BufferRegion::Quadrant::Undefined)); + } + + int secondIndex = 0; + int secondNCells = nCells - firstNCells; + if (i == 0) { + clearRows(secondIndex, secondNCells); + newRegions.push_back(BufferRegion(Index(secondIndex, 0), Size(secondNCells, getSize()(1)), BufferRegion::Quadrant::Undefined)); + } else if (i == 1) { + clearCols(secondIndex, secondNCells); + newRegions.push_back(BufferRegion(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() != 0); +} + +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(); +} + +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) +{ + std::vector layersToClear; + if (basicLayers_.size() > 0) layersToClear = basicLayers_; + else layersToClear = layers_; + for (auto& layer : layersToClear) { + data_.at(layer).block(index, 0, nRows, getSize()(1)).setConstant(NAN); + } +} + +void GridMap::clearCols(unsigned int index, unsigned int nCols) +{ + std::vector layersToClear; + if (basicLayers_.size() > 0) layersToClear = basicLayers_; + else layersToClear = layers_; + for (auto& layer : layersToClear) { + 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() + + f[2] * positionRedFlip.x() * positionRed.y() + + f[3] * positionRed.x() * positionRed.y(); + return true; +} + +void GridMap::resize(const Index& size) +{ + size_ = size; + for (auto& data : data_) { + data.second.resize(size_(0), size_(1)); + } +} + +} /* namespace */ + diff --git a/ros/src/vendor/grid_map/grid_map_core/src/GridMapMath.cpp b/ros/src/vendor/grid_map/grid_map_core/src/GridMapMath.cpp new file mode 100644 index 00000000000..7669bbfb672 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/src/GridMapMath.cpp @@ -0,0 +1,555 @@ +/* + * GridMapMath.cpp + * + * Created on: Dec 2, 2013 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/GridMapMath.hpp" + +// fabs +#include + +// Limits +#include + +using namespace std; + +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 Eigen::Matrix2i getMapFrameToBufferOrderTransformation() +{ + return getBufferOrderToMapFrameTransformation().transpose(); +} + +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 (getBufferOrderToMapFrameTransformation() * unwrappedIndex.matrix()).cast(); +} + +inline Index getIndexFromIndexVector( + const Vector& indexVector, + const Size& bufferSize, + const Index& bufferStartIndex) +{ + Index index = (getMapFrameToBufferOrderTransformation() * indexVector.cast()).array(); + 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 + +using namespace internal; + +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); + if (!checkIfPositionWithinMap(position, mapLength, mapPosition)) return false; + return true; +} + +bool checkIfPositionWithinMap(const Position& position, + const Length& mapLength, + const Position& mapPosition) +{ + Vector offset; + getVectorToOrigin(offset, mapLength); + Position positionTransformed = getMapFrameToBufferOrderTransformation().cast() * (position - mapPosition - offset); + + if (positionTransformed.x() >= 0.0 && positionTransformed.y() >= 0.0 + && positionTransformed.x() < mapLength(0) && positionTransformed.y() < mapLength(1)) { + return true; + } + return false; +} + +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 = (getMapFrameToBufferOrderTransformation() * indexShiftVector).array(); + return true; +} + +bool getPositionShiftFromIndexShift(Vector& positionShift, + const Index& indexShift, + const double& resolution) +{ + positionShift = (getBufferOrderToMapFrameTransformation() * indexShift.matrix()).cast() * resolution; + return true; +} + +bool checkIfIndexInRange(const Index& index, const Size& bufferSize) +{ + if (index[0] >= 0 && index[1] >= 0 && index[0] < bufferSize[0] && index[1] < bufferSize[1]) + { + return true; + } + return false; +} + +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, const int& bufferSize) +{ + if (index < 0) index += ((-index / bufferSize) + 1) * bufferSize; + 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; +} + +const 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) + Eigen::Matrix2d transform = getMapFrameToBufferOrderTransformation().cast(); + + // Corners of submap. + Position topLeftPosition = requestedSubmapPosition - transform * 0.5 * 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 + transform * 0.5 * 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 -= transform * Position::Constant(0.5 * 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. + if(!getIndexFromPosition(requestedIndexInSubmap, requestedSubmapPosition, submapLength, submapPosition, resolution, submapBufferSize)) return false; + + return true; +} + +Size getSubmapSizeFromCornerIndeces(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.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopLeft)); + return true; + } + + if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) { + Size topLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1)); + submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft)); + + Index topRightIndex(submapIndex(0), 0); + Size topRightSize(submapBufferSize(0), submapBufferSize(1) - topLeftSize(1)); + submapBufferRegions.push_back(BufferRegion(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight)); + return true; + } + + if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) { + Size topLeftSize(bufferSize(0) - submapIndex(0), submapBufferSize(1)); + submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft)); + + Index bottomLeftIndex(0, submapIndex(1)); + Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), submapBufferSize(1)); + submapBufferRegions.push_back(BufferRegion(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft)); + return true; + } + + if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) { + Size topLeftSize(bufferSize(0) - submapIndex(0), bufferSize(1) - submapIndex(1)); + submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft)); + + Index topRightIndex(submapIndex(0), 0); + Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1) - topLeftSize(1)); + submapBufferRegions.push_back(BufferRegion(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight)); + + Index bottomLeftIndex(0, submapIndex(1)); + Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), bufferSize(1) - submapIndex(1)); + submapBufferRegions.push_back(BufferRegion(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft)); + + Index bottomRightIndex = Index::Zero(); + Size bottomRightSize(bottomLeftSize(0), topRightSize(1)); + submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight)); + return true; + } + + } else if (quadrantOfTopLeft == BufferRegion::Quadrant::TopRight) { + + if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) { + submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopRight)); + return true; + } + + if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) { + + Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1)); + submapBufferRegions.push_back(BufferRegion(submapIndex, topRightSize, BufferRegion::Quadrant::TopRight)); + + Index bottomRightIndex(0, submapIndex(1)); + Size bottomRightSize(submapBufferSize(0) - topRightSize(0), submapBufferSize(1)); + submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight)); + return true; + } + + } else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomLeft) { + + if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) { + submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomLeft)); + return true; + } + + if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) { + Size bottomLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1)); + submapBufferRegions.push_back(BufferRegion(submapIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft)); + + Index bottomRightIndex(submapIndex(0), 0); + Size bottomRightSize(submapBufferSize(0), submapBufferSize(1) - bottomLeftSize(1)); + submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight)); + return true; + } + + } else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomRight) { + + if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) { + submapBufferRegions.push_back(BufferRegion(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)); +} + +void getIndicesForRegion(const Index& regionIndex, const Size& regionSize, + std::vector indices) +{ +// for (int i = line.index_; col < line.endIndex(); col++) { +// for (int i = 0; i < getSize()(0); i++) { +// +// } +// } +} + +void getIndicesForRegions(const std::vector& regionIndeces, const Size& regionSizes, + std::vector indices) +{ +} + +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) +{ + unsigned long color = (colorVector(0) << 16) + (colorVector(1) << 8) + colorVector(2); + // cppcheck-suppress invalidPointerCast + colorValue = *reinterpret_cast(&color); +} + +void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue) +{ + Eigen::Vector3i tempColorVector = (colorVector * 255.0).cast(); + colorVectorToValue(tempColorVector, colorValue); +} + +} // namespace + diff --git a/ros/src/vendor/grid_map/grid_map_core/src/Polygon.cpp b/ros/src/vendor/grid_map/grid_map_core/src/Polygon.cpp new file mode 100644 index 00000000000..7bc5381f2a2 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/src/Polygon.cpp @@ -0,0 +1,338 @@ +/* + * Polygon.cpp + * + * Created on: Nov 7, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include + +#include +#include + +#include + +namespace grid_map { + +Polygon::Polygon() + : timestamp_(0) +{ +} + +Polygon::Polygon(std::vector vertices) + : Polygon() +{ + vertices_ = vertices; +} + +Polygon::~Polygon() {} + +bool Polygon::isInside(const Position& point) const +{ + int cross = 0; + for (int 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_; +} + +const 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; +} + +const double Polygon::getArea() const +{ + double area = 0.0; + int j = vertices_.size() - 1; + for (int 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 (int 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()); + + std::vector hull(vertices.size()+1); + + // Sort points lexicographically. + std::sort(vertices.begin(), vertices.end(), sortVertices); + + int k = 0; + // Build lower hull + for (int i = 0; i < vertices.size(); ++i) { + while (k >= 2 + && computeCrossProduct2D(hull.at(k - 1) - hull.at(k - 2), + vertices.at(i) - hull.at(k - 2)) <= 0) + k--; + hull.at(k++) = vertices.at(i); + } + + // Build upper hull. + for (int i = vertices.size() - 2, t = k + 1; i >= 0; i--) { + while (k >= t + && computeCrossProduct2D(hull.at(k - 1) - hull.at(k - 2), + vertices.at(i) - hull.at(k - 2)) <= 0) + k--; + hull.at(k++) = vertices.at(i); + } + hull.resize(k - 1); + + Polygon polygon(hull); + 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()); +} + +} /* namespace grid_map */ + diff --git a/ros/src/vendor/grid_map/grid_map_core/src/SubmapGeometry.cpp b/ros/src/vendor/grid_map/grid_map_core/src/SubmapGeometry.cpp new file mode 100644 index 00000000000..032b3fa5061 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/src/SubmapGeometry.cpp @@ -0,0 +1,63 @@ +/* + * SubmapGeometry.cpp + * + * Created on: Aug 18, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#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()); +} + +SubmapGeometry::~SubmapGeometry() +{ +} + +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/ros/src/vendor/grid_map/grid_map_core/src/iterators/CircleIterator.cpp b/ros/src/vendor/grid_map/grid_map_core/src/iterators/CircleIterator.cpp new file mode 100644 index 00000000000..d4cf7a7f3d8 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/src/iterators/CircleIterator.cpp @@ -0,0 +1,96 @@ +/* + * Circleterator.hpp + * + * Created on: Nov 13, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/iterators/CircleIterator.hpp" +#include "grid_map_core/GridMapMath.hpp" + +using namespace std; + +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::shared_ptr(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize)); + if(!isInside()) ++(*this); +} + +CircleIterator& CircleIterator::operator =(const CircleIterator& other) +{ + center_ = other.center_; + radius_ = other.radius_; + radiusSquare_ = other.radiusSquare_; + internalIterator_ = other.internalIterator_; + mapLength_ = other.mapLength_; + mapPosition_ = other.mapPosition_; + resolution_ = other.resolution_; + bufferSize_ = other.bufferSize_; + bufferStartIndex_ = other.bufferStartIndex_; + return *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 = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_); +} + +} /* namespace grid_map */ + diff --git a/ros/src/vendor/grid_map/grid_map_core/src/iterators/EllipseIterator.cpp b/ros/src/vendor/grid_map/grid_map_core/src/iterators/EllipseIterator.cpp new file mode 100644 index 00000000000..5a7cd5196f9 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/src/iterators/EllipseIterator.cpp @@ -0,0 +1,110 @@ +/* + * EllipseIterator.hpp + * + * Created on: Dec 2, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/iterators/EllipseIterator.hpp" +#include "grid_map_core/GridMapMath.hpp" + +#include +#include + +using namespace std; + +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 = sin(rotation); + double cosRotation = 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::shared_ptr(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize)); + if(!isInside()) ++(*this); +} + +EllipseIterator& EllipseIterator::operator =(const EllipseIterator& other) +{ + center_ = other.center_; + semiAxisSquare_ = other.semiAxisSquare_; + transformMatrix_ = other.transformMatrix_; + internalIterator_ = other.internalIterator_; + mapLength_ = other.mapLength_; + mapPosition_ = other.mapPosition_; + resolution_ = other.resolution_; + bufferSize_ = other.bufferSize_; + bufferStartIndex_ = other.bufferStartIndex_; + return *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 = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_); +} + +} /* namespace grid_map */ + diff --git a/ros/src/vendor/grid_map/grid_map_core/src/iterators/GridMapIterator.cpp b/ros/src/vendor/grid_map/grid_map_core/src/iterators/GridMapIterator.cpp new file mode 100644 index 00000000000..62e31f0e785 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/src/iterators/GridMapIterator.cpp @@ -0,0 +1,85 @@ +/* + * GridMapIterator.cpp + * + * Created on: Sep 22, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#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_; +} + +GridMapIterator& GridMapIterator::operator =(const GridMapIterator& other) +{ + size_ = other.size_; + startIndex_ = other.startIndex_; + linearSize_ = other.linearSize_; + linearIndex_ = other.linearIndex_; + isPastEnd_ = other.isPastEnd_; + return *this; +} + +bool GridMapIterator::operator !=(const GridMapIterator& other) const +{ + return linearIndex_ != other.linearIndex_; +} + +const Index GridMapIterator::operator *() const +{ + return getIndexFromLinearIndex(linearIndex_, size_); +} + +const size_t& GridMapIterator::getLinearIndex() const +{ + return linearIndex_; +} + +const 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/ros/src/vendor/grid_map/grid_map_core/src/iterators/LineIterator.cpp b/ros/src/vendor/grid_map/grid_map_core/src/iterators/LineIterator.cpp new file mode 100644 index 00000000000..6f85bf07ac1 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/src/iterators/LineIterator.cpp @@ -0,0 +1,154 @@ +/* + * LineIterator.hpp + * + * Created on: Nov 13, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/iterators/LineIterator.hpp" +#include "grid_map_core/GridMapMath.hpp" + +using namespace std; + +namespace grid_map { + +LineIterator::LineIterator(const grid_map::GridMap& gridMap, const Position& start, + const Position& end) +{ + Index startIndex, endIndex; + if (getIndexLimitedToMapRange(gridMap, start, end, startIndex) + && getIndexLimitedToMapRange(gridMap, end, start, endIndex)) + initialize(gridMap, startIndex, endIndex); +} + +LineIterator::LineIterator(const grid_map::GridMap& gridMap, const Index& start, const Index& end) +{ + initialize(gridMap, start, end); +} + +LineIterator& LineIterator::operator =(const LineIterator& other) +{ + index_ = other.index_; + start_ = other.start_; + end_ = other.end_; + iCell_ = other.iCell_; + nCells_ = other.nCells_; + increment1_ = other.increment1_; + increment2_ = other.increment2_; + denominator_ = other.denominator_; + numerator_ = other.numerator_; + numeratorAdd_ = other.numeratorAdd_; + mapLength_ = other.mapLength_; + mapPosition_ = other.mapPosition_; + resolution_ = other.resolution_; + bufferSize_ = other.bufferSize_; + bufferStartIndex_ = other.bufferStartIndex_; + return *this; +} + +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/ros/src/vendor/grid_map/grid_map_core/src/iterators/PolygonIterator.cpp b/ros/src/vendor/grid_map/grid_map_core/src/iterators/PolygonIterator.cpp new file mode 100644 index 00000000000..5d6f563d1ff --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/src/iterators/PolygonIterator.cpp @@ -0,0 +1,94 @@ +/* + * PolygonIterator.hpp + * + * Created on: Sep 19, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/iterators/PolygonIterator.hpp" +#include "grid_map_core/GridMapMath.hpp" + +using namespace std; + +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::shared_ptr(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize)); + if(!isInside()) ++(*this); +} + +PolygonIterator& PolygonIterator::operator =(const PolygonIterator& other) +{ + polygon_ = other.polygon_; + internalIterator_ = other.internalIterator_; + mapLength_ = other.mapLength_; + mapPosition_ = other.mapPosition_; + resolution_ = other.resolution_; + bufferSize_ = other.bufferSize_; + bufferStartIndex_ = other.bufferStartIndex_; + return *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 = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_); +} + +} /* namespace grid_map */ + diff --git a/ros/src/vendor/grid_map/grid_map_core/src/iterators/SlidingWindowIterator.cpp b/ros/src/vendor/grid_map/grid_map_core/src/iterators/SlidingWindowIterator.cpp new file mode 100644 index 00000000000..af13130c01b --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/src/iterators/SlidingWindowIterator.cpp @@ -0,0 +1,108 @@ +/* + * SlidingWindowIterator.cpp + * + * Created on: Aug 17, 2017 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#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_ = 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; +} + +const Matrix SlidingWindowIterator::getData() const +{ + const Index centerIndex(*(*this)); + const Index windowMargin(Index::Constant(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(windowMargin_)); + const Index topLeftIndex(centerIndex - windowMargin); + const Index bottomRightIndex(centerIndex + windowMargin); + return checkIfIndexInRange(topLeftIndex, size_) && checkIfIndexInRange(bottomRightIndex, size_); +} + +} /* namespace grid_map */ diff --git a/ros/src/vendor/grid_map/grid_map_core/src/iterators/SpiralIterator.cpp b/ros/src/vendor/grid_map/grid_map_core/src/iterators/SpiralIterator.cpp new file mode 100644 index 00000000000..1930000b27f --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/src/iterators/SpiralIterator.cpp @@ -0,0 +1,123 @@ +/* + * SpiralIterator.hpp + * + * Created on: Jul 7, 2015 + * Author: Martin Wermelinger + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/iterators/SpiralIterator.hpp" +#include "grid_map_core/GridMapMath.hpp" + +#include + +using namespace std; + +namespace grid_map { + +SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Vector2d& center, + const double radius) + : center_(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_ = std::ceil(radius_ / resolution_); + if (checkIfIndexInRange(indexCenter_, bufferSize_)) + pointsRing_.push_back(indexCenter_); + else + while(pointsRing_.empty() && !isPastEnd()) + generateRing(); +} + +SpiralIterator& SpiralIterator::operator =(const SpiralIterator& other) +{ + center_ = other.center_; + indexCenter_ = other.indexCenter_; + radius_ = other.radius_; + radiusSquare_ = other.radiusSquare_; + nRings_ = other.nRings_; + distance_ = other.distance_; + pointsRing_ = other.pointsRing_; + mapLength_ = other.mapLength_; + mapPosition_ = other.mapPosition_; + resolution_ = other.resolution_; + bufferSize_ = other.bufferSize_; + return *this; +} + +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 + && (int) Vector(point.x() + normal.x(), point.y()).norm() == distance_) + point.x() += normal.x(); + else if (normal.y() != 0 + && (int) Vector(point.x(), point.y() + normal.y()).norm() == distance_) + point.y() += normal.y(); + else { + point.x() += normal.x(); + point.y() += normal.y(); + } + } while (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/ros/src/vendor/grid_map/grid_map_core/src/iterators/SubmapIterator.cpp b/ros/src/vendor/grid_map/grid_map_core/src/iterators/SubmapIterator.cpp new file mode 100644 index 00000000000..47b445c0703 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/src/iterators/SubmapIterator.cpp @@ -0,0 +1,96 @@ +/* + * SubmapIterator.hpp + * + * Created on: Sep 22, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/iterators/SubmapIterator.hpp" +#include "grid_map_core/GridMapMath.hpp" + +using namespace std; + +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_; +} + +SubmapIterator& SubmapIterator::operator =(const SubmapIterator& other) +{ + size_ = other.size_; + startIndex_ = other.startIndex_; + submapSize_ = other.submapSize_; + submapStartIndex_ = other.submapStartIndex_; + index_ = other.index_; + submapIndex_ = other.submapIndex_; + isPastEnd_ = other.isPastEnd_; + return *this; +} + +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/ros/src/vendor/grid_map/grid_map_core/test/EigenPluginsTest.cpp b/ros/src/vendor/grid_map/grid_map_core/test/EigenPluginsTest.cpp new file mode 100644 index 00000000000..d519b88967f --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/test/EigenPluginsTest.cpp @@ -0,0 +1,114 @@ +/* + * EigenMatrixBaseAddonsTest.cpp + * + * Created on: Sep 23, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/grid_map_core.hpp" + +// gtest +#include + +// Eigen +#include + +using namespace std; +using namespace Eigen; + +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, 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, 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/ros/src/vendor/grid_map/grid_map_core/test/EllipseIteratorTest.cpp b/ros/src/vendor/grid_map/grid_map_core/test/EllipseIteratorTest.cpp new file mode 100644 index 00000000000..5c939929596 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/test/EllipseIteratorTest.cpp @@ -0,0 +1,60 @@ +/* + * EllipseIteratorTest.cpp + * + * Created on: Dec 2, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/iterators/EllipseIterator.hpp" +#include "grid_map_core/GridMap.hpp" + +// Eigen +#include + +// gtest +#include + +// Limits +#include + +// Vector +#include + +using namespace std; +using namespace Eigen; +using namespace grid_map; + +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/ros/src/vendor/grid_map/grid_map_core/test/GridMapIteratorTest.cpp b/ros/src/vendor/grid_map/grid_map_core/test/GridMapIteratorTest.cpp new file mode 100644 index 00000000000..ffd2d6d4037 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/test/GridMapIteratorTest.cpp @@ -0,0 +1,63 @@ +/* + * GridMapDataIterator.cpp + * + * Created on: Feb 16, 2016 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/iterators/GridMapIterator.hpp" +#include "grid_map_core/GridMap.hpp" + +// Eigen +#include + +// gtest +#include + +// Limits +#include + +// Vector +#include + +using namespace std; +using namespace grid_map; + +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(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/ros/src/vendor/grid_map/grid_map_core/test/GridMapMathTest.cpp b/ros/src/vendor/grid_map/grid_map_core/test/GridMapMathTest.cpp new file mode 100644 index 00000000000..d094b113342 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/test/GridMapMathTest.cpp @@ -0,0 +1,987 @@ +/* + * GridMapMathTest.cpp + * + * Created on: Feb 10, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/GridMapMath.hpp" + +// Eigen +#include + +// gtest +#include + +// Limits +#include + +// Vector +#include + +using namespace std; +using 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); + + EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, -1.5), mapLength, mapPosition)); + 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 index; + int bufferSize = 10; + + 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 index; + int bufferSize = 10; + + 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 indeces + 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 indeces + 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 indeces + 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 indeces + 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 indeces + 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 indeces + 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()); +} diff --git a/ros/src/vendor/grid_map/grid_map_core/test/GridMapTest.cpp b/ros/src/vendor/grid_map/grid_map_core/test/GridMapTest.cpp new file mode 100644 index 00000000000..f6c0449bd54 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/test/GridMapTest.cpp @@ -0,0 +1,223 @@ +/* + * GridMapTest.cpp + * + * Created on: Aug 26, 2015 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/GridMap.hpp" + +// gtest +#include + +// Math +#include + +using namespace std; +using 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)); + + EXPECT_FALSE(map.isValid(Index(0, 0))); // TODO Check entire map. + EXPECT_TRUE(map.isValid(Index(3, 2))); + EXPECT_FALSE(map.isValid(Index(2, 2))); + EXPECT_FALSE(map.isValid(Index(3, 1))); + EXPECT_TRUE(map.isValid(Index(7, 4))); + + EXPECT_EQ(2, regions.size()); + EXPECT_EQ(0, regions[0].getStartIndex()[0]); + EXPECT_EQ(0, regions[0].getStartIndex()[1]); + EXPECT_EQ(3, regions[0].getSize()[0]); + EXPECT_EQ(5, regions[0].getSize()[1]); + EXPECT_EQ(0, regions[1].getStartIndex()[0]); + EXPECT_EQ(0, regions[1].getStartIndex()[1]); + EXPECT_EQ(8, regions[1].getSize()[0]); + EXPECT_EQ(2, regions[1].getSize()[1]); +} + +TEST(AddDataFrom, ExtendMapAligned) +{ + GridMap map1, map2; + map1.setGeometry(Length(5.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(5, 5) + map1.add("zero", 0.0); + map1.add("one", 1.0); + map1.setBasicLayers(map1.getLayers()); + + map2.setGeometry(Length(3.1, 3.1), 1.0, Position(2.0, 2.0)); + map2.add("one", 1.1); + map2.add("two", 2.0); + map2.setBasicLayers(map1.getLayers()); + + map1.addDataFrom(map2, true, true, true); + + EXPECT_TRUE(map1.exists("two")); + EXPECT_TRUE(map1.isInside(Position(3.0, 3.0))); + EXPECT_DOUBLE_EQ(6.0, map1.getLength().x()); + EXPECT_DOUBLE_EQ(6.0, map1.getLength().y()); + EXPECT_DOUBLE_EQ(0.5, map1.getPosition().x()); + EXPECT_DOUBLE_EQ(0.5, map1.getPosition().y()); + EXPECT_NEAR(1.1, map1.atPosition("one", Position(2, 2)), 1e-4); + EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(-2, -2))); + EXPECT_DOUBLE_EQ(0.0, map1.atPosition("zero", Position(0.0, 0.0))); +} + +TEST(AddDataFrom, ExtendMapNotAligned) +{ + GridMap map1, map2; + map1.setGeometry(Length(6.1, 6.1), 1.0, Position(0.0, 0.0)); // bufferSize(6, 6) + map1.add("nan"); + map1.add("one", 1.0); + map1.add("zero", 0.0); + map1.setBasicLayers(map1.getLayers()); + + map2.setGeometry(Length(3.1, 3.1), 1.0, Position(3.2, 3.2)); + map2.add("nan", 1.0); + map2.add("one", 1.1); + map2.add("two", 2.0); + map2.setBasicLayers(map1.getLayers()); + + std::vector stringVector; + stringVector.push_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, 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; + + 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; + + double value; + + // Close to the border -> reverting to INTER_NEAREST. + 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); +} diff --git a/ros/src/vendor/grid_map/grid_map_core/test/LineIteratorTest.cpp b/ros/src/vendor/grid_map/grid_map_core/test/LineIteratorTest.cpp new file mode 100644 index 00000000000..bd382855510 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/test/LineIteratorTest.cpp @@ -0,0 +1,177 @@ +/* + * LineIteratorTest.cpp + * + * Created on: Sep 14, 2016 + * Author: Dominic Jud + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#include "grid_map_core/iterators/LineIterator.hpp" +#include "grid_map_core/GridMap.hpp" + +// gtest +#include + +// Limits +#include + +using namespace grid_map; + +TEST(LineIterator, StartOutsideMap) +{ + GridMap map( { "types" }); + map.setGeometry(Length(8.0, 5.0), 1.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)); + + 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)); + + 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)); + + LineIterator iterator(map, Position(-8.0, 8.0), Position(8.0, 8.0)); + + EXPECT_TRUE(iterator.isPastEnd()); +} + +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)); + + 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)); + + 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()); +} diff --git a/ros/src/vendor/grid_map/grid_map_core/test/PolygonIteratorTest.cpp b/ros/src/vendor/grid_map/grid_map_core/test/PolygonIteratorTest.cpp new file mode 100644 index 00000000000..33db795a20c --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/test/PolygonIteratorTest.cpp @@ -0,0 +1,195 @@ +/* + * PolygonIteratorTest.cpp + * + * Created on: Sep 19, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/iterators/PolygonIterator.hpp" +#include "grid_map_core/GridMap.hpp" +#include "grid_map_core/Polygon.hpp" + +// Eigen +#include + +// gtest +#include + +// Limits +#include + +// Vector +#include + +using namespace std; +using namespace Eigen; +using namespace grid_map; + +TEST(PolygonIterator, FullCover) +{ + vector types; + types.push_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/ros/src/vendor/grid_map/grid_map_core/test/PolygonTest.cpp b/ros/src/vendor/grid_map/grid_map_core/test/PolygonTest.cpp new file mode 100644 index 00000000000..26531dbc5ca --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/test/PolygonTest.cpp @@ -0,0 +1,232 @@ +/* + * PolygonTest.cpp + * + * Created on: Mar 24, 2015 + * Author: Martin Wermelinger, Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/Polygon.hpp" + +// gtest +#include + +// Eigen +#include +#include + +using namespace std; +using namespace Eigen; +using namespace grid_map; + +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, 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/ros/src/vendor/grid_map/grid_map_core/test/SlidingWindowIteratorTest.cpp b/ros/src/vendor/grid_map/grid_map_core/test/SlidingWindowIteratorTest.cpp new file mode 100644 index 00000000000..b1ff23b01d6 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/test/SlidingWindowIteratorTest.cpp @@ -0,0 +1,140 @@ +/* + * 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 +#include +#include + +using namespace std; +using namespace grid_map; + +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/ros/src/vendor/grid_map/grid_map_core/test/SpiralIteratorTest.cpp b/ros/src/vendor/grid_map/grid_map_core/test/SpiralIteratorTest.cpp new file mode 100644 index 00000000000..a43dd2a8bf9 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/test/SpiralIteratorTest.cpp @@ -0,0 +1,41 @@ +/* + * 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" + +// Eigen +#include + +// gtest +#include + +// Limits +#include + +// Vector +#include + +using namespace std; +using namespace Eigen; +using namespace grid_map; + +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/ros/src/vendor/grid_map/grid_map_core/test/SubmapIteratorTest.cpp b/ros/src/vendor/grid_map/grid_map_core/test/SubmapIteratorTest.cpp new file mode 100644 index 00000000000..43df7f233f3 --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/test/SubmapIteratorTest.cpp @@ -0,0 +1,167 @@ +/* + * SubmapIteratorTest.cpp + * + * Created on: Sep 15, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, Autonomous Systems Lab + */ + +#include "grid_map_core/iterators/SubmapIterator.hpp" +#include "grid_map_core/GridMap.hpp" + +// Eigen +#include + +// gtest +#include + +// Limits +#include + +// Vector +#include + +using namespace std; +using namespace Eigen; +using 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.push_back("type"); + GridMap map(types); + map.setGeometry(Array2d(8.1, 5.1), 1.0, 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.push_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)); +} diff --git a/ros/src/vendor/grid_map/grid_map_core/test/test_grid_map_core.cpp b/ros/src/vendor/grid_map/grid_map_core/test/test_grid_map_core.cpp new file mode 100644 index 00000000000..c1f3a46c03c --- /dev/null +++ b/ros/src/vendor/grid_map/grid_map_core/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, Autonomous Systems Lab + */ + +// gtest +#include + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + srand((int)time(0)); + return RUN_ALL_TESTS(); +}