From bd6014ed54eed93c5b765d5d1847d9e1af7efeaf Mon Sep 17 00:00:00 2001 From: ryzhikovas Date: Fri, 17 Sep 2021 09:52:13 +0300 Subject: [PATCH] Impl noise filtering layer in the costmap_2d Filtering of noise-induced standalone obstacles (white costmap pixels) or small obstacles groups is implemented. A fast algorithm is used to remove single obstacles. This algorithm performs dilation to determine if each obstacle cell has neighbors. Then replaces single obstacles without neighbors with empty cells. To remove groups of obstacles, segmenting of the costmap image is used. Segments with a size less than the user-specified are deleted. Resolve #2082. Signed-off-by: ryzhikovas --- nav2_costmap_2d/CMakeLists.txt | 4 + nav2_costmap_2d/costmap_plugins.xml | 3 + .../include/nav2_costmap_2d/denoise_layer.hpp | 259 +++++++ nav2_costmap_2d/plugins/denoise_layer.cpp | 274 ++++++++ nav2_costmap_2d/test/unit/CMakeLists.txt | 5 + .../test/unit/denoise_layer_test.cpp | 647 ++++++++++++++++++ 6 files changed, 1192 insertions(+) create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/denoise_layer.hpp create mode 100644 nav2_costmap_2d/plugins/denoise_layer.cpp create mode 100644 nav2_costmap_2d/test/unit/denoise_layer_test.cpp diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 84f86617ab5..2fd20e910e4 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(tf2_ros REQUIRED) find_package(tf2_sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(angles REQUIRED) +find_package(OpenCV REQUIRED) remove_definitions(-DDISABLE_LIBUSB-1.0) find_package(Eigen3 REQUIRED) @@ -32,6 +33,7 @@ nav2_package() include_directories( include ${EIGEN3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ) add_definitions(${EIGEN3_DEFINITIONS}) @@ -85,12 +87,14 @@ add_library(layers SHARED src/observation_buffer.cpp plugins/voxel_layer.cpp plugins/range_sensor_layer.cpp + plugins/denoise_layer.cpp ) ament_target_dependencies(layers ${dependencies} ) target_link_libraries(layers nav2_costmap_2d_core + ${OpenCV_LIBS} ) add_library(filters SHARED diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index d7cb9854933..45c2e7c7f3b 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -15,6 +15,9 @@ A range-sensor (sonar, IR) based obstacle layer for costmap_2d + + Filters noise-induced freestanding obstacles or small obstacles groups + diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise_layer.hpp new file mode 100644 index 00000000000..5a648ff78f8 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise_layer.hpp @@ -0,0 +1,259 @@ +/********************************************************************* + * + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + * + * Author: Andrey Ryzhikov + *********************************************************************/ +#ifndef NAV2_COSTMAP_2D__DENOISE_LAYER_HPP_ +#define NAV2_COSTMAP_2D__DENOISE_LAYER_HPP_ + +#include +#include + +#include "nav2_costmap_2d/layer.hpp" + +class DenoiseLayerTester; // For test some private methods using gtest + +namespace nav2_costmap_2d +{ +/** + * @class DenoiseLayer + * @brief Layer filters noise-induced standalone obstacles (white costmap pixels) or + * small obstacles groups + */ +class DenoiseLayer : public Layer +{ + friend class ::DenoiseLayerTester; // For test some private methods using gtest + +private: + /// Pixels connectivity type (is the way in which pixels in image relate to + /// their neighbors) + enum class ConnectivityType: int + { + /// neighbors pixels are connected horizontally and vertically + Way4 = 4, + /// neighbors pixels are connected horizontally, vertically and diagonally + Way8 = 8 + }; + +public: + DenoiseLayer() = default; + ~DenoiseLayer() = default; + + /** + * @brief Reset this layer + */ + void reset() override; + + /** + * @brief Reports that no clearing operation is required + */ + bool isClearable() override; + + /** + * @brief Reports that no expansion is required + * + * The method is called to ask the plugin: which area of costmap it needs to update. + * A layer is essentially a filter, so it never needs to expand bounds. + */ + void updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, + double * max_x, double * max_y) override; + + /** + * @brief Filters noise-induced obstacles in the selected region of the costmap + * + * The method is called when costmap recalculation is required. + * It updates the costmap within its window bounds. + * @param master_grid The master costmap grid to update + * \param min_x X min map coord of the window to update + * \param min_y Y min map coord of the window to update + * \param max_x X max map coord of the window to update + * \param max_y Y max map coord of the window to update + */ + void updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_x, int min_y, int max_x, int max_y) override; + +protected: + /** + * @brief Initializes the layer on startup + * + * This method is called at the end of plugin initialization. + * Reads plugin parameters from a config file + */ + void onInitialize() override; + +private: + /** + * @brief Removes from the image single obstacles (white pixels) or small obstacles groups + * + * Pixels less than 255 will be interpreted as background (free space), 255 - as obstacles. + * Replaces groups of obstacles smaller than minimal_group_size to free space. + * Groups connectivity type is determined by the connectivity parameter. + * + * If minimal_group_size is 1 or 0, it does nothing + * (all standalone obstacles will be preserved, since it satisfies this condition). + * If minimal_group_size equals 2, performs fast filtering based on the dilation operation. + * Otherwise, it performs a slower segmentation-based operation. + * + * @param image source single channel image with depth CV_8U. + * @throw std::logic_error in case inner logic errors + */ + void denoise(cv::Mat & image) const; + + /** + * @brief Removes from the image groups of white pixels smaller than minimal_group_size + * + * Segments the image into groups of connected pixels + * Replace pixels in groups whose size smaller than minimal_group_size to zero value (background) + * @param image source single channel binary image with depth CV_8U + * @throw std::logic_error in case inner logic errors + * @warning If image.empty() or image.type() != CV_8UC1, the behavior is undefined + */ + void removeGroups(cv::Mat & image) const; + + /** + * @brief Removes from the image freestanding single white pixels + * + * Works similarly to removeGroups with minimal_group_size = 2, but about 10x faster + * @param image source single channel binary image with depth CV_8U + * @throw std::logic_error in case inner logic errors + * @warning If image.empty() or image.type() != CV_8UC1, the behavior is undefined + */ + void removeSinglePixels(cv::Mat & image) const; + + /** + * @brief Calculate truncated histogram of image. + * + * Creates a histogram of image_max bins. + * Bin with index i keep min of (, bin_max). + * This truncation avoids overflow and is acceptable in the problem being solved. + * For example, the image may have 100'000 pixels equal to 0 + * (100'000 > std::numeric_limits). In this case, an overflow will occur when + * calculating a traditional histogram with bins of the uint16_t type. But in this function, + * the bin value will increase to the bin_max value, then stop. Overflow will not happen. + * + * Faster (because simpler) than cv::calcHist + * @param image source single channel image with depth CV_16U + * @param image_max max image pixel value + * @param bin_max max histogram bin value + * @return vector of histogram bins + * @throw std::logic_error if image.type() != CV_16UC1 + * @warning If source contains a pixel with a value, large then image_max, + * the behavior is undefined + */ + std::vector calculateHistogram( + const cv::Mat & image, uint16_t image_max, uint16_t bin_max) const; + + /** + * @brief Convert each pixel of source image to target by lookup table + * + * Perform target[i,j] = table[ source[i,j] ] + * @param source source single channel image with depth CV_16UC1 + * @param target source single channel image with depth CV_8UC1 + * @param table lookup table + * @throw std::logic_error if the source and target of different sizes + * @throw std::logic_error if source.type() != CV_16UC1 or target.type() != CV_8UC1 + * @warning If table.size() not more then max pixel value of source image, + * the behavior is undefined + */ + void applyLookupTable( + const cv::Mat & source, cv::Mat & target, const std::vector & table) const; + + /** + * @brief Creates a lookup table for binary thresholding + * + * The table size is equal to groups_sizes.size() + * Lookup table[i] = filled_cell_value if groups_sizes[i] >= threshold, + * empty_cell_value in other case + */ + std::vector makeLookupTable( + const std::vector & groups_sizes, uint16_t threshold) const; + + /** + * @brief Convert each pixel of source to corresponding pixel of target using a custom function + * + * The source and target must be the same size. + * For calculation of new target value the operation can use source value and + * an optionally current target value. + * This function call operation(source[i, j], target[i, j]) for each pixel + * where target[i, j] is mutable + * @tparam SourceElement type of source pixel + * @tparam TargetElement type of target pixel + * @tparam Converter function object. + * Signature should be equivalent to the following: + * void fn(const SourceElement& src, TargetElement& trg) + * @param source input image with SourceElement-type pixels + * @param target output image with TargetElement-type pixels + * @param operation the binary operation op is applied to pairs of pixels: + * first (const) from source and second (mutable) from target + * @throw std::logic_error if the source and target of different sizes + * @warning If SourceElement/TargetElement type does not match the source/target image type, + * the behavior is undefined + */ + template + void convert(const cv::Mat & source, cv::Mat & target, Converter operation) const; + + /** + * @brief Checks that the image type is as expected + * @param image the image to be checked + * @param cv_type expected image type + * @param error_prefix prefix of the exception text that will be thrown is types are different + * @throw std::logic_error if the image type and cv_type are different + * @warning If error_prefix is nullptr, the behavior is undefined + */ + void checkImageType(const cv::Mat & image, int cv_type, const char * error_prefix) const; + + /** + * @brief Checks that the a.size() == b.size() + * @param error_prefix prefix of the exception text that will be thrown is sizes are different + * @throw std::logic_error if the images sizes are different + * @warning If error_prefix is nullptr, the behavior is undefined + */ + void checkImagesSizesEqual(const cv::Mat & a, const cv::Mat & b, const char * error_prefix) const; + +private: + // Pixels connectivity type. Determines how pixels belonging to the same group can be arranged + size_t minimal_group_size{}; + // The border value of group size. Groups of this and larger size will be kept + ConnectivityType group_connectivity_type{ConnectivityType::Way8}; + const uint8_t empty_cell_value = 0; + const uint8_t filled_cell_value = 255; +}; + +template +void DenoiseLayer::convert(const cv::Mat & source, cv::Mat & target, Converter operation) const +{ + checkImagesSizesEqual(source, target, "DenoiseLayer::convert. The source and target"); + + for (int row = 0; row < source.rows; ++row) { + const auto src_begin = source.ptr(row); + const auto src_end = src_begin + source.cols; + auto trg = target.ptr(row); + + for (auto src = src_begin; src != src_end; ++src, ++trg) { + operation(*src, *trg); + } + } +} + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__DENOISE_LAYER_HPP_ diff --git a/nav2_costmap_2d/plugins/denoise_layer.cpp b/nav2_costmap_2d/plugins/denoise_layer.cpp new file mode 100644 index 00000000000..4615f8d0f9d --- /dev/null +++ b/nav2_costmap_2d/plugins/denoise_layer.cpp @@ -0,0 +1,274 @@ +/********************************************************************* + * + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + * + * Author: Andrey Ryzhikov + *********************************************************************/ +#include "nav2_costmap_2d/denoise_layer.hpp" + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_costmap_2d +{ + +void +DenoiseLayer::reset() +{ + current_ = false; +} + +bool +DenoiseLayer::isClearable() +{ + return false; +} + +void +DenoiseLayer::updateBounds( + double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, + double * /*min_x*/, double * /*min_y*/, + double * /*max_x*/, double * /*max_y*/) {} + +void +DenoiseLayer::updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, int min_x, int min_y, int max_x, int max_y) +{ + if (!enabled_) { + return; + } + std::unique_lock lock(*master_grid.getMutex()); + + unsigned char * master_array = master_grid.getCharMap(); + const int step = static_cast(master_grid.getSizeInCellsX()); + + const cv::Size roi_size {max_x - min_x, max_y - min_y}; + cv::Mat roi_image(roi_size, CV_8UC1, static_cast(master_array + min_x), step); + + try { + denoise(roi_image); + } catch (std::exception & ex) { + RCLCPP_ERROR(logger_, (std::string("Inner error: ") + ex.what()).c_str()); + } + + current_ = true; +} + +void +DenoiseLayer::onInitialize() +{ + // Enable/disable plugin + declareParameter("enabled", rclcpp::ParameterValue(true)); + // Smaller groups should be filtered + declareParameter("minimal_group_size", rclcpp::ParameterValue(2)); + // Pixels connectivity type + declareParameter("group_connectivity_type", rclcpp::ParameterValue(8)); + + // node != nullptr. If node_ is nullptr, declareParameter (...) will throw + const auto node = node_.lock(); + node->get_parameter(name_ + "." + "enabled", enabled_); + + auto getInt = [&](const std::string & parameter_name) { + int param{}; + node->get_parameter(name_ + "." + parameter_name, param); + return param; + }; + + const int minimal_group_size_param = getInt("minimal_group_size"); + + if (minimal_group_size_param <= 1) { + RCLCPP_WARN( + logger_, + "DenoiseLayer::onInitialize(): param minimal_group_size: %i." + " A value of 1 or less means that all map cells will be left as they are.", + minimal_group_size_param); + this->minimal_group_size = 1; + } else { + this->minimal_group_size = static_cast(minimal_group_size_param); + } + + const int group_connectivity_type_param = getInt("group_connectivity_type"); + + if (group_connectivity_type_param == 4) { + this->group_connectivity_type = ConnectivityType::Way4; + } else if (group_connectivity_type_param == 8) { + this->group_connectivity_type = ConnectivityType::Way8; + } else { + RCLCPP_WARN( + logger_, "DenoiseLayer::onInitialize(): param group_connectivity_type: %i." + " Possible values are 4 (neighbors pixels are connected horizontally and vertically) " + "or 8 (neighbors pixels are connected horizontally, vertically and diagonally)." + "The default value 8 will be used", + group_connectivity_type_param); + this->group_connectivity_type = ConnectivityType::Way8; + } + current_ = true; +} + +void +DenoiseLayer::denoise(cv::Mat & image) const +{ + checkImageType(image, CV_8UC1, "DenoiseLayer::denoise_"); + + if (image.empty()) { + return; + } + + if (minimal_group_size <= 1) { + return; // A smaller group cannot exist. No one pixel will be changed + } + + if (minimal_group_size == 2) { + // Performs fast filtration based on erosion function + removeSinglePixels(image); + } else { + // Performs a slower segmentation-based operation + removeGroups(image); + } +} + +void +DenoiseLayer::removeGroups(cv::Mat & image) const +{ + // Creates image binary (the same type and size as image) + // binary[i,j] = filled_cell_value if image[i,j] == filled_cell_value, + // empty_cell_value in other cases + cv::Mat binary; + cv::threshold( + image, binary, filled_cell_value - 1, filled_cell_value, cv::ThresholdTypes::THRESH_BINARY); + + // Creates an image in which each group is labeled with a unique code + cv::Mat labels; + // There is an simple alternative: connectedComponentsWithStats. + // But cv::connectedComponents + calculateHistogram is about 20% faster + const uint16_t groups_count = static_cast( + cv::connectedComponents(binary, labels, static_cast(group_connectivity_type), CV_16U) + ); + + // Calculates the size of each group. + // Group size is equal to the number of pixels with the same label + const auto max_label_value = groups_count - 1; // It's safe. groups_count always non-zero + std::vector groups_sizes = calculateHistogram( + labels, max_label_value, minimal_group_size + 1); + // The group of pixels labeled 0 corresponds to empty map cells. + // Zero bin of the histogram is equal to the number of pixels in this group. + // Because the values of empty map cells should not be changed, we will reset this bin + groups_sizes.front() = 0; // don't change image background value + + // Replace the pixel values from the small groups to background code + const std::vector lookup_table = makeLookupTable(groups_sizes, minimal_group_size); + convert( + labels, image, [&lookup_table, this](uint16_t src, uint8_t & trg) { + if (trg == filled_cell_value) { // This check is required for non-binary input image + trg = lookup_table[src]; + } + }); +} + +void +DenoiseLayer::checkImageType(const cv::Mat & image, int cv_type, const char * error_prefix) const +{ + if (image.type() != cv_type) { + std::string description = std::string(error_prefix) + " expected image type " + + std::to_string(cv_type) + " but got " + std::to_string(image.type()); + throw std::logic_error(description); + } +} + +void +DenoiseLayer::checkImagesSizesEqual( + const cv::Mat & a, const cv::Mat & b, const char * error_prefix) const +{ + if (a.size() != b.size()) { + std::string description = std::string(error_prefix) + " images sizes are different"; + throw std::logic_error(description); + } +} + +std::vector +DenoiseLayer::calculateHistogram(const cv::Mat & image, uint16_t image_max, uint16_t bin_max) const +{ + checkImageType(image, CV_16UC1, "DenoiseLayer::calculateHistogram"); + + if (image.empty()) { + return {}; + } + std::vector histogram(image_max + 1); + + // Increases the bin value corresponding to the pixel by one + auto add_pixel_value = [&histogram, bin_max](uint8_t pixel) { + auto & h = histogram[pixel]; + h = std::min(uint16_t(h + 1), bin_max); + }; + + // Loops through all pixels in the image and updates the histogram at each iteration + for (int row = 0; row < image.rows; ++row) { + auto input_line_begin = image.ptr(row); + auto input_line_end = input_line_begin + image.cols; + std::for_each(input_line_begin, input_line_end, add_pixel_value); + } + return histogram; +} + +std::vector +DenoiseLayer::makeLookupTable(const std::vector & groups_sizes, uint16_t threshold) const +{ + std::vector lookup_table(groups_sizes.size(), empty_cell_value); + + auto transform_fn = [&threshold, this](uint16_t bin_value) { + if (bin_value >= threshold) { + return filled_cell_value; + } + return empty_cell_value; + }; + std::transform(groups_sizes.begin(), groups_sizes.end(), lookup_table.begin(), transform_fn); + return lookup_table; +} + +void +DenoiseLayer::removeSinglePixels(cv::Mat & image) const +{ + const int shape_code = group_connectivity_type == ConnectivityType::Way4 ? + cv::MorphShapes::MORPH_CROSS : cv::MorphShapes::MORPH_RECT; + cv::Mat shape = cv::getStructuringElement(shape_code, {3, 3}); + shape.at(1, 1) = 0; + + cv::Mat max_neighbors_image; + // Building a map of 4 or 8-connected neighbors. + // The pixel of the map is 255 if there is an obstacle nearby + cv::dilate(image, max_neighbors_image, shape); + + convert( + max_neighbors_image, image, [this](uint8_t maxNeighbor, uint8_t & img) { + // img == filled_cell_value is required for non-binary input image + if (maxNeighbor != filled_cell_value && img == filled_cell_value) { + img = empty_cell_value; + } + }); +} + +} // namespace nav2_costmap_2d + +// This is the macro allowing a DenoiseLayer class +// to be registered in order to be dynamically loadable of base type nav2_costmap_2d::Layer. +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::DenoiseLayer, nav2_costmap_2d::Layer) diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 7a1ec6a9098..e839624f683 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -34,3 +34,8 @@ ament_add_gtest(copy_window_test copy_window_test.cpp) target_link_libraries(copy_window_test nav2_costmap_2d_core ) + +ament_add_gtest(denoise_layer_test denoise_layer_test.cpp) +target_link_libraries(denoise_layer_test + nav2_costmap_2d_core layers +) diff --git a/nav2_costmap_2d/test/unit/denoise_layer_test.cpp b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp new file mode 100644 index 00000000000..654536f4092 --- /dev/null +++ b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp @@ -0,0 +1,647 @@ +/********************************************************************* +* +* Licensed to the Apache Software Foundation (ASF) under one +* or more contributor license agreements. See the NOTICE file +* distributed with this work for additional information +* regarding copyright ownership. The ASF licenses this file +* to you under the Apache License, Version 2.0 (the +* "License"); you may not use this file except in compliance +* with the License. You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +* Unless required by applicable law or agreed to in writing, +* software distributed under the License is distributed on an +* "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY +* KIND, either express or implied. See the License for the +* specific language governing permissions and limitations +* under the License. +* +* Author: Andrey Ryzhikov +*********************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include "nav2_costmap_2d/denoise_layer.hpp" + +/** + * @brief nav2_costmap_2d::DenoiseLayer class wrapper + * + * Provides access to DenoiseLayer private methods for testing them in isolation + */ +class DenoiseLayerTester : public ::testing::Test +{ +public: + using ConnectivityType = nav2_costmap_2d::DenoiseLayer::ConnectivityType; + template + void convert(const cv::Mat & source, cv::Mat & target, Converter operation) const + { + denoise_.convert(source, target, operation); + } + + void checkImagesSizesEqual(const cv::Mat & a, const cv::Mat & b, const char * error_prefix) const + { + denoise_.checkImagesSizesEqual(a, b, error_prefix); + } + + void checkImageType(const cv::Mat & image, int cv_type, const char * error_prefix) const + { + denoise_.checkImageType(image, cv_type, error_prefix); + } + + std::vector makeLookupTable( + const std::vector & groups_sizes, uint16_t threshold) const + { + return denoise_.makeLookupTable(groups_sizes, threshold); + } + + std::vector calculateHistogram( + const cv::Mat & image, uint16_t image_max, uint16_t bin_max) const + { + return denoise_.calculateHistogram(image, image_max, bin_max); + } + + void removeSinglePixels(cv::Mat & image, ConnectivityType connectivity) + { + denoise_.group_connectivity_type = connectivity; + denoise_.removeSinglePixels(image); + } + + void removeGroups(cv::Mat & image, ConnectivityType connectivity, size_t minimal_group_size) + { + denoise_.group_connectivity_type = connectivity; + denoise_.minimal_group_size = minimal_group_size; + denoise_.removeGroups(image); + } + + void denoise(cv::Mat & image, ConnectivityType connectivity, size_t minimal_group_size) + { + denoise_.group_connectivity_type = connectivity; + denoise_.minimal_group_size = minimal_group_size; + denoise_.denoise(image); + } + + bool reset() + { + denoise_.current_ = true; + denoise_.reset(); + return denoise_.current_; + } + + static void initialize(nav2_costmap_2d::DenoiseLayer & d) + { + d.onInitialize(); + } + + static bool & touchCurrent(nav2_costmap_2d::DenoiseLayer & d) + { + return d.current_; + } + + static void configure( + nav2_costmap_2d::DenoiseLayer & d, ConnectivityType connectivity, size_t minimal_group_size) + { + d.enabled_ = true; + d.group_connectivity_type = connectivity; + d.minimal_group_size = minimal_group_size; + } + + static std::tuple getParameters( + const nav2_costmap_2d::DenoiseLayer & d) + { + return std::make_tuple(d.enabled_, d.group_connectivity_type, d.minimal_group_size); + } + +private: + nav2_costmap_2d::DenoiseLayer denoise_; +}; + +TEST_F(DenoiseLayerTester, checkEqualImagesSize) { + const cv::Mat a(3, 2, CV_8UC1); + const cv::Mat b(3, 2, CV_16UC1); + + ASSERT_NO_THROW(checkImagesSizesEqual(a, b, "")); +} + +TEST_F(DenoiseLayerTester, checkEqualImagesSizeForDifferentImages) { + const cv::Mat a(3, 2, CV_8UC1); + const cv::Mat b(2, 3, CV_16UC1); + + ASSERT_THROW(checkImagesSizesEqual(a, b, ""), std::logic_error); +} + +TEST_F(DenoiseLayerTester, checkImageType) { + const cv::Mat a(1, 1, CV_16UC1); + + ASSERT_NO_THROW(checkImageType(a, CV_16UC1, "")); +} + +TEST_F(DenoiseLayerTester, checkImageTypeForDifferentTypes) { + const cv::Mat a(1, 1, CV_16UC1); + + ASSERT_THROW(checkImageType(a, CV_32FC1, ""), std::logic_error); +} + +TEST_F(DenoiseLayerTester, convert) { + const cv::Mat source = (cv::Mat_(2, 3) << 1, 2, 3, 4, 5, 6); + cv::Mat target(2, 3, CV_8UC1); + + convert( + source, target, [](uint16_t src, uint8_t & trg) { + trg = src * 2; + }); + + const std::array expected = {2, 4, 6, 8, 10, 12}; + ASSERT_TRUE(std::equal(expected.begin(), expected.end(), target.ptr())); +} + +TEST_F(DenoiseLayerTester, convertDifferentSizes) { + const cv::Mat source(2, 3, CV_8UC1); + cv::Mat target(3, 2, CV_8UC1); + auto do_nothing = [](uint16_t /*src*/, uint8_t & /*trg*/) {}; + + // Extra parentheses need to protect commas in template arguments + ASSERT_THROW((convert(source, target, do_nothing)), std::logic_error); +} + +TEST_F(DenoiseLayerTester, convertEmptyImages) { + const cv::Mat source; + cv::Mat target; + auto shouldn_t_be_called = [](uint16_t /*src*/, uint8_t & /*trg*/) { + throw std::logic_error(""); + }; + + // Extra parentheses need to protect commas in template arguments + ASSERT_NO_THROW((convert(source, target, shouldn_t_be_called))); +} + +TEST_F(DenoiseLayerTester, makeLookupTable) { + const auto result = makeLookupTable({1, 0, 2, 3, 4}, 2); + + ASSERT_EQ(result, std::vector({0, 0, 255, 255, 255})); +} + +TEST_F(DenoiseLayerTester, makeLookupTableFromEmpty) { + const auto result = makeLookupTable({}, 2); + + ASSERT_TRUE(result.empty()); +} + +TEST_F(DenoiseLayerTester, calculateHistogramWithoutTruncation) { + const cv::Mat source = (cv::Mat_(3, 3) << 0, 2, 1, 0, 3, 4, 1, 2, 0); + const size_t max_bin_size = 3; // three zeros + const size_t max_value = 4; + + const auto histogram = calculateHistogram(source, max_value, max_bin_size); + + const std::array expected = {3, 2, 2, 1, 1}; + ASSERT_EQ(histogram.size(), expected.size()); + ASSERT_TRUE(std::equal(expected.begin(), expected.end(), histogram.begin())); +} + +TEST_F(DenoiseLayerTester, calculateHistogramWithTruncation) { + const cv::Mat source = (cv::Mat_(3, 3) << 0, 2, 1, 0, 3, 4, 1, 2, 0); + const size_t max_bin_size = 2; // truncate zero bin + const size_t max_value = 4; + + const auto histogram = calculateHistogram(source, max_value, max_bin_size); + + const std::array expected = {2, 2, 2, 1, 1}; + ASSERT_EQ(histogram.size(), expected.size()); + ASSERT_TRUE(std::equal(expected.begin(), expected.end(), histogram.begin())); +} + +TEST_F(DenoiseLayerTester, calculateHistogramOfEmpty) { + const cv::Mat source(0, 0, CV_16UC1); + const size_t max_bin_size = 1; + const size_t max_value = 0; + + const auto histogram = calculateHistogram(source, max_value, max_bin_size); + ASSERT_TRUE(histogram.empty()); +} + +TEST_F(DenoiseLayerTester, calculateHistogramWrongType) { + const cv::Mat source(1, 1, CV_8UC1, cv::Scalar(0)); + const size_t max_bin_size = 1; + const size_t max_value = 0; + + ASSERT_THROW(calculateHistogram(source, max_value, max_bin_size), std::logic_error); +} + +/** + * @brief Decodes a single channel 8-bit image with values 0 and 255 from a string + * + * Used only for tests. + * String format: '.' - pixel with code 0, 'x' - pixel with code 255. + * The image is always square, i.e. the number of rows is equal to the number of columns + * For example, string + * "x.x" + * ".x." + * "..." + * describes a 3x3 image in which a v-shape is drawn with code 255 + * @throw std::logic_error if the format of the string is incorrect + */ +cv::Mat parseBinaryMatrix(const std::string & s) +{ + const int side_size = static_cast(std::sqrt(s.size())); + + if (size_t(side_size) * side_size != s.size()) { + throw std::logic_error("Test data error: parseBinaryMatrix: Unexpected input string size"); + } + cv::Mat mat(side_size, side_size, CV_8UC1); + + std::transform( + s.begin(), s.end(), mat.ptr(), [](char symbol) -> uint8_t { + if (symbol == '.') { + return 0; + } else if (symbol == 'x') { + return 255; + } else { + throw std::logic_error( + "Test data error: parseBinaryMatrix: Unexpected symbol: " + std::string(1, symbol)); + } + }); + return mat; +} + +/** + * @brief Checks exact match of images + * + * @return true if images a and b have the same type, size, and data. Otherwise false + */ +bool isEqual(const cv::Mat & a, const cv::Mat & b) +{ + return a.size() == b.size() && a.type() == b.type() && cv::countNonZero(a != b) == 0; +} + +TEST_F(DenoiseLayerTester, removeSinglePixels4way) { + const cv::Mat in = parseBinaryMatrix( + "x.x." + "x..x" + ".x.." + "xx.x"); + const cv::Mat exp = parseBinaryMatrix( + "x..." + "x..." + ".x.." + "xx.."); + + cv::Mat out = in.clone(); + removeSinglePixels(out, ConnectivityType::Way4); + + ASSERT_TRUE(isEqual(out, exp)) << + "input:" << std::endl << in << std::endl << + "output:" << std::endl << out << std::endl << + "expected:" << std::endl << exp; +} + +TEST_F(DenoiseLayerTester, removeSinglePixels8way) { + const cv::Mat in = parseBinaryMatrix( + "x.x." + "x..x" + ".x.." + "xx.x"); + const cv::Mat exp = parseBinaryMatrix( + "x.x." + "x..x" + ".x.." + "xx.."); + + cv::Mat out = in.clone(); + removeSinglePixels(out, ConnectivityType::Way8); + + ASSERT_TRUE(isEqual(out, exp)) << + "input:" << std::endl << in << std::endl << + "output:" << std::endl << out << std::endl << + "expected:" << std::endl << exp; +} + +TEST_F(DenoiseLayerTester, removeSinglePixelsFromExtremelySmallImage) { + { + const cv::Mat in = parseBinaryMatrix( + "x"); + const cv::Mat exp = parseBinaryMatrix( + "."); + + cv::Mat out = in.clone(); + removeSinglePixels(out, ConnectivityType::Way8); + + ASSERT_TRUE(isEqual(out, exp)); + } + + { + const cv::Mat in = parseBinaryMatrix( + "x." + ".x"); + const cv::Mat exp = parseBinaryMatrix( + "x." + ".x"); + + cv::Mat out = in.clone(); + removeSinglePixels(out, ConnectivityType::Way8); + + ASSERT_TRUE(isEqual(out, exp)); + } + + { + const cv::Mat in = parseBinaryMatrix( + "x." + ".x"); + const cv::Mat exp = parseBinaryMatrix( + ".." + ".."); + + cv::Mat out = in.clone(); + removeSinglePixels(out, ConnectivityType::Way4); + + ASSERT_TRUE(isEqual(out, exp)); + } +} + +TEST_F(DenoiseLayerTester, removeSinglePixelsFromNonBinary) { + cv::Mat in = cv::Mat(3, 3, CV_8UC1, cv::Scalar(254)); + in.at(1, 1) = 255; + + cv::Mat exp = cv::Mat(3, 3, CV_8UC1, cv::Scalar(254)); + exp.at(1, 1) = 0; + + cv::Mat out = in.clone(); + removeSinglePixels(out, ConnectivityType::Way4); + + ASSERT_TRUE(isEqual(out, exp)) << + "input:" << std::endl << in << std::endl << + "output:" << std::endl << out << std::endl << + "expected:" << std::endl << exp; +} + +TEST_F(DenoiseLayerTester, removePixelsGroup4way) { + const cv::Mat in = parseBinaryMatrix( + ".xx..xx" + "..x.x.." + "x..x..x" + "x......" + "...x.xx" + "xxx..xx" + "....xx."); + const cv::Mat exp = parseBinaryMatrix( + ".xx...." + "..x...." + "......." + "......." + ".....xx" + "xxx..xx" + "....xx."); + + cv::Mat out = in.clone(); + removeGroups(out, ConnectivityType::Way4, 3); + + ASSERT_TRUE(isEqual(out, exp)) << + "input:" << std::endl << in << std::endl << + "output:" << std::endl << out << std::endl << + "expected:" << std::endl << exp; +} + +TEST_F(DenoiseLayerTester, removePixelsGroup8way) { + const cv::Mat in = parseBinaryMatrix( + ".xx..xx" + "..x.x.." + "x..x..x" + "x......" + "...x.xx" + "xxx..xx" + "....xx."); + const cv::Mat exp = parseBinaryMatrix( + ".xx..xx" + "..x.x.." + "...x..." + "......." + "...x.xx" + "xxx..xx" + "....xx."); + + cv::Mat out = in.clone(); + removeGroups(out, ConnectivityType::Way8, 3); + + ASSERT_TRUE(isEqual(out, exp)) << + "input:" << std::endl << in << std::endl << + "output:" << std::endl << out << std::endl << + "expected:" << std::endl << exp; +} + +TEST_F(DenoiseLayerTester, removePixelsGroupFromExtremelySmallImage) { + { + const cv::Mat in = parseBinaryMatrix( + "x"); + const cv::Mat exp = parseBinaryMatrix( + "."); + + cv::Mat out = in.clone(); + removeGroups(out, ConnectivityType::Way8, 3); + + ASSERT_TRUE(isEqual(out, exp)); + } + + { + const cv::Mat in = parseBinaryMatrix( + "x." + ".x"); + const cv::Mat exp = parseBinaryMatrix( + ".." + ".."); + + cv::Mat out = in.clone(); + removeGroups(out, ConnectivityType::Way8, 3); + + ASSERT_TRUE(isEqual(out, exp)); + } +} + +TEST_F(DenoiseLayerTester, removePixelsGroupFromNonBinary) { + cv::Mat in = cv::Mat(3, 3, CV_8UC1, cv::Scalar(254)); + in.at(1, 1) = 255; + + cv::Mat exp = cv::Mat(3, 3, CV_8UC1, cv::Scalar(254)); + exp.at(1, 1) = 0; + + cv::Mat out = in.clone(); + removeGroups(out, ConnectivityType::Way4, 2); + + ASSERT_TRUE(isEqual(out, exp)) << + "input:" << std::endl << in << std::endl << + "output:" << std::endl << out << std::endl << + "expected:" << std::endl << exp; +} + +TEST_F(DenoiseLayerTester, denoiseSingles) { + const cv::Mat in = parseBinaryMatrix( + "xx." + "..." + "..x"); + const cv::Mat exp = parseBinaryMatrix( + "xx." + "..." + "..."); + + cv::Mat out = in.clone(); + denoise(out, ConnectivityType::Way4, 2); + + ASSERT_TRUE(isEqual(out, exp)) << + "input:" << std::endl << in << std::endl << + "output:" << std::endl << out << std::endl << + "expected:" << std::endl << exp; +} + +TEST_F(DenoiseLayerTester, denoiseGroups) { + const cv::Mat in = parseBinaryMatrix( + "xx." + "x.x" + "..x"); + const cv::Mat exp = parseBinaryMatrix( + "xx." + "x.." + "..."); + + cv::Mat out = in.clone(); + denoise(out, ConnectivityType::Way4, 3); + + ASSERT_TRUE(isEqual(out, exp)) << + "input:" << std::endl << in << std::endl << + "output:" << std::endl << out << std::endl << + "expected:" << std::endl << exp; +} + +TEST_F(DenoiseLayerTester, denoiseWrongImageType) { + cv::Mat in(1, 1, CV_16UC1); + + ASSERT_THROW(denoise(in, ConnectivityType::Way4, 2), std::logic_error); +} + +TEST_F(DenoiseLayerTester, denoiseEmpty) { + cv::Mat in(0, 0, CV_8UC1); + + ASSERT_NO_THROW(denoise(in, ConnectivityType::Way4, 2)); +} + +TEST_F(DenoiseLayerTester, denoiseNothing) { + cv::Mat in(1, 1, CV_8UC1); + + ASSERT_NO_THROW(denoise(in, ConnectivityType::Way4, 1)); +} + +TEST_F(DenoiseLayerTester, constructorAndDestructor) { + ASSERT_NO_THROW( + []() { + nav2_costmap_2d::DenoiseLayer layer; + }); +} + +TEST_F(DenoiseLayerTester, reset) { + ASSERT_FALSE(reset()); +} + +TEST_F(DenoiseLayerTester, isClearable) { + nav2_costmap_2d::DenoiseLayer layer; + + ASSERT_FALSE(layer.isClearable()); +} + +TEST_F(DenoiseLayerTester, updateBounds) { + nav2_costmap_2d::DenoiseLayer layer; + + const std::array region = {1., 2., 3., 4.}; + auto r = region; + + ASSERT_NO_THROW(layer.updateBounds(0., 0., 0., &r[0], &r[1], &r[2], &r[3])); + ASSERT_EQ(r, region); +} + +TEST_F(DenoiseLayerTester, updateCostsIfDisabled) { + nav2_costmap_2d::DenoiseLayer layer; + nav2_costmap_2d::Costmap2D costmap(1, 1, 1., 0., 0., 255); + + layer.updateCosts(costmap, 0, 0, 1, 1); + + ASSERT_EQ(costmap.getCost(0), 255); +} + +TEST_F(DenoiseLayerTester, updateCosts) { + nav2_costmap_2d::DenoiseLayer layer; + nav2_costmap_2d::Costmap2D costmap(1, 1, 1., 0., 0., 255); + DenoiseLayerTester::configure(layer, DenoiseLayerTester::ConnectivityType::Way4, 2); + + layer.updateCosts(costmap, 0, 0, 1, 1); + + ASSERT_EQ(costmap.getCost(0), 0); +} + +// Copy paste from declare_parameter_test.cpp +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture rcl_cpp_fixture; + +std::shared_ptr constructLayer( + std::shared_ptr node = + std::make_shared("test_node")) +{ + auto tf = std::make_shared(node->get_clock()); + auto layers = std::make_shared("frame", false, false); + + auto deleter = [node, tf, layers](nav2_costmap_2d::DenoiseLayer * ptr) + { + delete ptr; + }; + auto layer = std::shared_ptr( + new nav2_costmap_2d::DenoiseLayer, deleter); + layer->initialize(layers.get(), "test_layer", tf.get(), node, nullptr, nullptr); + return layer; +} + +TEST_F(DenoiseLayerTester, initializeDefault) { + auto layer = constructLayer(); + + DenoiseLayerTester::initialize(*layer); + + ASSERT_EQ( + DenoiseLayerTester::getParameters(*layer), + std::make_tuple(true, DenoiseLayerTester::ConnectivityType::Way8, 2)); +} + +TEST_F(DenoiseLayerTester, initializeCustom) { + auto node = std::make_shared("test_node"); + auto layer = constructLayer(node); + node->set_parameter( + rclcpp::Parameter(layer->getFullName("minimal_group_size"), rclcpp::ParameterValue(5))); + node->set_parameter( + rclcpp::Parameter(layer->getFullName("group_connectivity_type"), rclcpp::ParameterValue(4))); + + DenoiseLayerTester::initialize(*layer); + + ASSERT_EQ( + DenoiseLayerTester::getParameters(*layer), + std::make_tuple(true, DenoiseLayerTester::ConnectivityType::Way4, 5)); +} + +TEST_F(DenoiseLayerTester, initializeInvalid) { + auto node = std::make_shared("test_node"); + auto layer = constructLayer(node); + node->set_parameter( + rclcpp::Parameter(layer->getFullName("minimal_group_size"), rclcpp::ParameterValue(-1))); + node->set_parameter( + rclcpp::Parameter(layer->getFullName("group_connectivity_type"), rclcpp::ParameterValue(3))); + + DenoiseLayerTester::initialize(*layer); + + ASSERT_EQ( + DenoiseLayerTester::getParameters(*layer), + std::make_tuple(true, DenoiseLayerTester::ConnectivityType::Way8, 1)); +}