From bc2b78020f1c3b5ac7b8b6093c68aeb2378f54b5 Mon Sep 17 00:00:00 2001 From: ryzhikovas Date: Thu, 7 Oct 2021 11:53:23 +0300 Subject: [PATCH] Minor refactoring Signed-off-by: ryzhikovas --- .../include/nav2_costmap_2d/denoise_layer.hpp | 31 ++--- nav2_costmap_2d/plugins/denoise_layer.cpp | 106 +++++++++--------- .../test/unit/denoise_layer_test.cpp | 16 +-- 3 files changed, 73 insertions(+), 80 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise_layer.hpp index ff7efcf0cd3..50919c10d01 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise_layer.hpp @@ -58,7 +58,6 @@ class DenoiseLayer : public Layer /** * @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. */ @@ -69,14 +68,13 @@ class DenoiseLayer : public Layer /** * @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 + * @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, @@ -85,7 +83,6 @@ class DenoiseLayer : public Layer protected: /** * @brief Initializes the layer on startup - * * This method is called at the end of plugin initialization. * Reads plugin parameters from a config file */ @@ -96,12 +93,12 @@ class DenoiseLayer : public Layer * @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. + * 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 + * 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. + * 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. @@ -110,10 +107,9 @@ class DenoiseLayer : public Layer void denoise(cv::Mat & image) const; /** - * @brief Removes from the image groups of white pixels smaller than minimal_group_size - * + * @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) + * 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 @@ -122,8 +118,7 @@ class DenoiseLayer : public Layer /** * @brief Removes from the image freestanding single white pixels - * - * Works similarly to removeGroups with minimal_group_size = 2, but about 10x faster + * 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 @@ -155,7 +150,6 @@ class DenoiseLayer : public Layer /** * @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 @@ -170,7 +164,6 @@ class DenoiseLayer : public Layer /** * @brief Creates a lookup table for binary thresholding - * * The table size is equal to groups_sizes.size() * Lookup table[i] = OBSTACLE_CELL if groups_sizes[i] >= threshold, * FREE_SPACE in other case @@ -222,9 +215,9 @@ class DenoiseLayer : public Layer private: // Pixels connectivity type. Determines how pixels belonging to the same group can be arranged - size_t minimal_group_size{}; + 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}; + ConnectivityType group_connectivity_type_{ConnectivityType::Way8}; }; template diff --git a/nav2_costmap_2d/plugins/denoise_layer.cpp b/nav2_costmap_2d/plugins/denoise_layer.cpp index 913c0dc4596..6fceda3f0cb 100644 --- a/nav2_costmap_2d/plugins/denoise_layer.cpp +++ b/nav2_costmap_2d/plugins/denoise_layer.cpp @@ -25,47 +25,6 @@ namespace nav2_costmap_2d { static constexpr unsigned char OBSTACLE_CELL = 255; -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; - } - - 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() { @@ -97,17 +56,17 @@ DenoiseLayer::onInitialize() "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; + this->minimal_group_size_ = 1; } else { - this->minimal_group_size = static_cast(minimal_group_size_param); + 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; + this->group_connectivity_type_ = ConnectivityType::Way4; } else if (group_connectivity_type_param == 8) { - this->group_connectivity_type = ConnectivityType::Way8; + this->group_connectivity_type_ = ConnectivityType::Way8; } else { RCLCPP_WARN( logger_, "DenoiseLayer::onInitialize(): param group_connectivity_type: %i." @@ -115,25 +74,66 @@ DenoiseLayer::onInitialize() "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; + this->group_connectivity_type_ = ConnectivityType::Way8; } current_ = true; } +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; + } + + 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::denoise(cv::Mat & image) const { - checkImageType(image, CV_8UC1, "DenoiseLayer::denoise_"); + checkImageType(image, CV_8UC1, "DenoiseLayer::denoise"); if (image.empty()) { return; } - if (minimal_group_size <= 1) { + if (minimal_group_size_ <= 1) { return; // A smaller group cannot exist. No one pixel will be changed } - if (minimal_group_size == 2) { + if (minimal_group_size_ == 2) { // Performs fast filtration based on erosion function removeSinglePixels(image); } else { @@ -157,21 +157,21 @@ DenoiseLayer::removeGroups(cv::Mat & image) const // 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) + 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); + 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); + 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 == OBSTACLE_CELL) { // This check is required for non-binary input image @@ -243,7 +243,7 @@ DenoiseLayer::makeLookupTable(const std::vector & groups_sizes, uint16 void DenoiseLayer::removeSinglePixels(cv::Mat & image) const { - const int shape_code = group_connectivity_type == ConnectivityType::Way4 ? + 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; diff --git a/nav2_costmap_2d/test/unit/denoise_layer_test.cpp b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp index 1d903513b48..d7c4760261f 100644 --- a/nav2_costmap_2d/test/unit/denoise_layer_test.cpp +++ b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp @@ -63,21 +63,21 @@ class DenoiseLayerTester : public ::testing::Test void removeSinglePixels(cv::Mat & image, ConnectivityType connectivity) { - denoise_.group_connectivity_type = 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_.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_.group_connectivity_type_ = connectivity; + denoise_.minimal_group_size_ = minimal_group_size; denoise_.denoise(image); } @@ -102,14 +102,14 @@ class DenoiseLayerTester : public ::testing::Test 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; + 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); + return std::make_tuple(d.enabled_, d.group_connectivity_type_, d.minimal_group_size_); } private: