Added unit tests for grid_map_cv. Updated documentation.

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

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()

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<typename Type_, int NChannels_>
   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<typename Type_, int NChannels_>
+  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<Type_, NChannels_>(gridMap, layer, encoding, minValue, maxValue, image);
+  };
+
   /*!
    * Creates a cv mat from a grid map layer.
    * @param[in] grid map to be added.

grid_map_cv/test/GridMapCvTest.cpp

@@ -6,10 +6,90 @@
  *	 Institute: ETH Zurich, Autonomous Systems Lab
  */
 
+#include <grid_map_core/grid_map_core.hpp>
+#include <grid_map_core/gtest_eigen.hpp>
 #include "grid_map_cv/grid_map_cv.hpp"
 
 // gtest
 #include <gtest/gtest.h>
 
+// OpenCV
+#include <opencv/cv.h>
+
 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<unsigned char, 3>(mapIn, "layer", CV_8UC3, minValue, maxValue, image);
+
+  // Convert back to grid map.
+  GridMap mapOut(mapIn);
+  mapOut["layer"].setConstant(NAN);
+  GridMapCvConverter::addLayerFromImage<unsigned char, 3>(image, "layer", mapOut, minValue, maxValue);
+
+  // Check data.
+  const float resolution = (maxValue - minValue) / (float) std::numeric_limits<unsigned char>::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<unsigned char, 4>(mapIn, "layer", CV_8UC4, minValue, maxValue, image);
+
+  // Convert back to grid map.
+  GridMap mapOut(mapIn);
+  mapOut["layer"].setConstant(NAN);
+  GridMapCvConverter::addLayerFromImage<unsigned char, 4>(image, "layer", mapOut, minValue, maxValue);
+
+  // Check data.
+  const float resolution = (maxValue - minValue) / (float) std::numeric_limits<unsigned char>::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<unsigned short, 1>(mapIn, "layer", CV_16UC1, minValue, maxValue, image);
+
+  // Convert back to grid map.
+  GridMap mapOut(mapIn);
+  mapOut["layer"].setConstant(NAN);
+  GridMapCvConverter::addLayerFromImage<unsigned short, 1>(image, "layer", mapOut, minValue, maxValue);
+
+  // Check data.
+  const float resolution = (maxValue - minValue) / (float) std::numeric_limits<unsigned char>::max();
+  expectNear(mapIn["layer"], mapOut["layer"], resolution, "");
+  EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all());
+  EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all());
+}

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.

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<unsigned char, 1>(cvImage->image, layer, gridMap, lowerValue, upperValue);
+      return GridMapCvConverter::addLayerFromImage<unsigned char, 1>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
     case CV_8UC3:
-      return GridMapCvConverter::addLayerFromImage<unsigned char, 3>(cvImage->image, layer, gridMap, lowerValue, upperValue);
+      return GridMapCvConverter::addLayerFromImage<unsigned char, 3>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
     case CV_8UC4:
-      return GridMapCvConverter::addLayerFromImage<unsigned char, 4>(cvImage->image, layer, gridMap, lowerValue, upperValue);
+      return GridMapCvConverter::addLayerFromImage<unsigned char, 4>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
     case CV_16UC1:
-      return GridMapCvConverter::addLayerFromImage<unsigned short, 1>(cvImage->image, layer, gridMap, lowerValue, upperValue);
+      return GridMapCvConverter::addLayerFromImage<unsigned short, 1>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
     case CV_16UC3:
-      return GridMapCvConverter::addLayerFromImage<unsigned short, 3>(cvImage->image, layer, gridMap, lowerValue, upperValue);
+      return GridMapCvConverter::addLayerFromImage<unsigned short, 3>(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold);
     case CV_16UC4:
-      return GridMapCvConverter::addLayerFromImage<unsigned short, 4>(cvImage->image, layer, gridMap, lowerValue, upperValue);
+      return GridMapCvConverter::addLayerFromImage<unsigned short, 4>(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;