diff --git a/README.md b/README.md index f9b99e4be..5198d40df 100644 --- a/README.md +++ b/README.md @@ -11,6 +11,7 @@ Features: * **Based on Eigen:** Grid map data is stored as [Eigen] data types. Users can apply available Eigen algorithms directly to the map data for versatile and efficient data manipulation. * **Convenience functions:** Several helper methods allow for convenient and memory safe cell data access. For example, iterator functions for rectangular, circular, polygonal regions and lines are implemented. * **ROS interface:** Grid maps can be directly converted to ROS message types such as PointCloud2, OccupancyGrid, GridCells, and our custom GridMap message. +* **OpenCV interface:** Grid maps can be seamlessly converted from and to [OpenCV] image types to make use of the tools provided by [OpenCV]. * **Visualizations:** The *grid_map_visualization* package helps to visualize grid maps in various form in [RViz]. The grid map package has been tested with [ROS] Indigo and Jade under Ubuntu 14.04. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. @@ -54,29 +55,33 @@ in Robot Operating System (ROS) – The Complete Reference (Volume 1), A. Koubaa To install all packages from the grid map library as Debian packages use sudo apt-get install ros-indigo-grid-map - + ### Building from Source #### Dependencies -Except for ROS packages that are part of the standard installation (*roscpp*, *tf*, *filters*, *sensor_msgs*, *nav_msgs*, and *cv_bridge*), the grid map library depends only on the linear algebra library [Eigen]. +The *grid_map_core* package depends only on the linear algebra library [Eigen]. sudo apt-get install libeigen3-dev +The *grid_map_cv* package depends additionally on [OpenCV]. + +The other packages depend additionally on the [ROS] standard installation (*roscpp*, *tf*, *filters*, *sensor_msgs*, *nav_msgs*, and *cv_bridge*). + #### Building -To install, clone the latest version from this repository into your catkin workspace and compile the package using +To build from source, clone the latest version from this repository into your catkin workspace and compile the package using cd catkin_ws/src git clone https://github.com/ethz-asl/grid_map.git cd ../ catkin_make - + To maximize performance, make sure to build in *Release* mode. You can specify the build type by setting catkin_make -DCMAKE_BUILD_TYPE=Release - + ### Packages Overview @@ -85,9 +90,10 @@ This repository consists of following packages: * ***grid_map*** is the meta-package for the grid map library. * ***grid_map_core*** implements the algorithms of the grid map library. It provides the `GridMap` class and several helper classes such as the iterators. This package is implemented without [ROS] dependencies. * ***grid_map_ros*** is the main package for [ROS] dependent projects using the grid map library. It provides the interfaces to convert the base classes to several [ROS] message types. +* ***grid_map_cv*** provides conversions of grid maps from and to [OpenCV] image types. * ***grid_map_msgs*** holds the [ROS] message and service definitions around the [grid_map_msg/GridMap] message type. * ***grid_map_visualization*** contains a node written to convert GridMap messages to other [ROS] message types for visualization in [RViz]. The visualization parameters are configurable through [ROS] parameters. -* ***grid_map_filters*** builds on the ROS [filters](http://wiki.ros.org/filters) package to process grid maps as a sequence of filters. +* ***grid_map_filters*** builds on the ROS [filters](http://wiki.ros.org/filters) package to process grid maps as a sequence of filters. * ***grid_map_demos*** contains several nodes for demonstration purposes. @@ -100,7 +106,7 @@ Run the unit tests with or catkin build grid_map --no-deps --verbose --catkin-make-args run_tests - + if you are using [catkin tools](http://catkin-tools.readthedocs.org/). ## Usage @@ -112,21 +118,27 @@ The *grid_map_demos* package contains several demonstration nodes. Use this code * *[simple_demo](grid_map_demos/src/simple_demo_node.cpp)* demonstrates a simple example for using the grid map library. This ROS node creates a grid map, adds data to it, and publishes it. To see the result in RViz, execute the command roslaunch grid_map_demos simple_demo.launch - + * *[tutorial_demo](grid_map_demos/src/tutorial_demo_node.cpp)* is an extended demonstration of the library's functionalities. Launch the *tutorial_demo* with - + roslaunch grid_map_demos tutorial_demo.launch * *[iterators_demo](grid_map_demos/src/IteratorsDemo.cpp)* showcases the usage of the grid map iterators. Launch it with roslaunch grid_map_demos iterators_demo.launch - + * *[image_to_gridmap_demo](grid_map_demos/src/ImageToGridmapDemo.cpp)* demonstrates how to convert data from an [image](grid_map_demos/scripts/example_image.png) to a grid map. Start the demonstration with - + roslaunch grid_map_demos image_to_gridmap_demo.launch ![Image to grid map demo result](grid_map_demos/doc/image_to_grid_map_demo_result.png) +* *[opencv_demo](grid_map_demos/src/opencv_demo_node.cpp)* demonstrates map manipulations with help of [OpenCV] functions. Start the demonstration with + + roslaunch grid_map_demos opencv_demo.launch + + ![OpenCV demo result](grid_map_demos/doc/opencv_demo_result.gif) + ### Conventions & Definitions @@ -179,7 +191,7 @@ Beware that while iterators are convenient, it is often the cleanest and most ef map["layer"] = 2.0 * map["layer"]; -* Max. values between two layers: +* Max. values between two layers: map["max"] = map["layer_1"].cwiseMax(map["layer_2"]); @@ -198,7 +210,7 @@ There are two different methods to change the position of the map: `setPosition(...)` | `move(...)` :---: | :---: -![Grid map iterator](grid_map_core/doc/setposition_method.gif) | ![Submap iterator](grid_map_core/doc/move_method.gif)| +![Grid map iterator](grid_map_core/doc/setposition_method.gif) | ![Submap iterator](grid_map_core/doc/move_method.gif)| ## Nodes @@ -216,7 +228,7 @@ Point cloud | Vectors | Occupancy grid | Grid cells * **`grid_map_topic`** (string, default: "/grid_map") The name of the grid map topic to be visualized. See below for the description of the visualizers. - + #### Subscribed Topics @@ -253,7 +265,7 @@ The published topics are configured with the [YAML parameter file](grid_map_demo * **`vectors`** ([visualization_msgs/Marker]) Visualizes vector data of the grid map as visual markers. Specify the layers which hold the *x*-, *y*-, and *z*-components of the vectors with the `layer_prefix` parameter. The parameter `position_layer` defines the layer to be used as start point of the vectors. - + name: surface_normals type: vectors params: @@ -262,11 +274,11 @@ The published topics are configured with the [YAML parameter file](grid_map_demo scale: 0.06 line_width: 0.005 color: 15600153 # red - + * **`occupancy_grid`** ([nav_msgs/OccupancyGrid]) Visualizes a layer of the grid map as occupancy grid. Specify the layer to be visualized with the `layer` parameter, and the upper and lower bound with `data_min` and `data_max`. - + name: traversability_grid type: occupancy_grid params: @@ -277,18 +289,18 @@ The published topics are configured with the [YAML parameter file](grid_map_demo * **`grid_cells`** ([nav_msgs/GridCells]) Visualizes a layer of the grid map as grid cells. Specify the layer to be visualized with the `layer` parameter, and the upper and lower bounds with `lower_threshold` and `upper_threshold`. - + name: elevation_cells type: grid_cells params: layer: elevation lower_threshold: -0.08 # optional, default: -inf upper_threshold: 0.08 # optional, default: inf - + * **`region`** ([visualization_msgs/Marker]) - + Shows the boundary of the grid map. - + name: map_region type: map_region params: @@ -328,6 +340,7 @@ Please report bugs and request features using the [Issue Tracker](https://github [ROS]: http://www.ros.org [RViz]: http://wiki.ros.org/rviz [Eigen]: http://eigen.tuxfamily.org +[OpenCV]: http://opencv.org/ [grid_map_msgs/GridMapInfo]: http://docs.ros.org/api/grid_map_msgs/html/msg/GridMapInfo.html [grid_map_msgs/GridMap]: http://docs.ros.org/api/grid_map_msgs/html/msg/GridMap.html [grid_map_msgs/GetGridMap]: http://docs.ros.org/api/grid_map_msgs/html/srv/GetGridMap.html diff --git a/grid_map_cv/CMakeLists.txt b/grid_map_cv/CMakeLists.txt index b643cf09c..d7021beb1 100644 --- a/grid_map_cv/CMakeLists.txt +++ b/grid_map_cv/CMakeLists.txt @@ -59,3 +59,6 @@ install( 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_cv.cpp test/GridMapCvTest.cpp) +if(TARGET ${PROJECT_NAME}-test) + target_link_libraries(${PROJECT_NAME}-test ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) +endif() diff --git a/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp index ae6d65fa4..62a126901 100644 --- a/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp +++ b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp @@ -39,12 +39,14 @@ class GridMapCvConverter * @param[out] gridMap the grid map to be populated. * @param[in](optional) lowerValue value of the layer corresponding to black image pixels. * @param[in](optional) upperValue value of the layer corresponding to white image pixels. + * @param[in](optional) alphaThreshold the threshold ([0.0, 1.0]) for the alpha value at which + * cells in the grid map are marked as empty. * @return true if successful, false otherwise. */ template static bool addLayerFromImage(const cv::Mat& image, const std::string& layer, - grid_map::GridMap& gridMap, const float lowerValue, - const float upperValue, const double alphaThreshold = 0.5) + grid_map::GridMap& gridMap, const float lowerValue = 0.0, + const float upperValue = 1.0, const double alphaThreshold = 0.5) { if (gridMap.getSize()(0) != image.rows || gridMap.getSize()(1) != image.cols) { std::cerr << "Image size does not correspond to grid map size!" << std::endl; @@ -93,6 +95,27 @@ class GridMapCvConverter return true; }; + /*! + * Creates a cv mat from a grid map layer. + * This conversion sets the corresponding black and white pixel value to the + * min. and max. data of the layer data. + * @param[in] grid map to be added. + * @param[in] layer the layer that is converted to the image. + * @param[in] encoding the desired encoding of the image. + * @param[in] lowerValue the value of the layer corresponding to black image pixels. + * @param[in] upperValue the value of the layer corresponding to white image pixels. + * @param[out] image the image to be populated. + * @return true if successful, false otherwise. + */ + template + static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer, + const int encoding, cv::Mat& image) + { + const float minValue = gridMap.get(layer).minCoeffOfFinites(); + const float maxValue = gridMap.get(layer).maxCoeffOfFinites(); + return toImage(gridMap, layer, encoding, minValue, maxValue, image); + }; + /*! * Creates a cv mat from a grid map layer. * @param[in] grid map to be added. diff --git a/grid_map_cv/test/GridMapCvTest.cpp b/grid_map_cv/test/GridMapCvTest.cpp index 56daf6d2e..42038bea7 100644 --- a/grid_map_cv/test/GridMapCvTest.cpp +++ b/grid_map_cv/test/GridMapCvTest.cpp @@ -6,10 +6,90 @@ * Institute: ETH Zurich, Autonomous Systems Lab */ +#include +#include #include "grid_map_cv/grid_map_cv.hpp" // gtest #include +// OpenCV +#include + using namespace std; using namespace grid_map; + +TEST(ImageConversion, roundTrip8UC3) +{ + // Create grid map. + GridMap mapIn({"layer"}); + mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01); + mapIn["layer"].setRandom(); + const float minValue = -1.0; + const float maxValue = 1.0; + + // Convert to image. + cv::Mat image; + GridMapCvConverter::toImage(mapIn, "layer", CV_8UC3, minValue, maxValue, image); + + // Convert back to grid map. + GridMap mapOut(mapIn); + mapOut["layer"].setConstant(NAN); + GridMapCvConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue); + + // Check data. + const float resolution = (maxValue - minValue) / (float) std::numeric_limits::max(); + expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); + EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all()); + EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all()); +} + +TEST(ImageConversion, roundTrip8UC4) +{ + // Create grid map. + GridMap mapIn({"layer"}); + mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01); + mapIn["layer"].setRandom(); + const float minValue = -1.0; + const float maxValue = 1.0; + + // Convert to image. + cv::Mat image; + GridMapCvConverter::toImage(mapIn, "layer", CV_8UC4, minValue, maxValue, image); + + // Convert back to grid map. + GridMap mapOut(mapIn); + mapOut["layer"].setConstant(NAN); + GridMapCvConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue); + + // Check data. + const float resolution = (maxValue - minValue) / (float) std::numeric_limits::max(); + expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); + EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all()); + EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all()); +} + +TEST(ImageConversion, roundTrip16UC1) +{ + // Create grid map. + GridMap mapIn({"layer"}); + mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01); + mapIn["layer"].setRandom(); + const float minValue = -1.0; + const float maxValue = 1.0; + + // Convert to image. + cv::Mat image; + GridMapCvConverter::toImage(mapIn, "layer", CV_16UC1, minValue, maxValue, image); + + // Convert back to grid map. + GridMap mapOut(mapIn); + mapOut["layer"].setConstant(NAN); + GridMapCvConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue); + + // Check data. + const float resolution = (maxValue - minValue) / (float) std::numeric_limits::max(); + expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); + EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all()); + EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all()); +} diff --git a/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp b/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp index f40ec6243..1991b76dd 100644 --- a/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp +++ b/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp @@ -150,11 +150,13 @@ class GridMapRosConverter * @param[out] gridMap the grid map to be populated. * @param[in](optional) lowerValue value of the layer corresponding to black image pixels. * @param[in](optional) upperValue value of the layer corresponding to white image pixels. + * @param[in](optional) alphaThreshold the threshold ([0.0, 1.0]) for the alpha value at which + * cells in the grid map are marked as empty. * @return true if successful, false otherwise. */ static bool addLayerFromImage(const sensor_msgs::Image& image, const std::string& layer, grid_map::GridMap& gridMap, const float lowerValue = 0.0, - const float upperValue = 1.0); + const float upperValue = 1.0, const double alphaThreshold = 0.5); /*! * Adds a color layer with data from an image. diff --git a/grid_map_ros/src/GridMapRosConverter.cpp b/grid_map_ros/src/GridMapRosConverter.cpp index f1e2e8a72..6f24cd31c 100644 --- a/grid_map_ros/src/GridMapRosConverter.cpp +++ b/grid_map_ros/src/GridMapRosConverter.cpp @@ -322,7 +322,8 @@ bool GridMapRosConverter::initializeFromImage(const sensor_msgs::Image& image, bool GridMapRosConverter::addLayerFromImage(const sensor_msgs::Image& image, const std::string& layer, grid_map::GridMap& gridMap, - const float lowerValue, const float upperValue) + const float lowerValue, const float upperValue, + const double alphaThreshold) { cv_bridge::CvImageConstPtr cvImage; try { @@ -336,17 +337,17 @@ bool GridMapRosConverter::addLayerFromImage(const sensor_msgs::Image& image, const int cvEncoding = cv_bridge::getCvType(image.encoding); switch (cvEncoding) { case CV_8UC1: - return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); + return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold); case CV_8UC3: - return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); + return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold); case CV_8UC4: - return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); + return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold); case CV_16UC1: - return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); + return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold); case CV_16UC3: - return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); + return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold); case CV_16UC4: - return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); + return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold); default: ROS_ERROR("Expected MONO8, MONO16, RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding."); return false; diff --git a/grid_map_ros/test/GridMapRosTest.cpp b/grid_map_ros/test/GridMapRosTest.cpp index e7aa1bb98..58f83cbf5 100644 --- a/grid_map_ros/test/GridMapRosTest.cpp +++ b/grid_map_ros/test/GridMapRosTest.cpp @@ -186,48 +186,53 @@ TEST(OccupancyGridConversion, roundTrip) TEST(ImageConversion, roundTripBGRA8) { // Create grid map. - GridMap gridMapInput({"layer"}); - gridMapInput.setGeometry(grid_map::Length(2.0, 1.0), 0.01); - gridMapInput["layer"].setRandom(); + GridMap mapIn({"layer"}); + mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01); + mapIn["layer"].setRandom(); const float minValue = -1.0; const float maxValue = 1.0; // Convert to image message. sensor_msgs::Image image; - GridMapRosConverter::toImage(gridMapInput, "layer", sensor_msgs::image_encodings::BGRA8, minValue, + GridMapRosConverter::toImage(mapIn, "layer", sensor_msgs::image_encodings::BGRA8, minValue, maxValue, image); // Convert back to grid map. - GridMap gridMapOutput; - GridMapRosConverter::initializeFromImage(image, gridMapInput.getResolution(), gridMapOutput); - GridMapRosConverter::addLayerFromImage(image, "layer", gridMapOutput, minValue, maxValue); + GridMap mapOut; + GridMapRosConverter::initializeFromImage(image, mapIn.getResolution(), mapOut); + GridMapRosConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue); // Check data. const float resolution = (maxValue - minValue) / (float) std::numeric_limits::max(); - expectNear(gridMapInput["layer"], gridMapOutput["layer"], resolution, ""); + expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); + EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all()); + EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all()); } TEST(ImageConversion, roundTripMONO16) { // Create grid map. - GridMap gridMapInput({"layer"}); - gridMapInput.setGeometry(grid_map::Length(2.0, 1.0), 0.01); - gridMapInput["layer"].setRandom(); + GridMap mapIn({"layer"}); + mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01); + mapIn["layer"].setRandom(); const float minValue = -1.0; const float maxValue = 1.0; // Convert to image message. sensor_msgs::Image image; - GridMapRosConverter::toImage(gridMapInput, "layer", sensor_msgs::image_encodings::MONO16, + GridMapRosConverter::toImage(mapIn, "layer", sensor_msgs::image_encodings::MONO16, minValue, maxValue, image); // Convert back to grid map. - GridMap gridMapOutput; - GridMapRosConverter::initializeFromImage(image, gridMapInput.getResolution(), gridMapOutput); - GridMapRosConverter::addLayerFromImage(image, "layer", gridMapOutput, minValue, maxValue); + GridMap mapOut; + GridMapRosConverter::initializeFromImage(image, mapIn.getResolution(), mapOut); + GridMapRosConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue); // Check data. // TODO Why is factor 300 necessary? const float resolution = 300.0 * (maxValue - minValue) / (float) std::numeric_limits::max(); - expectNear(gridMapInput["layer"], gridMapOutput["layer"], resolution, ""); + expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); + EXPECT_EQ(mapIn.getTimestamp(), mapOut.getTimestamp()); + EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all()); + EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all()); }