From 4c1d8d40aa5003ffe35801eaea6fbbe138c5a689 Mon Sep 17 00:00:00 2001 From: Aarushi Mehta Date: Fri, 15 Jul 2022 15:01:45 +0530 Subject: [PATCH] adds overloads for faster toImage and addLayerToImage --- grid_map_cv/CMakeLists.txt | 1 + .../grid_map_cv/GridMapCvConverter.hpp | 118 ++++++++++++++++++ grid_map_cv/test/GridMapCvTest.cpp | 54 ++++++++ grid_map_demos/src/opencv_demo_node.cpp | 7 +- 4 files changed, 176 insertions(+), 4 deletions(-) diff --git a/grid_map_cv/CMakeLists.txt b/grid_map_cv/CMakeLists.txt index d67dbc209..04cdbabb0 100644 --- a/grid_map_cv/CMakeLists.txt +++ b/grid_map_cv/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS find_package(OpenCV REQUIRED COMPONENTS + opencv_core opencv_photo ) diff --git a/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp index bc1d412ce..19183dadc 100644 --- a/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp +++ b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp @@ -12,6 +12,7 @@ // OpenCV #include +#include // STD #include @@ -117,7 +118,73 @@ class GridMapCvConverter return true; } + /*! + * Adds a layer with data from image, uses faster cv conversions for images without transparency + * @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] isAlpha transparent 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[in] 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, bool isAlpha, const float lowerValue, + const float upperValue, const double alphaThreshold) + { + 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; + if (image.channels() >= 4 || isAlpha) { + return addLayerFromImage(image, layer, gridMap, lowerValue, upperValue, alphaThreshold); + } + + cv::Mat imageMono; + if (isColor) { + cv::cvtColor(image, imageMono, CV_BGR2GRAY); + } else if (!isColor){ + imageMono = image; + } else { + std::cerr << "Something went wrong when adding grid map layer from image!" << std::endl; + return false; + } + const float mapValueDifference = upperValue - lowerValue; + + float maxImageValue; + if (std::is_same::value || std::is_same::value) { + maxImageValue = 1.0; + } else if (std::is_same::value || std::is_same::value) { + maxImageValue = (float)std::numeric_limits::max(); + } else { + std::cerr << "This image type is not supported." << std::endl; + return false; + } + + gridMap.add(layer); + grid_map::Matrix& data = gridMap[layer]; + grid_map::Matrix image_data(data.rows(), data.cols()); + cv2eigen(imageMono, image_data); + image_data.array()/maxImageValue * mapValueDifference + lowerValue; + + grid_map::GridMap unbuffered_grid = gridMap; + Index def(0, 0); + unbuffered_grid.setStartIndex(def); + unbuffered_grid.get(layer) = image_data; + grid_map::Index reflected_index(data.rows() - gridMap.getStartIndex()(0), data.cols() - gridMap.getStartIndex()(1)); + unbuffered_grid.setStartIndex(reflected_index); + unbuffered_grid.convertToDefaultStartIndex(); + gridMap.get(layer) = unbuffered_grid[layer]; + return true; + } /*! * Adds a color layer with data from an image. * @param[in] image the image to be added (BGR format). @@ -245,7 +312,58 @@ class GridMapCvConverter return true; } + /*! + * Creates a cv mat from a grid map layer, uses faster cv conversions for images without transparency. + * @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] isAlpha is transparent + * @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, bool isAlpha, 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. + Type_ imageMax; + if (std::is_same::value || std::is_same::value) { + imageMax = 1.0; + } else if (std::is_same::value || std::is_same::value) { + imageMax = (Type_)std::numeric_limits::max(); + } else { + std::cerr << "This image type is not supported." << std::endl; + return false; + } + // Convert to image. + bool isColor = false; + if (image.channels() >= 3) isColor = true; + if (image.channels() >= 4 || isAlpha) { + return toImage(gridMap, layer, encoding, lowerValue, upperValue, image); + } + + // Clamp outliers. + grid_map::GridMap map = gridMap; + map.get(layer) = map.get(layer).unaryExpr(grid_map::Clamp(lowerValue, upperValue)); + + grid_map::GridMap unbuffered_grid = gridMap; + unbuffered_grid.convertToDefaultStartIndex(); + grid_map::Matrix &unbuffered_data = unbuffered_grid[layer]; + eigen2cv(unbuffered_data, image); + return true; + } }; } /* namespace */ diff --git a/grid_map_cv/test/GridMapCvTest.cpp b/grid_map_cv/test/GridMapCvTest.cpp index 996d0b35f..9b2f61c41 100644 --- a/grid_map_cv/test/GridMapCvTest.cpp +++ b/grid_map_cv/test/GridMapCvTest.cpp @@ -61,6 +61,33 @@ TEST(ImageConversion, roundTrip8UC3) EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all()); } +TEST(ImageConversionFast, roundTrip8UC3) +{ + // Create grid map. + GridMap mapIn({"layer"}); + mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.1); + mapIn["layer"].setRandom(); // Sets the layer to random values in [-1.0, 1.0]. + mapIn.move(Position(0.5, -0.2)); + const float minValue = -1.0; + const float maxValue = 1.0; + replaceNan(mapIn.get("layer"), minValue); // When we move `mapIn`, new areas are filled with NaN. As `toImage` does not support NaN, we replace NaN with `minValue` instead. + + // Convert to image. + cv::Mat image; + GridMapCvConverter::toImage(mapIn, "layer", CV_8UC3, false, minValue, maxValue, image); + + // Convert back to grid map. + GridMap mapOut(mapIn); + mapOut["layer"].setConstant(NAN); + GridMapCvConverter::addLayerFromImage(image, "layer", mapOut, false, minValue, maxValue, 0); + + // 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. @@ -115,6 +142,33 @@ TEST(ImageConversion, roundTrip16UC1) EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all()); } +TEST(ImageConversionFast, roundTrip16UC1) +{ + // Create grid map. + GridMap mapIn({"layer"}); + mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.1); + mapIn["layer"].setRandom(); // Sets the layer to random values in [-1.0, 1.0]. + mapIn.move(Position(0.5, -0.2)); + const float minValue = -1.0; + const float maxValue = 1.0; + replaceNan(mapIn.get("layer"), minValue); // When we move `mapIn`, new areas are filled with NaN. As `toImage` does not support NaN, we replace NaN with `minValue` instead. + + // Convert to image. + cv::Mat image; + GridMapCvConverter::toImage(mapIn, "layer", CV_16UC1, false, minValue, maxValue, image); + + // Convert back to grid map. + GridMap mapOut(mapIn); + mapOut["layer"].setConstant(NAN); + GridMapCvConverter::addLayerFromImage(image, "layer", mapOut, false, minValue, maxValue, 0); + + // 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, roundTrip32FC1) { // Create grid map. diff --git a/grid_map_demos/src/opencv_demo_node.cpp b/grid_map_demos/src/opencv_demo_node.cpp index bfdc4a58c..43848bfec 100644 --- a/grid_map_demos/src/opencv_demo_node.cpp +++ b/grid_map_demos/src/opencv_demo_node.cpp @@ -42,15 +42,14 @@ int main(int argc, char** argv) for (grid_map::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); ++iterator) { map.at("original", *iterator) = 0.3; } - // Convert to CV image. cv::Mat originalImage; if (useTransparency) { // 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); + GridMapCvConverter::toImage(map, "original", CV_16UC4, useTransparency, 0.0, 0.3, originalImage); } else { - GridMapCvConverter::toImage(map, "original", CV_16UC1, 0.0, 0.3, originalImage); + GridMapCvConverter::toImage(map, "original", CV_16UC1, useTransparency, 0.0, 0.3, originalImage); } // Create OpenCV window. @@ -77,7 +76,7 @@ int main(int argc, char** argv) if (useTransparency) { GridMapCvConverter::addLayerFromImage(modifiedImage, "elevation", map, 0.0, 0.3, 0.3); } else { - GridMapCvConverter::addLayerFromImage(modifiedImage, "elevation", map, 0.0, 0.3); + GridMapCvConverter::addLayerFromImage(modifiedImage, "elevation", map, useTransparency, 0.0, 0.3, 0.3); } // Publish grid map.