From e752b1f844ffcf5811fa4df2c85ab8712483a330 Mon Sep 17 00:00:00 2001 From: Peter Fankhauser Date: Thu, 14 Apr 2016 18:55:13 +0200 Subject: [PATCH] Separated OpenCV to grid map conversions to grid_map_cv package. --- grid_map/package.xml | 1 + grid_map_core/CMakeLists.txt | 1 - grid_map_cv/CHANGELOG.rst | 7 + grid_map_cv/CMakeLists.txt | 61 +++++++ .../grid_map_cv/GridMapCvConverter.hpp | 157 ++++++++++++++++++ .../include/grid_map_cv/grid_map_cv.hpp | 12 ++ grid_map_cv/package.xml | 14 ++ grid_map_cv/test/GridMapCvTest.cpp | 15 ++ grid_map_cv/test/test_grid_map_cv.cpp | 18 ++ grid_map_demos/CMakeLists.txt | 1 + grid_map_demos/package.xml | 1 + grid_map_demos/src/opencv_demo_node.cpp | 33 ++-- grid_map_ros/CMakeLists.txt | 3 +- .../grid_map_ros/GridMapRosConverter.hpp | 146 ++++------------ grid_map_ros/package.xml | 1 + grid_map_ros/src/GridMapRosConverter.cpp | 44 +++-- .../{GridMapTest.cpp => GridMapRosTest.cpp} | 30 ++-- 17 files changed, 382 insertions(+), 163 deletions(-) create mode 100644 grid_map_cv/CHANGELOG.rst create mode 100644 grid_map_cv/CMakeLists.txt create mode 100644 grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp create mode 100644 grid_map_cv/include/grid_map_cv/grid_map_cv.hpp create mode 100644 grid_map_cv/package.xml create mode 100644 grid_map_cv/test/GridMapCvTest.cpp create mode 100644 grid_map_cv/test/test_grid_map_cv.cpp rename grid_map_ros/test/{GridMapTest.cpp => GridMapRosTest.cpp} (88%) diff --git a/grid_map/package.xml b/grid_map/package.xml index b1acbfc4a..8e7575e91 100644 --- a/grid_map/package.xml +++ b/grid_map/package.xml @@ -11,6 +11,7 @@ catkin grid_map_core grid_map_ros + grid_map_cv grid_map_msgs grid_map_filters grid_map_visualization diff --git a/grid_map_core/CMakeLists.txt b/grid_map_core/CMakeLists.txt index c76c790f6..689abbea8 100644 --- a/grid_map_core/CMakeLists.txt +++ b/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() - diff --git a/grid_map_cv/CHANGELOG.rst b/grid_map_cv/CHANGELOG.rst new file mode 100644 index 000000000..c97196c7f --- /dev/null +++ b/grid_map_cv/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package grid_map_cv +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Created package. diff --git a/grid_map_cv/CMakeLists.txt b/grid_map_cv/CMakeLists.txt new file mode 100644 index 000000000..b643cf09c --- /dev/null +++ b/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) diff --git a/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp new file mode 100644 index 000000000..ae6d65fa4 --- /dev/null +++ b/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 + +// OpenCV +#include + +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 + 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::max(); + const Type_ alphaTreshold = (Type_)(alphaThreshold * std::numeric_limits::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>(index(0), index(1))[NChannels_ - 1]; + if (alpha < alphaTreshold) continue; + } + + // Compute value. + const Type_ imageValue = imageMono.at(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 + 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::max(); + + // Clamp outliers. + grid_map::GridMap map = gridMap; + map.get(layer) = map.get(layer).unaryExpr(grid_map::Clamp(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>(imageIndex(0), imageIndex(1))[channel] = imageValue; + + if (isColor) { + image.at>(imageIndex(0), imageIndex(1))[++channel] = imageValue; + image.at>(imageIndex(0), imageIndex(1))[++channel] = imageValue; + } + if (hasAlpha) { + image.at>(imageIndex(0), imageIndex(1))[++channel] = imageMax; + } + } + } + + return true; + }; + +}; + +} /* namespace */ diff --git a/grid_map_cv/include/grid_map_cv/grid_map_cv.hpp b/grid_map_cv/include/grid_map_cv/grid_map_cv.hpp new file mode 100644 index 000000000..db22ab82c --- /dev/null +++ b/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 +#include diff --git a/grid_map_cv/package.xml b/grid_map_cv/package.xml new file mode 100644 index 000000000..c57cf2401 --- /dev/null +++ b/grid_map_cv/package.xml @@ -0,0 +1,14 @@ + + + grid_map_cv + 1.2.0 + Conversions between grid maps and OpenCV images. + Péter Fankhauser + BSD + http://github.com/ethz-asl/grid_map + http://github.com/ethz-asl/grid_map/issues + Péter Fankhauser + catkin + grid_map_core + opencv3 + diff --git a/grid_map_cv/test/GridMapCvTest.cpp b/grid_map_cv/test/GridMapCvTest.cpp new file mode 100644 index 000000000..56daf6d2e --- /dev/null +++ b/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 + +using namespace std; +using namespace grid_map; diff --git a/grid_map_cv/test/test_grid_map_cv.cpp b/grid_map_cv/test/test_grid_map_cv.cpp new file mode 100644 index 000000000..e773f4d2c --- /dev/null +++ b/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 + +// 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(); +} diff --git a/grid_map_demos/CMakeLists.txt b/grid_map_demos/CMakeLists.txt index 99eaa8d46..52894f45e 100644 --- a/grid_map_demos/CMakeLists.txt +++ b/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 diff --git a/grid_map_demos/package.xml b/grid_map_demos/package.xml index e7c18c50d..b21b8cde7 100644 --- a/grid_map_demos/package.xml +++ b/grid_map_demos/package.xml @@ -11,6 +11,7 @@ catkin roscpp grid_map_ros + grid_map_cv grid_map_loader grid_map_msgs grid_map_visualization diff --git a/grid_map_demos/src/opencv_demo_node.cpp b/grid_map_demos/src/opencv_demo_node.cpp index 5e00f9aba..f36f25b54 100644 --- a/grid_map_demos/src/opencv_demo_node.cpp +++ b/grid_map_demos/src/opencv_demo_node.cpp @@ -1,6 +1,7 @@ #include #include -#include +#include +#include #include #include @@ -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", 1, true); - const bool useTransparency = false; + NodeHandle nodeHandle("~"); + Publisher publisher = nodeHandle.advertise("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(map, "original", CV_16UC4, 0.0, 0.3, originalImage); } else { - encoding = sensor_msgs::image_encodings::MONO16; + GridMapCvConverter::toImage(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(modifiedImage.image, "elevation", map, 0.0, 0.3, 0.3); + GridMapCvConverter::addLayerFromImage(modifiedImage, "elevation", map, 0.0, 0.3, 0.3); } else { - GridMapRosConverter::addLayerFromImage(modifiedImage.image, "elevation", map, 0.0, 0.3); + GridMapCvConverter::addLayerFromImage(modifiedImage, "elevation", map, 0.0, 0.3); } // Publish grid map. diff --git a/grid_map_ros/CMakeLists.txt b/grid_map_ros/CMakeLists.txt index d701b7dc4..eef0f9f14 100644 --- a/grid_map_ros/CMakeLists.txt +++ b/grid_map_ros/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp grid_map_core grid_map_msgs + grid_map_cv sensor_msgs nav_msgs std_msgs @@ -99,7 +100,7 @@ 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_ros.cpp test/GridMapTest.cpp) +catkin_add_gtest(${PROJECT_NAME}-test test/test_grid_map_ros.cpp test/GridMapRosTest.cpp) if(TARGET ${PROJECT_NAME}-test) target_link_libraries(${PROJECT_NAME}-test grid_map_ros) endif() diff --git a/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp b/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp index 78eb8e80a..f40ec6243 100644 --- a/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp +++ b/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp @@ -156,58 +156,6 @@ class GridMapRosConverter grid_map::GridMap& gridMap, const float lowerValue = 0.0, const float upperValue = 1.0); - template - static bool addLayerFromImage(const cv::Mat& cvMat, const std::string& layer, - grid_map::GridMap& gridMap, const float lowerValue, - const float upperValue, const double alphaThreshold = 0.5) - { - if (gridMap.getSize()(0) != cvMat.rows || gridMap.getSize()(1) != cvMat.cols) { - std::cerr << "Image size does not correspond to grid map size!" << std::endl; - return false; - } - - bool isColor = false; - if (cvMat.channels() >= 3) isColor = true; - bool hasAlpha = false; - if (cvMat.channels() >= 4) hasAlpha = true; - - cv::Mat cvMatMono; - if (isColor && !hasAlpha) { - cv::cvtColor(cvMat, cvMatMono, CV_BGR2GRAY); - } else if (isColor && hasAlpha) { - cv::cvtColor(cvMat, cvMatMono, CV_BGRA2GRAY); - } else if (!isColor && !hasAlpha){ - cvMatMono = cvMat; - } 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::max(); - const Type_ alphaTreshold = (Type_)(alphaThreshold * std::numeric_limits::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 = cvMat.at>(index(0), index(1))[NChannels_ - 1]; - if (alpha < alphaTreshold) continue; - } - - // Compute value. - const Type_ imageValue = cvMatMono.at(index(0), index(1)); - const float mapValue = lowerValue + mapValueDifference * ((float) imageValue / maxImageValue); - data(index(0), index(1)) = mapValue; - } - - return true; - }; - /*! * Adds a color layer with data from an image. * @param[in] image the image to be added. @@ -218,6 +166,33 @@ class GridMapRosConverter static bool addColorLayerFromImage(const sensor_msgs::Image& image, const std::string& layer, grid_map::GridMap& gridMap); + /*! + * Creates an image message 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[out] image the message to be populated. + * @return true if successful, false otherwise. + */ + static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer, + const std::string encoding, sensor_msgs::Image& image); + + /*! + * Creates an image message 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 message to be populated. + * @return true if successful, false otherwise. + */ + static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer, + const std::string encoding, const float lowerValue, const float upperValue, + sensor_msgs::Image& image); + /*! * Creates a cv image from a grid map layer. * This conversion sets the corresponding black and white pixel value to the @@ -225,7 +200,7 @@ class GridMapRosConverter * @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[out] cv image to be populated. + * @param[out] cvImage the to be populated. * @return true if successful, false otherwise. */ static bool toCvImage(const grid_map::GridMap& gridMap, const std::string& layer, @@ -236,74 +211,15 @@ class GridMapRosConverter * @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 value of the layer corresponding to black image pixels. - * @param[in] upperValue value of the layer corresponding to white image pixels. - * @param[out] cv image to be populated. + * @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] cvImage to be populated. * @return true if successful, false otherwise. */ static bool toCvImage(const grid_map::GridMap& gridMap, const std::string& layer, const std::string encoding, const float lowerValue, const float upperValue, cv_bridge::CvImage& cvImage); - /*! - * 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 value of the layer corresponding to black image pixels. - * @param[in] upperValue value of the layer corresponding to white image pixels. - * @param[out] cv mat to be populated. - * @return true if successful, false otherwise. - */ - template - static bool toCvMat(const grid_map::GridMap& gridMap, const std::string& layer, - const int encoding, const float lowerValue, const float upperValue, - cv::Mat& cvMat) - { - // Initialize image. - if (gridMap.getSize()(0) > 0 && gridMap.getSize()(1) > 0) { - cvMat = cv::Mat::zeros(gridMap.getSize()(0), gridMap.getSize()(1), encoding); - } else { - ROS_ERROR("Invalid grid map?"); - return false; - } - - // Get max image value. - unsigned int imageMax = (unsigned int)std::numeric_limits::max(); - - // Clamp outliers. - grid_map::GridMap map = gridMap; - map.get(layer) = map.get(layer).unaryExpr(grid_map::Clamp(lowerValue, upperValue)); - const grid_map::Matrix& data = gridMap[layer]; - - // Convert to image. - bool isColor = false; - if (cvMat.channels() >= 3) isColor = true; - bool hasAlpha = false; - if (cvMat.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; - cvMat.at>(imageIndex(0), imageIndex(1))[channel] = imageValue; - - if (isColor) { - cvMat.at>(imageIndex(0), imageIndex(1))[++channel] = imageValue; - cvMat.at>(imageIndex(0), imageIndex(1))[++channel] = imageValue; - } - if (hasAlpha) { - cvMat.at>(imageIndex(0), imageIndex(1))[++channel] = imageMax; - } - } - } - - return true; - }; - /*! * Saves a grid map into a ROS bag. The timestamp of the grid map * is used as time for storing the message in the ROS bag. The time diff --git a/grid_map_ros/package.xml b/grid_map_ros/package.xml index eeac50fa8..9130c7922 100644 --- a/grid_map_ros/package.xml +++ b/grid_map_ros/package.xml @@ -12,6 +12,7 @@ roscpp grid_map_core grid_map_msgs + grid_map_cv sensor_msgs nav_msgs std_msgs diff --git a/grid_map_ros/src/GridMapRosConverter.cpp b/grid_map_ros/src/GridMapRosConverter.cpp index d930e79f6..f1e2e8a72 100644 --- a/grid_map_ros/src/GridMapRosConverter.cpp +++ b/grid_map_ros/src/GridMapRosConverter.cpp @@ -8,6 +8,7 @@ #include "grid_map_ros/GridMapRosConverter.hpp" #include "grid_map_ros/GridMapMsgHelpers.hpp" +#include // ROS #include @@ -335,17 +336,17 @@ bool GridMapRosConverter::addLayerFromImage(const sensor_msgs::Image& image, const int cvEncoding = cv_bridge::getCvType(image.encoding); switch (cvEncoding) { case CV_8UC1: - return addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); + return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); case CV_8UC3: - return addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); + return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); case CV_8UC4: - return addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); + return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); case CV_16UC1: - return addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); + return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); case CV_16UC3: - return addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); + return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); case CV_16UC4: - return addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); + return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue); default: ROS_ERROR("Expected MONO8, MONO16, RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding."); return false; @@ -384,6 +385,25 @@ bool GridMapRosConverter::addColorLayerFromImage(const sensor_msgs::Image& image return true; } +bool GridMapRosConverter::toImage(const grid_map::GridMap& gridMap, const std::string& layer, + const std::string encoding, sensor_msgs::Image& image) +{ + cv_bridge::CvImage cvImage; + if (!toCvImage(gridMap, layer, encoding, cvImage)) return false; + cvImage.toImageMsg(image); + return true; +} + +bool GridMapRosConverter::toImage(const grid_map::GridMap& gridMap, const std::string& layer, + const std::string encoding, const float lowerValue, + const float upperValue, sensor_msgs::Image& image) +{ + cv_bridge::CvImage cvImage; + if (!toCvImage(gridMap, layer, encoding, lowerValue, upperValue, cvImage)) return false; + cvImage.toImageMsg(image); + return true; +} + bool GridMapRosConverter::toCvImage(const grid_map::GridMap& gridMap, const std::string& layer, const std::string encoding, cv_bridge::CvImage& cvImage) { @@ -403,17 +423,17 @@ bool GridMapRosConverter::toCvImage(const grid_map::GridMap& gridMap, const std: const int cvEncoding = cv_bridge::getCvType(encoding); switch (cvEncoding) { case CV_8UC1: - return toCvMat(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); + return GridMapCvConverter::toImage(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); case CV_8UC3: - return toCvMat(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); + return GridMapCvConverter::toImage(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); case CV_8UC4: - return toCvMat(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); + return GridMapCvConverter::toImage(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); case CV_16UC1: - return toCvMat(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); + return GridMapCvConverter::toImage(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); case CV_16UC3: - return toCvMat(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); + return GridMapCvConverter::toImage(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); case CV_16UC4: - return toCvMat(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); + return GridMapCvConverter::toImage(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); 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/GridMapTest.cpp b/grid_map_ros/test/GridMapRosTest.cpp similarity index 88% rename from grid_map_ros/test/GridMapTest.cpp rename to grid_map_ros/test/GridMapRosTest.cpp index 726a2383e..e7aa1bb98 100644 --- a/grid_map_ros/test/GridMapTest.cpp +++ b/grid_map_ros/test/GridMapRosTest.cpp @@ -1,5 +1,5 @@ /* - * GridMapTest.cpp + * GridMapRosTest.cpp * * Created on: Mar 24, 2015 * Author: Peter Fankhauser, Martin Wermelinger @@ -192,17 +192,15 @@ TEST(ImageConversion, roundTripBGRA8) const float minValue = -1.0; const float maxValue = 1.0; - // Convert to CV image. - cv_bridge::CvImage cvImage; - GridMapRosConverter::toCvImage(gridMapInput, "layer", sensor_msgs::image_encodings::BGRA8, - minValue, maxValue, cvImage); - sensor_msgs::Image rosImage; - cvImage.toImageMsg(rosImage); + // Convert to image message. + sensor_msgs::Image image; + GridMapRosConverter::toImage(gridMapInput, "layer", sensor_msgs::image_encodings::BGRA8, minValue, + maxValue, image); // Convert back to grid map. GridMap gridMapOutput; - GridMapRosConverter::initializeFromImage(rosImage, gridMapInput.getResolution(), gridMapOutput); - GridMapRosConverter::addLayerFromImage(rosImage, "layer", gridMapOutput, minValue, maxValue); + GridMapRosConverter::initializeFromImage(image, gridMapInput.getResolution(), gridMapOutput); + GridMapRosConverter::addLayerFromImage(image, "layer", gridMapOutput, minValue, maxValue); // Check data. const float resolution = (maxValue - minValue) / (float) std::numeric_limits::max(); @@ -218,17 +216,15 @@ TEST(ImageConversion, roundTripMONO16) const float minValue = -1.0; const float maxValue = 1.0; - // Convert to CV image. - cv_bridge::CvImage cvImage; - GridMapRosConverter::toCvImage(gridMapInput, "layer", sensor_msgs::image_encodings::MONO16, - minValue, maxValue, cvImage); - sensor_msgs::Image rosImage; - cvImage.toImageMsg(rosImage); + // Convert to image message. + sensor_msgs::Image image; + GridMapRosConverter::toImage(gridMapInput, "layer", sensor_msgs::image_encodings::MONO16, + minValue, maxValue, image); // Convert back to grid map. GridMap gridMapOutput; - GridMapRosConverter::initializeFromImage(rosImage, gridMapInput.getResolution(), gridMapOutput); - GridMapRosConverter::addLayerFromImage(rosImage, "layer", gridMapOutput, minValue, maxValue); + GridMapRosConverter::initializeFromImage(image, gridMapInput.getResolution(), gridMapOutput); + GridMapRosConverter::addLayerFromImage(image, "layer", gridMapOutput, minValue, maxValue); // Check data. // TODO Why is factor 300 necessary?