Minor refactoring

Signed-off-by: ryzhikovas <ryzhikovas@gmail.com>

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<class SourceElement, class TargetElement, class Converter>

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<int>(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<void *>(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,43 +56,84 @@ 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<size_t>(minimal_group_size_param);
+ this->minimal_group_size_ = static_cast<size_t>(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."
  " 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;
+ 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<int>(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<void *>(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<uint16_t>(
- cv::connectedComponents(binary, labels, static_cast<int>(group_connectivity_type), CV_16U)
+ cv::connectedComponents(binary, labels, static_cast<int>(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<uint16_t> 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<uint8_t> lookup_table = makeLookupTable(groups_sizes, minimal_group_size);
+ const std::vector<uint8_t> lookup_table = makeLookupTable(groups_sizes, minimal_group_size_);
  convert<uint16_t, uint8_t>(
  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<uint16_t> & 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<uint8_t>(1, 1) = 0;

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<bool, DenoiseLayerTester::ConnectivityType, size_t> 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: