Separated OpenCV to grid map conversions to grid_map_cv package.

grid_map/package.xml

@@ -11,6 +11,7 @@
   <buildtool_depend>catkin</buildtool_depend>
   <exec_depend>grid_map_core</exec_depend>
   <exec_depend>grid_map_ros</exec_depend>
+  <exec_depend>grid_map_cv</exec_depend>
   <exec_depend>grid_map_msgs</exec_depend>
   <exec_depend>grid_map_filters</exec_depend>
   <exec_depend>grid_map_visualization</exec_depend>

grid_map_core/CMakeLists.txt

@@ -111,4 +111,3 @@ catkin_add_gtest(${PROJECT_NAME}-test
 if(TARGET ${PROJECT_NAME}-test)
   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
 endif()
-

grid_map_cv/CHANGELOG.rst

@@ -0,0 +1,7 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package grid_map_cv
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Forthcoming
+-----------
+* Created package.

grid_map_cv/CMakeLists.txt

@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(grid_map_cv)
+
+set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+  grid_map_core
+)
+
+find_package(OpenCV REQUIRED)
+
+###################################
+## 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
+  LIBRARIES
+  CATKIN_DEPENDS
+    grid_map_core
+  DEPENDS
+    OpenCV
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+#############
+## Install ##
+#############
+
+# Mark cpp header files for installation
+install(
+  DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.hpp"
+)
+
+#############
+## 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_cv.cpp test/GridMapCvTest.cpp)

grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp

@@ -0,0 +1,157 @@
+/*
+ * GridMapCvConverter.hpp
+ *
+ *  Created on: Apr 14, 2016
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, Autonomous Systems Lab
+ */
+
+#pragma once
+
+#include <grid_map_core/grid_map_core.hpp>
+
+// OpenCV
+#include <opencv/cv.h>
+
+namespace grid_map {
+
+/*!
+ * Conversions between grid maps and OpenCV images.
+ */
+class GridMapCvConverter
+{
+ public:
+  /*!
+   * Default constructor.
+   */
+  GridMapCvConverter();
+
+  /*!
+   * Destructor.
+   */
+  virtual ~GridMapCvConverter();
+
+  /*!
+   * Adds a layer with data from image.
+   * @param[in] image the image to be added. If it is a color image
+   * (bgr or bgra encoding), it will be transformed in a grayscale image.
+   * @param[in] layer the layer that is filled with the image data.
+   * @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.
+   * @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)
+  {
+    if (gridMap.getSize()(0) != image.rows || gridMap.getSize()(1) != image.cols) {
+      std::cerr << "Image size does not correspond to grid map size!" << std::endl;
+      return false;
+    }
+
+    bool isColor = false;
+    if (image.channels() >= 3) isColor = true;
+    bool hasAlpha = false;
+    if (image.channels() >= 4) hasAlpha = true;
+
+    cv::Mat imageMono;
+    if (isColor && !hasAlpha) {
+      cv::cvtColor(image, imageMono, CV_BGR2GRAY);
+    } else if (isColor && hasAlpha) {
+      cv::cvtColor(image, imageMono, CV_BGRA2GRAY);
+    } else if (!isColor && !hasAlpha){
+      imageMono = image;
+    } else {
+      std::cerr << "Something went wrong when adding grid map layer form image!" << std::endl;
+      return false;
+    }
+
+    const float mapValueDifference = upperValue - lowerValue;
+    const float maxImageValue = (float)std::numeric_limits<Type_>::max();
+    const Type_ alphaTreshold = (Type_)(alphaThreshold * std::numeric_limits<Type_>::max());
+
+    gridMap.add(layer);
+    grid_map::Matrix& data = gridMap[layer];
+
+    for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
+      const Index index(*iterator);
+
+      // Check for alpha layer.
+      if (hasAlpha) {
+        const Type_ alpha = image.at<cv::Vec<Type_, NChannels_>>(index(0), index(1))[NChannels_ - 1];
+        if (alpha < alphaTreshold) continue;
+      }
+
+      // Compute value.
+      const Type_ imageValue = imageMono.at<Type_>(index(0), index(1));
+      const float mapValue = lowerValue + mapValueDifference * ((float) imageValue / maxImageValue);
+      data(index(0), index(1)) = mapValue;
+    }
+
+    return true;
+  };
+
+  /*!
+   * Creates a cv mat from a grid map layer.
+   * @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, const float lowerValue, const float upperValue,
+                      cv::Mat& image)
+  {
+    // Initialize image.
+    if (gridMap.getSize()(0) > 0 && gridMap.getSize()(1) > 0) {
+      image = cv::Mat::zeros(gridMap.getSize()(0), gridMap.getSize()(1), encoding);
+    } else {
+      std::cerr << "Invalid grid map?" << std::endl;
+      return false;
+    }
+
+    // Get max image value.
+    unsigned int imageMax = (unsigned int)std::numeric_limits<Type_>::max();
+
+    // Clamp outliers.
+    grid_map::GridMap map = gridMap;
+    map.get(layer) = map.get(layer).unaryExpr(grid_map::Clamp<float>(lowerValue, upperValue));
+    const grid_map::Matrix& data = gridMap[layer];
+
+    // Convert to image.
+    bool isColor = false;
+    if (image.channels() >= 3) isColor = true;
+    bool hasAlpha = false;
+    if (image.channels() >= 4) hasAlpha = true;
+
+    for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
+      const Index index(*iterator);
+      if (std::isfinite(data(index(0), index(1)))) {
+        const float& value = data(index(0), index(1));
+        const Type_ imageValue = (Type_) (((value - lowerValue) / (upperValue - lowerValue)) * (float) imageMax);
+        const Index imageIndex(iterator.getUnwrappedIndex());
+        unsigned int channel = 0;
+        image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[channel] = imageValue;
+
+        if (isColor) {
+          image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageValue;
+          image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageValue;
+        }
+        if (hasAlpha) {
+          image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageMax;
+        }
+      }
+    }
+
+    return true;
+  };
+
+};
+
+} /* namespace */

grid_map_cv/include/grid_map_cv/grid_map_cv.hpp

@@ -0,0 +1,12 @@
+/*
+ * grid_map_cv.hpp
+ *
+ *  Created on: Apr 14, 2016
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, Autonomous Systems Lab
+ */
+
+#pragma once
+
+#include <grid_map_core/grid_map_core.hpp>
+#include <grid_map_cv/GridMapCvConverter.hpp>

grid_map_cv/package.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>grid_map_cv</name>
+  <version>1.2.0</version>
+  <description>Conversions between grid maps and OpenCV images.</description>
+  <maintainer email="[email protected]">Péter Fankhauser</maintainer>
+  <license>BSD</license>
+  <url type="website">http://github.com/ethz-asl/grid_map</url>
+  <url type="bugtracker">http://github.com/ethz-asl/grid_map/issues</url>
+  <author email="[email protected]">Péter Fankhauser</author>
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>grid_map_core</depend>
+  <depend>opencv3</depend>
+</package>

grid_map_cv/test/GridMapCvTest.cpp

@@ -0,0 +1,15 @@
+/*
+ * GridMapCvTest.cpp
+ *
+ *  Created on: Apr 14, 2016
+ *      Author: Peter Fankhauser, Martin Wermelinger
+ *	 Institute: ETH Zurich, Autonomous Systems Lab
+ */
+
+#include "grid_map_cv/grid_map_cv.hpp"
+
+// gtest
+#include <gtest/gtest.h>
+
+using namespace std;
+using namespace grid_map;

grid_map_cv/test/test_grid_map_cv.cpp

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

grid_map_demos/CMakeLists.txt

@@ -7,6 +7,7 @@ set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   grid_map_ros
+  grid_map_cv
   grid_map_msgs
   geometry_msgs
   sensor_msgs

grid_map_demos/package.xml

@@ -11,6 +11,7 @@
   <buildtool_depend>catkin</buildtool_depend>
   <depend>roscpp</depend>
   <depend>grid_map_ros</depend>
+  <depend>grid_map_cv</depend>
   <depend>grid_map_loader</depend>
   <depend>grid_map_msgs</depend>
   <depend>grid_map_visualization</depend>

grid_map_demos/src/opencv_demo_node.cpp

@@ -1,6 +1,7 @@
 #include <ros/ros.h>
 #include <grid_map_ros/grid_map_ros.hpp>
-#include <cv_bridge/cv_bridge.h>
+#include <grid_map_cv/grid_map_cv.hpp>
+#include <opencv/cv.h>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv/highgui.h>
 
@@ -11,9 +12,9 @@ int main(int argc, char** argv)
 {
   // Initialize node and publisher.
   init(argc, argv, "opencv_demo");
-  NodeHandle nh("~");
-  Publisher publisher = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
-  const bool useTransparency = false;
+  NodeHandle nodeHandle("~");
+  Publisher publisher = nodeHandle.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
+  const bool useTransparency = true;
 
   // Create grid map.
   GridMap map({"original", "elevation"});
@@ -44,42 +45,40 @@ int main(int argc, char** argv)
   }
 
   // Convert to CV image.
-  cv_bridge::CvImage originalImage;
-  std::string encoding;
+  cv::Mat originalImage;
   if (useTransparency) {
-    encoding = sensor_msgs::image_encodings::RGBA16;
+    // Note: The template parameters have to be set based on your encoding
+    // of the image. For 8-bit images use `unsigned char`.
+    GridMapCvConverter::toImage<unsigned short, 4>(map, "original", CV_16UC4, 0.0, 0.3, originalImage);
   } else {
-    encoding = sensor_msgs::image_encodings::MONO16;
+    GridMapCvConverter::toImage<unsigned short, 1>(map, "original", CV_16UC1, 0.0, 0.3, originalImage);
   }
-  GridMapRosConverter::toCvImage(map, "original", encoding, 0.0, 0.3, originalImage);
 
   // Create OpenCV window.
   cv::namedWindow("OpenCV Demo");
 
   // Work with copy of image in a loop.
-  while (nh.ok()) {
+  while (nodeHandle.ok()) {
 
     // Initialize.
     ros::Time time = ros::Time::now();
     map.setTimestamp(time.toNSec());
-    cv_bridge::CvImage modifiedImage(originalImage.header, originalImage.encoding);
+    cv::Mat modifiedImage;
     int blurRadius = 200 - abs((int)(200.0 * sin(time.toSec())));
     blurRadius = blurRadius - (blurRadius % 2) + 1;
 
     // Apply Gaussian blur.
-    cv::GaussianBlur(originalImage.image, modifiedImage.image, cv::Size(blurRadius, blurRadius), 0.0, 0.0);
+    cv::GaussianBlur(originalImage, modifiedImage, cv::Size(blurRadius, blurRadius), 0.0, 0.0);
 
     // Visualize as image.
-    cv::imshow("OpenCV Demo", modifiedImage.image);
+    cv::imshow("OpenCV Demo", modifiedImage);
     cv::waitKey(40);
 
     // Convert resulting image to a grid map.
-    // Note: The template parameters have to be set based on your encoding
-    // of the image. For MONO8 encoding use `unsigned char`.
     if (useTransparency) {
-      GridMapRosConverter::addLayerFromImage<unsigned short, 4>(modifiedImage.image, "elevation", map, 0.0, 0.3, 0.3);
+      GridMapCvConverter::addLayerFromImage<unsigned short, 4>(modifiedImage, "elevation", map, 0.0, 0.3, 0.3);
     } else {
-      GridMapRosConverter::addLayerFromImage<unsigned short, 1>(modifiedImage.image, "elevation", map, 0.0, 0.3);
+      GridMapCvConverter::addLayerFromImage<unsigned short, 1>(modifiedImage, "elevation", map, 0.0, 0.3);
     }
 
     // Publish grid map.