diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index 683cb04fdd..f039cf5d42 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -87,6 +87,7 @@ 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}
diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml
index e5867d5108..6748cd5fca 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/image.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp
new file mode 100644
index 0000000000..db7ae8fce8
--- /dev/null
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp
@@ -0,0 +1,207 @@
+// Copyright (c) 2023 Andrey Ryzhikov
+//
+// Licensed 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.
+
+#ifndef NAV2_COSTMAP_2D__DENOISE__IMAGE_HPP_
+#define NAV2_COSTMAP_2D__DENOISE__IMAGE_HPP_
+
+#include
+#include
+
+namespace nav2_costmap_2d
+{
+
+/**
+ * @brief Image with pixels of type T
+ * Сan own data, be a wrapper over some memory buffer, or refer to a fragment of another image
+ * Pixels of one row are stored continuity. But rows continuity is not guaranteed.
+ * The distance (number of elements of type T) from row(i) to row(i + 1) is equal to step()
+ * @tparam T type of pixel
+ */
+template
+class Image
+{
+public:
+ /// @brief Create empty image
+ Image() = default;
+
+ /**
+ * @brief Create image referencing to a third-party buffer
+ * @param rows number of image rows
+ * @param columns number of image columns
+ * @param data existing memory buffer with size at least rows * columns
+ * @param step offset from row(i) to row(i + 1) in memory buffer (number of elements of type T).
+ * offset = columns if rows are stored continuity
+ */
+ Image(size_t rows, size_t columns, T * data, size_t step);
+
+ /**
+ * @brief Create image referencing to the other
+ * Share image data between new and old object.
+ * Changing data in a new object will affect the given one and vice versa
+ */
+ Image(Image & other);
+
+ /**
+ * @brief Create image from the other (move constructor)
+ */
+ Image(Image && other) noexcept;
+
+ /// @return number of image rows
+ size_t rows() const {return rows_;}
+
+ /// @return number of image columns
+ size_t columns() const {return columns_;}
+
+ /// @return true if image empty
+ bool empty() const {return rows_ == 0 || columns_ == 0;}
+
+ /// @return offset (number of elements of type T) from row(i) to row(i + 1)
+ size_t step() const {return step_;}
+
+ /**
+ * @return pointer to first pixel of row
+ * @warning If row >= rows(), the behavior is undefined
+ */
+ T * row(size_t row);
+
+ /// @overload
+ const T * row(size_t row) const;
+
+ /**
+ * @brief Read (and modify, if need) each pixel sequentially
+ * @tparam Functor function object.
+ * Signature should be equivalent to the following:
+ * void fn(T& pixel) or void fn(const T& pixel)
+ * @param fn a function that will be applied to each pixel in the image. Can modify image data
+ */
+ template
+ void forEach(Functor && fn);
+
+ /**
+ * @brief Read each pixel sequentially
+ * @tparam Functor function object.
+ * Signature should be equivalent to the following:
+ * void fn(const T& pixel)
+ * @param fn a function that will be applied to each pixel in the image
+ */
+ template
+ void forEach(Functor && fn) const;
+ /**
+ * @brief Convert each pixel 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(this[i, j], target[i, j]) for each pixel
+ * where target[i, j] is mutable
+ * @tparam TargetElement type of target pixel
+ * @tparam Converter function object.
+ * Signature should be equivalent to the following:
+ * void fn(const T& src, TargetElement& trg)
+ * @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
+ */
+ template
+ void convert(Image & target, Converter && converter) const;
+
+private:
+ T * data_start_{};
+ size_t rows_{};
+ size_t columns_{};
+ size_t step_{};
+};
+
+template
+Image::Image(size_t rows, size_t columns, T * data, size_t step)
+: rows_{rows}, columns_{columns}, step_{step}
+{
+ data_start_ = data;
+}
+
+template
+Image::Image(Image & other)
+: data_start_{other.data_start_},
+ rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {}
+
+template
+Image::Image(Image && other) noexcept
+: data_start_{other.data_start_},
+ rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {}
+
+template
+T * Image::row(size_t row)
+{
+ return const_cast( static_cast &>(*this).row(row) );
+}
+
+template
+const T * Image::row(size_t row) const
+{
+ return data_start_ + row * step_;
+}
+
+template
+template
+void Image::forEach(Functor && fn)
+{
+ static_cast &>(*this).forEach(
+ [&](const T & pixel) {
+ fn(const_cast(pixel));
+ });
+}
+
+template
+template
+void Image::forEach(Functor && fn) const
+{
+ const T * rowPtr = row(0);
+
+ for (size_t row = 0; row < rows(); ++row) {
+ const T * rowEnd = rowPtr + columns();
+
+ for (const T * pixel = rowPtr; pixel != rowEnd; ++pixel) {
+ fn(*pixel);
+ }
+ rowPtr += step();
+ }
+}
+
+template
+template
+void Image::convert(Image & target, Converter && converter) const
+{
+ if (rows() != target.rows() || columns() != target.columns()) {
+ throw std::logic_error("Image::convert. The source and target images size are different");
+ }
+ const T * source_row = row(0);
+ TargetElement * target_row = target.row(0);
+
+ for (size_t row = 0; row < rows(); ++row) {
+ const T * rowInEnd = source_row + columns();
+ const T * src = source_row;
+ TargetElement * trg = target_row;
+
+ for (; src != rowInEnd; ++src, ++trg) {
+ converter(*src, *trg);
+ }
+ source_row += step();
+ target_row += target.step();
+ }
+}
+
+} // namespace nav2_costmap_2d
+
+#endif // NAV2_COSTMAP_2D__DENOISE__IMAGE_HPP_
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp
new file mode 100644
index 0000000000..ee79894847
--- /dev/null
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp
@@ -0,0 +1,1051 @@
+// Copyright (c) 2023 Andrey Ryzhikov
+//
+// Licensed 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.
+
+#ifndef NAV2_COSTMAP_2D__DENOISE__IMAGE_PROCESSING_HPP_
+#define NAV2_COSTMAP_2D__DENOISE__IMAGE_PROCESSING_HPP_
+
+#include "image.hpp"
+#include
+#include
+#include
+#include
+#include
+
+namespace nav2_costmap_2d
+{
+
+/**
+ * @enum nav2_costmap_2d::ConnectivityType
+ * @brief Describes the type of pixel connectivity (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
+};
+
+/**
+ * @brief A memory buffer that can grow to an upper-bounded capacity
+ */
+class MemoryBuffer
+{
+public:
+ /// @brief Free memory allocated for the buffer
+ inline ~MemoryBuffer() {reset();}
+ /**
+ * @brief Return a pointer to an uninitialized array of count elements
+ * Delete the old block of memory and allocates a new one if the size of the old is too small.
+ * The returned pointer is valid until the next call to get() or destructor.
+ * @tparam T type of element
+ * @param count number of elements
+ * @throw std::bad_alloc or any other exception thrown by allocator
+ */
+ template
+ T * get(std::size_t count);
+
+private:
+ inline void reset();
+ inline void allocate(size_t bytes);
+
+private:
+ void * data_{};
+ size_t size_{};
+};
+
+// forward declarations
+namespace imgproc_impl
+{
+template
+class EquivalenceLabelTrees;
+
+template
+void morphologyOperation(
+ const Image & input, Image & output,
+ const Image & shape, AggregateFn aggregate);
+
+using ShapeBuffer3x3 = std::array;
+inline Image createShape(ShapeBuffer3x3 & buffer, ConnectivityType connectivity);
+} // namespace imgproc_impl
+
+/**
+ * @brief Perform morphological dilation
+ * @tparam Max function object
+ * @param input input image
+ * @param output output image
+ * @param connectivity selector for selecting structuring element (Way4-> cross, Way8-> rect)
+ * @param max_function takes as input std::initializer_list with three elements.
+ * Returns the greatest value in list
+ */
+template
+inline void dilate(
+ const Image & input, Image & output,
+ ConnectivityType connectivity, Max && max_function)
+{
+ using namespace imgproc_impl;
+ ShapeBuffer3x3 shape_buffer;
+ Image shape = createShape(shape_buffer, connectivity);
+ morphologyOperation(input, output, shape, max_function);
+}
+
+/**
+* @brief Compute the connected components labeled image of binary image
+* Implements the SAUF algorithm
+* (Two Strategies to Speed up Connected Component Labeling Algorithms
+* Kesheng Wu, Ekow Otoo, Kenji Suzuki).
+* @tparam connectivity pixels connectivity type
+* @tparam Label integer type of label
+* @tparam IsBg functor with signature bool (uint8_t)
+* @param image input image
+* @param buffer memory block that will be used to store the result (labeled image)
+* and the internal buffer for labels trees
+* @param label_trees union-find data structure
+* @param is_background returns true if the passed pixel value is background
+* @throw LabelOverflow if all possible values of the Label type are used and
+* it is impossible to create a new unique
+* @return pair(labeled image, total number of labels)
+* Labeled image has the same size as image. Label 0 represents the background label,
+* labels [1, - 1] - separate components.
+* Total number of labels == 0 for empty image.
+* In other cases, label 0 is always counted,
+* even if there is no background in the image.
+* For example, for an image of one background pixel, the total number of labels == 2.
+* Two labels (0, 1) have been counted, although label 0 is not used)
+*/
+template
+std::pair, Label> connectedComponents(
+ const Image & image, MemoryBuffer & buffer,
+ imgproc_impl::EquivalenceLabelTrees & label_trees,
+ IsBg && is_background);
+
+// Implementation
+
+template
+T * MemoryBuffer::get(std::size_t count)
+{
+ // Check the memory allocated by ::operator new can be used to store the type T
+ static_assert(
+ alignof(std::max_align_t) >= alignof(T),
+ "T alignment is more than the fundamental alignment of the platform");
+
+ const size_t required_bytes = sizeof(T) * count;
+
+ if (size_ < required_bytes) {
+ allocate(required_bytes);
+ }
+ return static_cast(data_);
+}
+
+void MemoryBuffer::reset()
+{
+ ::operator delete(data_);
+ size_ = 0;
+}
+
+void MemoryBuffer::allocate(size_t bytes)
+{
+ reset();
+ data_ = ::operator new(bytes);
+ size_ = bytes;
+}
+
+namespace imgproc_impl
+{
+
+/**
+ * @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 with pixel type uint16_t 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.
+ * @tparam T image pixel type
+ * @param image source image
+ * @param image_max max image pixel value
+ * @param bin_max max histogram bin value
+ * @return vector of histogram bins
+ * @warning If source contains a pixel with a value, large then image_max,
+ * the behavior is undefined
+*/
+template
+std::vector
+histogram(const Image & image, T image_max, Bin bin_max)
+{
+ if (image.empty()) {
+ return {};
+ }
+ std::vector histogram(size_t(image_max) + 1);
+
+ // Increases the bin value corresponding to the pixel by one
+ auto add_pixel_value = [&histogram, bin_max](T pixel) {
+ auto & h = histogram[pixel];
+ h = std::min(Bin(h + 1), bin_max);
+ };
+
+ image.forEach(add_pixel_value);
+ return histogram;
+}
+
+namespace out_of_bounds_policy
+{
+
+/**
+ * @brief Boundary case object stub. Used as parameter of class Window.
+ * Dereferences a pointer to a pixel without any checks
+ * @tparam T image pixel type
+ * @sa ReplaceToZero
+ */
+template
+struct DoNothing
+{
+ T & up(T * v) const {return *v;}
+ T & down(T * v) const {return *v;}
+};
+
+/**
+ * @brief Boundary case object. Used as parameter of class Window.
+ * Dereferences a pointer to a existing pixel. If the pixel is out of bounds, it returns a ref to 0.
+ * @tparam T image pixel type
+ * @sa DoNothing
+ */
+template
+class ReplaceToZero
+{
+public:
+ /**
+ * @brief Create an object that will replace pointers outside the specified range
+ * @param up_row_start pointer to the first pixel of up row. Can be nullptr.
+ * @param down_row_start pointer to the first pixel of down row
+ * @param columns number of pixels in both rows
+ */
+ ReplaceToZero(const T * up_row_start, const T * down_row_start, size_t columns)
+ : up_row_start_{up_row_start}, up_row_end_{up_row_start + columns},
+ down_row_start_{down_row_start}, down_row_end_{down_row_start + columns} {}
+
+ /**
+ * @brief Return ref to pixel or to zero value if up_row_start_ is nullptr or the pointer is out of bounds
+ * @param v pointer to pixel
+ */
+ T & up(T * v)
+ {
+ if (up_row_start_ == nullptr) {
+ return zero_;
+ }
+ return replaceOutOfBounds(v, up_row_start_, up_row_end_);
+ }
+
+ /**
+ * @brief Return ref to pixel or to zero value if the pointer is out of bounds
+ * @param v pointer to pixel
+ */
+ T & down(T * v)
+ {
+ return replaceOutOfBounds(v, down_row_start_, down_row_end_);
+ }
+
+private:
+ /**
+ * @brief Replaces an out-of-bounds pointer with a pointer to 0
+ * @return a dereferenced pointer or a reference to 0 if the pointer is out of range
+ */
+ T & replaceOutOfBounds(T * v, const T * begin, const T * end)
+ {
+ if (v < begin || v >= end) {
+ return zero_;
+ }
+ return *v;
+ }
+
+ const T * up_row_start_;
+ const T * up_row_end_;
+ const T * down_row_start_;
+ const T * down_row_end_;
+ T zero_{};
+};
+
+} // namespace out_of_bounds_policy
+
+/**
+ * @brief Forward scan mask sliding window
+ * Provides an interface for access to neighborhood of the current pixel
+ * (includes three neighbors of the top row, the pixel to the left and the current one).
+ * In the illustration below, the current pixel is e.
+ * |a|b|c|
+ * |d|e| |
+ * | | | |
+ * @tparam T image pixel type
+ * @tparam Border optional check of access to pixels outside the image boundary (DoNothing or ReplaceToZero)
+ */
+template class Border>
+class Window
+{
+public:
+ /**
+ * Construct mask window
+ * @param up_row pointer to the pixel above the current one (a)
+ * @param down_row pointer to the current pixel (e)
+ * @param border boundary case object
+ */
+ inline Window(T * up_row, T * down_row, Border border = {})
+ : up_row_{up_row}, down_row_{down_row}, border_{border} {}
+
+ inline T & a() {return border_.up(up_row_ - 1);}
+ inline T & b() {return border_.up(up_row_);}
+ inline T & c() {return border_.up(up_row_ + 1);}
+ inline T & d() {return border_.down(down_row_ - 1);}
+ inline T & e() {return *down_row_;}
+ inline const T * anchor() const {return down_row_;}
+
+ /// @brief Shifts the window to the right
+ inline void next()
+ {
+ ++up_row_;
+ ++down_row_;
+ }
+
+private:
+ T * up_row_;
+ T * down_row_;
+ Border border_;
+};
+
+/// @brief Discards const
+template
+T * dropConst(const T * ptr)
+{
+ return const_cast(ptr);
+}
+
+/**
+ * @brief Create a sliding window with boundary case object
+ * @tparam T image pixel type
+ * @param up_row pointer to the row above the current one
+ * @param down_row pointer to the current row
+ * @param columns number of pixels in both rows
+ * @param offset offset from rows start to current window pixel
+ * @return forward scan mask sliding window
+ * @warning Breaks the constant guarantees.
+ * Always returns a non-constant window, using which you can potentially change data in up_row, down_row.
+ * This could have been avoided by creating a ConstWindow similar to Window.
+ * But probably code bloat is the bigger evil
+ */
+template
+Window makeSafeWindow(
+ const T * up_row, const T * down_row, size_t columns, size_t offset = 0)
+{
+ return {
+ dropConst(up_row) + offset, dropConst(down_row) + offset,
+ out_of_bounds_policy::ReplaceToZero{up_row, down_row, columns}
+ };
+}
+
+/**
+ * @brief Create a sliding window without any out of borders checks
+ * @tparam T image pixel type
+ * @param up_row pointer to the row above the current one
+ * @param down_row pointer to the current row
+ * @return forward scan mask sliding window
+ * @warning Breaks the constant guarantees. See warning in makeSafeWindow
+ */
+template
+Window makeUnsafeWindow(const T * up_row, const T * down_row)
+{
+ return {dropConst(up_row), dropConst(down_row)};
+}
+
+struct EquivalenceLabelTreesBase
+{
+ virtual ~EquivalenceLabelTreesBase() = default;
+};
+
+struct LabelOverflow : public std::runtime_error
+{
+ LabelOverflow(const std::string & message)
+ : std::runtime_error(message) {}
+};
+
+/**
+ * @brief Union-find data structure
+ * Implementation of union-find data structure, described in reference article.
+ * Store rooted trees, where each node of a tree is a provisional label and each edge represents an
+ * equivalence between two labels
+ * @tparam Label integer type of label
+ */
+template
+class EquivalenceLabelTrees : public EquivalenceLabelTreesBase
+{
+public:
+ /**
+ * @brief Reset labels tree to initial state
+ * @param rows number of image rows
+ * @param columns number of image columns
+ * @param connectivity pixels connectivity type
+ */
+ void reset(const size_t rows, const size_t columns, ConnectivityType connectivity)
+ {
+ // Trying to reserve memory with a margin
+ const size_t max_labels_count = maxLabels(rows, columns, connectivity);
+ // Number of labels cannot exceed std::numeric_limits::max()
+ labels_size_ = static_cast(
+ std::min(max_labels_count, size_t(std::numeric_limits::max()))
+ );
+
+ try {
+ labels_.reserve(labels_size_);
+ } catch (...) {
+ // ignore any exception
+ // perhaps the entire requested amount of memory will not be required
+ }
+
+ // Label 0 is reserved for the background pixels, i.e. labels[0] is always 0
+ labels_ = {0};
+ next_free_ = 1;
+ }
+
+ /**
+ * @brief Creates new next unused label and returns it back
+ * @throw LabelOverflow if all possible labels already used
+ * @return label
+ */
+ Label makeLabel()
+ {
+ // Check the next_free_ counter does not overflow.
+ if (next_free_ == labels_size_) {
+ throw LabelOverflow("EquivalenceLabelTrees: Can't create new label");
+ }
+ labels_.push_back(next_free_);
+ return next_free_++;
+ }
+
+ /**
+ * @brief Unite the two trees containing nodes i and j and return the new root
+ * See union function in reference article
+ * @param i tree node
+ * @param j tree node
+ * @return root of joined tree
+ */
+ Label unionTrees(Label i, Label j)
+ {
+ Label root = findRoot(i);
+
+ if (i != j) {
+ Label root_j = findRoot(j);
+ root = std::min(root, root_j);
+ setRoot(j, root);
+ }
+ setRoot(i, root);
+ return root;
+ }
+
+ /**
+ * @brief Convert union-find trees to labels lookup table
+ * @return pair(labels lookup table, unique labels count)
+ * @warning This method invalidate EquivalenceLabelTrees inner state
+ * @warning Returns an internal buffer that will be invalidated
+ * on subsequent calls to the methods of EquivalenceLabelTrees
+ */
+ const std::vector & getLabels()
+ {
+ Label k = 1;
+ for (Label i = 1; i < next_free_; ++i) {
+
+ if (labels_[i] < i) {
+ labels_[i] = labels_[labels_[i]];
+ } else {
+ labels_[i] = k;
+ ++k;
+ }
+ }
+ labels_.resize(k);
+ return labels_;
+ }
+
+private:
+ /**
+ * @brief Defines the upper bound for the number of labels
+ * @param rows number of image rows
+ * @param columns number of image columns
+ * @param connectivity pixels connectivity type
+ * @return max labels count
+ */
+ static size_t maxLabels(const size_t rows, const size_t columns, ConnectivityType connectivity)
+ {
+ size_t max_labels{};
+
+ if (connectivity == ConnectivityType::Way4) {
+ /* The maximum of individual components will be reached in the chessboard image,
+ * where the white cells correspond to obstacle pixels */
+ max_labels = (rows * columns) / 2 + 1;
+ } else {
+ /* The maximum of individual components will be reached in image like this:
+ * x.x.x.x~
+ * .......~
+ * x.x.x.x~
+ * .......~
+ * x.x.x.x~
+ * ~
+ * where 'x' - pixel with obstacle, '.' - background pixel,
+ * '~' - row continuation in the same style */
+ max_labels = (rows * columns) / 3 + 1;
+ }
+ ++max_labels; // add zero label
+ max_labels = std::min(max_labels, size_t(std::numeric_limits::max()));
+ return max_labels;
+ }
+
+ /// @brief Find the root of the tree of node i
+ Label findRoot(Label i)
+ {
+ Label root = i;
+ for (; labels_[root] < root; root = labels_[root]) { /*do nothing*/}
+ return root;
+ }
+
+ /// @brief Set the root of the tree of node i
+ void setRoot(Label i, Label root)
+ {
+ while (labels_[i] < i) {
+ auto j = labels_[i];
+ labels_[i] = root;
+ i = j;
+ }
+ labels_[i] = root;
+ }
+
+private:
+ /**
+ * Linear trees container. If we have two trees: (2 -> 1) and (4 -> 3), (5 -> 3)
+ * and one single node 0, the content of the vector will be:
+ * index: 0|1|2|3|4|5
+ * value: 0|1|1|3|3|3
+ * After unionTrees(1, 3) we have one tree (2 -> 1), (3 -> 1), (4 -> 3), (5 -> 3) and one single node 0:
+ * index: 0|1|2|3|4|5
+ * value: 0|1|1|1|3|3
+ */
+ std::vector labels_;
+ Label labels_size_{};
+ Label next_free_{};
+};
+
+/// @brief The specializations of this class provide the definition of the pixel label
+template
+struct ProcessPixel;
+
+/// @brief Define the label of a pixel in an 8-linked image
+template<>
+struct ProcessPixel
+{
+ /**
+ * @brief Set the label of the current pixel image.e() based on labels in its neighborhood
+ * @tparam ImageWindow Window parameterized by class DoNothing or ReplaceToZero
+ * @tparam LabelsWindow Window parameterized by class DoNothing or ReplaceToZero
+ * @tparam Label integer type of label
+ * @param image input image window. Image data will not be changed. De facto, image is a const ref
+ * @param label output label window
+ * @param eq_trees union-find structure
+ * @throw LabelOverflow if all possible labels already used
+ */
+ template
+ static void pass(
+ ImageWindow & image, LabelsWindow & label, EquivalenceLabelTrees & eq_trees,
+ IsBg && is_bg)
+ {
+ Label & current = label.e();
+
+ //The decision tree traversal. See reference article for details
+ if (!is_bg(image.e())) {
+ if (label.b()) {
+ current = label.b();
+ } else {
+ if (!is_bg(image.c())) {
+ if (!is_bg(image.a())) {
+ current = eq_trees.unionTrees(label.c(), label.a());
+ } else {
+ if (!is_bg(image.d())) {
+ current = eq_trees.unionTrees(label.c(), label.d());
+ } else {
+ current = label.c();
+ }
+ }
+ } else {
+ if (!is_bg(image.a())) {
+ current = label.a();
+ } else {
+ if (!is_bg(image.d())) {
+ current = label.d();
+ } else {
+ current = eq_trees.makeLabel();
+ }
+ }
+ }
+ }
+ } else {
+ current = 0;
+ }
+ }
+};
+
+/// @brief Define the label of a pixel in an 4-linked image
+template<>
+struct ProcessPixel
+{
+ /**
+ * @brief Set the label of the current pixel image.e() based on labels in its neighborhood
+ * @tparam ImageWindow Window parameterized by class DoNothing or ReplaceToZero
+ * @tparam LabelsWindow Window parameterized by class DoNothing or ReplaceToZero
+ * @tparam Label integer type of label
+ * @param image input image window. Image data will not be changed. De facto, image is a const ref
+ * @param label output label window
+ * @param eq_trees union-find structure
+ * @throw LabelOverflow if all possible labels already used
+ */
+ template
+ static void pass(
+ ImageWindow & image, LabelsWindow & label, EquivalenceLabelTrees & eq_trees,
+ IsBg && is_bg)
+ {
+ Label & current = label.e();
+
+ // Simplified decision tree traversal. See reference article for details
+ if (!is_bg(image.e())) {
+ if (!is_bg(image.b())) {
+ if (!is_bg(image.d())) {
+ current = eq_trees.unionTrees(label.d(), label.b());
+ } else {
+ current = label.b();
+ }
+ } else {
+ if (!is_bg(image.d())) {
+ current = label.d();
+ } else {
+ current = eq_trees.makeLabel();
+ }
+ }
+ } else {
+ current = 0;
+ }
+ }
+};
+
+/**
+ * @brief Applies a 1d shape to the neighborhood of each pixel of the input image.
+ * Applies a 1d shape (row by row) to the neighborhood of each pixel and passes the result of the overlay to touch_fn.
+ * Special case: When processing the first and last pixel of each row, interpreting the missing neighbor as 0.
+ * @tparam TouchFn function object.
+ * Signature should be equivalent to the following:
+ * void fn(uint8_t& out, std::initializer_list in),
+ * where out - pixel of the output image, in - result of overlaying the shape on the neighborhood of source pixel
+ * @param input input image
+ * @param first_input_row row from which to start processing on the input image
+ * @param output output image
+ * @param first_output_row row from which to start processing on the output image
+ * @param shape structuring element row (size 3, i.e. shape[0], shape[1], shape[2])
+ * Should only contain values 0 (ignore neighborhood pixel) or 255 (use pixel).
+ * @param touch_fn binary operation that updates a pixel in the output image with an overlay
+ */
+template
+void probeRows(
+ const Image & input, size_t first_input_row,
+ Image & output, size_t first_output_row,
+ const uint8_t * shape, Apply touch_fn)
+{
+ const size_t rows = input.rows() - std::max(first_input_row, first_output_row);
+ const size_t columns = input.columns();
+
+ auto apply_shape = [&shape](uint8_t value, uint8_t index) -> uint8_t {
+ return value & shape[index];
+ };
+
+ auto get_input_row = [&input, first_input_row](size_t row) {
+ return input.row(row + first_input_row);
+ };
+ auto get_output_row = [&output, first_output_row](size_t row) {
+ return output.row(row + first_output_row);
+ };
+
+ if (columns == 1) {
+ for (size_t i = 0; i < rows; ++i) {
+ // process single column. Interpret pixel from column -1 and 1 as 0
+ auto overlay = {uint8_t(0), apply_shape(*get_input_row(i), 1), uint8_t(0)};
+ touch_fn(*get_output_row(i), overlay);
+ }
+ } else {
+ for (size_t i = 0; i < rows; ++i) {
+ const uint8_t * in = get_input_row(i);
+ const uint8_t * last_column_pixel = in + columns - 1;
+ uint8_t * out = get_output_row(i);
+
+ // process first column. Interpret pixel from column -1 as 0
+ {
+ auto overlay = {uint8_t(0), apply_shape(*in, 1), apply_shape(*(in + 1), 2)};
+ touch_fn(*out, overlay);
+ ++in;
+ ++out;
+ }
+
+ // process next columns up to last
+ for (; in != last_column_pixel; ++in, ++out) {
+ auto overlay = {
+ apply_shape(*(in - 1), 0),
+ apply_shape(*(in), 1),
+ apply_shape(*(in + 1), 2)
+ };
+ touch_fn(*out, overlay);
+ }
+
+ // process last column
+ {
+ auto overlay = {apply_shape(*(in - 1), 0), apply_shape(*(in), 1), uint8_t(0)};
+ touch_fn(*out, overlay);
+ ++in;
+ ++out;
+ }
+ }
+ }
+}
+
+/**
+ * @brief Perform morphological operations
+ * @tparam AggregateFn function object.
+ * Signature should be equivalent to the following:
+ * uint8_t fn(std::initializer_list in),
+ * where in - result of overlaying the shape on the neighborhood of source pixel
+ * @param input input image
+ * @param output output image
+ * @param shape structuring element image with size 3x3.
+ * Should only contain values 0 (ignore neighborhood pixel) or 255 (use pixel).
+ * @param aggregate neighborhood pixels aggregator
+ * @throw std::logic_error if the sizes of the input and output images are different or
+ * shape size is not equal to 3x3
+ */
+template
+void morphologyOperation(
+ const Image & input, Image & output,
+ const Image & shape, AggregateFn aggregate)
+{
+ if (input.rows() != output.rows() || input.columns() != output.columns()) {
+ throw std::logic_error(
+ "morphologyOperation: the sizes of the input and output images are different");
+ }
+
+ if (shape.rows() != 3 || shape.columns() != 3) {
+ throw std::logic_error("morphologyOperation: wrong shape size");
+ }
+
+ if (input.empty()) {
+ return;
+ }
+
+ // Simple write the pixel of the output image (first pass only)
+ auto set = [&](uint8_t & res, std::initializer_list lst) {res = aggregate(lst);};
+ // Update the pixel of the output image
+ auto update = [&](uint8_t & res, std::initializer_list lst) {
+ res = aggregate({res, aggregate(lst), 0});
+ };
+
+ // Apply the central shape row.
+ // This operation is applicable to all rows of the image, because at any position of the sliding window,
+ // its central row is located on the image. So we start from the zero line of input and output
+ probeRows(input, 0, output, 0, shape.row(1), set);
+
+ if (input.rows() > 1) {
+ // Apply the top shape row.
+ // In the uppermost position of the sliding window, its first row is outside the image border.
+ // Therefore, we start filling the output image starting from the line 1 and will process input.rows() - 1 lines in total
+ probeRows(input, 0, output, 1, shape.row(0), update);
+ // Apply the bottom shape row.
+ // Similarly, the input image starting from the line 1 and will process input.rows() - 1 lines in total
+ probeRows(input, 1, output, 0, shape.row(2), update);
+ }
+}
+
+/**
+ * @brief Return structuring element 3x3 image by predefined figure type
+ * @details Used in morphologyOperation
+ */
+Image createShape(ShapeBuffer3x3 & buffer, ConnectivityType connectivity)
+{
+ /**
+ * Shape - a binary matrix that is used as a mask. Each element of which is one of two values:
+ * code u - the corresponding pixel of the image will be used
+ * code i - the corresponding pixel of the image will be ignored
+ */
+ static constexpr uint8_t u = 255;
+ static constexpr uint8_t i = 0;
+
+ if (connectivity == ConnectivityType::Way8) {
+ buffer = {
+ u, u, u,
+ u, i, u,
+ u, u, u};
+ } else {
+ buffer = {
+ i, u, i,
+ u, i, u,
+ i, u, i};
+ }
+ return Image(3, 3, buffer.data(), 3);
+}
+
+/**
+ * @brief Implementation details for connectedComponents
+ * @sa connectedComponents
+ */
+template
+Label connectedComponentsImpl(
+ const Image & image, Image & labels,
+ imgproc_impl::EquivalenceLabelTrees & label_trees, const IsBg & is_background)
+{
+ using namespace imgproc_impl;
+ using PixelPass = ProcessPixel;
+
+ // scanning phase
+ // scan row 0
+ {
+ auto img = makeSafeWindow(nullptr, image.row(0), image.columns());
+ auto lbl = makeSafeWindow(nullptr, labels.row(0), image.columns());
+
+ const uint8_t * first_row_end = image.row(0) + image.columns();
+
+ for (; img.anchor() < first_row_end; img.next(), lbl.next()) {
+ PixelPass::pass(img, lbl, label_trees, is_background);
+ }
+ }
+
+ // scan rows 1, 2, ...
+ for (size_t row = 0; row < image.rows() - 1; ++row) {
+ // we can safely ignore checks label_mask for first column
+ Window label_mask{labels.row(row), labels.row(row + 1)};
+
+ auto up = image.row(row);
+ auto current = image.row(row + 1);
+
+ // scan column 0
+ {
+ auto img = makeSafeWindow(up, current, image.columns());
+ PixelPass::pass(img, label_mask, label_trees, is_background);
+ }
+
+ // scan columns 1, 2... image.columns() - 2
+ label_mask.next();
+
+ auto img = makeUnsafeWindow(std::next(up), std::next(current));
+ const uint8_t * current_row_last_element = current + image.columns() - 1;
+
+ for (; img.anchor() < current_row_last_element; img.next(), label_mask.next()) {
+ PixelPass::pass(img, label_mask, label_trees, is_background);
+ }
+
+ // scan last column
+ if (image.columns() > 1) {
+ auto last_img = makeSafeWindow(up, current, image.columns(), image.columns() - 1);
+ auto last_label = makeSafeWindow(
+ labels.row(row), labels.row(row + 1),
+ image.columns(), image.columns() - 1);
+ PixelPass::pass(last_img, last_label, label_trees, is_background);
+ }
+ }
+
+ // analysis phase
+ const std::vector & labels_map = label_trees.getLabels();
+
+ // labeling phase
+ labels.forEach(
+ [&](Label & l) {
+ l = labels_map[l];
+ });
+ return labels_map.size();
+}
+
+/**
+ * @brief Object to eliminate grouped noise on the image
+ * Stores a label tree that is reused
+ * @sa connectedComponents
+ */
+class GroupsRemover
+{
+public:
+ /// @brief Constructs the object and initializes the label tree
+ GroupsRemover()
+ {
+ label_trees_ = std::make_unique>();
+ }
+
+ /**
+ * @brief Calls removeGroupsPickLabelType with the Way4/Way8
+ * template parameter based on the runtime value of group_connectivity_type
+ * @tparam IsBg functor with signature bool (uint8_t)
+ * @param[in,out] image image to be denoised
+ * @param buffer dynamic memory block that will be used to store the temp labeled image
+ * @param group_connectivity_type pixels connectivity type
+ * @param minimal_group_size the border value of group size. Groups of this and larger
+ * size will be kept
+ * @param is_background returns true if the passed pixel value is background
+ */
+ template
+ void removeGroups(
+ Image & image, MemoryBuffer & buffer,
+ ConnectivityType group_connectivity_type, size_t minimal_group_size,
+ const IsBg & is_background) const
+ {
+ if (group_connectivity_type == ConnectivityType::Way4) {
+ removeGroupsPickLabelType(
+ image, buffer, minimal_group_size,
+ is_background);
+ } else {
+ removeGroupsPickLabelType(
+ image, buffer, minimal_group_size,
+ is_background);
+ }
+ }
+
+private:
+ /**
+ * @brief Calls tryRemoveGroupsWithLabelType with the label tree stored in this object.
+ * If the stored tree labels are 16 bits and the call fails,
+ * change the stored tree type to 32 bit and retry the call.
+ * @throw imgproc_impl::LabelOverflow if 32 bit label tree is not enough
+ * to complete the operation
+ */
+ template
+ void removeGroupsPickLabelType(
+ Image & image, MemoryBuffer & buffer,
+ size_t minimal_group_size, const IsBg & is_background) const
+ {
+ bool success{};
+ auto label_trees16 =
+ dynamic_cast *>(label_trees_.get());
+
+ if (label_trees16) {
+ success = tryRemoveGroupsWithLabelType(
+ image, buffer, minimal_group_size,
+ *label_trees16, is_background, false);
+ }
+
+ if (!success) {
+ auto label_trees32 =
+ dynamic_cast *>(label_trees_.get());
+
+ if (!label_trees32) {
+ label_trees_ = std::make_unique>();
+ label_trees32 =
+ dynamic_cast *>(label_trees_.get());
+ }
+ tryRemoveGroupsWithLabelType(
+ image, buffer, minimal_group_size, *label_trees32,
+ is_background, true);
+ }
+ }
+ /**
+ * @brief Calls removeGroupsImpl catching its exceptions if throw_on_label_overflow is true
+ * @param throw_on_label_overflow defines the policy for handling exceptions thrown
+ * from removeGroupsImpl. If throw_on_label_overflow is true, exceptions are simply
+ * rethrown. Otherwise, this function will return false on exception.
+ * @return true if removeGroupsImpl throw and throw_on_label_overflow false.
+ * False in other case
+ */
+ template
+ bool tryRemoveGroupsWithLabelType(
+ Image & image, MemoryBuffer & buffer, size_t minimal_group_size,
+ imgproc_impl::EquivalenceLabelTrees & label_trees,
+ const IsBg & is_background,
+ bool throw_on_label_overflow) const
+ {
+ bool success{};
+ try {
+ removeGroupsImpl(image, buffer, label_trees, minimal_group_size, is_background);
+ success = true;
+ } catch (imgproc_impl::LabelOverflow &) {
+ if (throw_on_label_overflow) {
+ throw;
+ }
+ }
+ return success;
+ }
+ /// @brief Eliminate group noise in the image
+ template
+ void removeGroupsImpl(
+ Image & image, MemoryBuffer & buffer,
+ imgproc_impl::EquivalenceLabelTrees & label_trees, size_t minimal_group_size,
+ const IsBg & is_background) const
+ {
+ // Creates an image labels in which each obstacles group is labeled with a unique code
+ auto components = connectedComponents(image, buffer, label_trees, is_background);
+ const Label groups_count = components.second;
+ const Image & labels = components.first;
+
+ // Calculates the size of each group.
+ // Group size is equal to the number of pixels with the same label
+ const Label max_label_value = groups_count - 1; // It's safe. groups_count always non-zero
+ std::vector groups_sizes = histogram(
+ labels, max_label_value, size_t(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
+
+ // noise_labels_table[i] = true if group with label i is noise
+ std::vector noise_labels_table(groups_sizes.size());
+ auto transform_fn = [&minimal_group_size](size_t bin_value) {
+ return bin_value < minimal_group_size;
+ };
+ std::transform(
+ groups_sizes.begin(), groups_sizes.end(), noise_labels_table.begin(),
+ transform_fn);
+
+ // Replace the pixel values from the small groups to background code
+ labels.convert(
+ image, [&](Label src, uint8_t & trg) {
+ if (!is_background(trg) && noise_labels_table[src]) {
+ trg = 0;
+ }
+ });
+ }
+
+private:
+ mutable std::unique_ptr label_trees_;
+};
+
+} // namespace imgproc_impl
+
+template
+std::pair, Label> connectedComponents(
+ const Image & image, MemoryBuffer & buffer,
+ imgproc_impl::EquivalenceLabelTrees & label_trees, const IsBg & is_background)
+{
+ using namespace imgproc_impl;
+ const size_t pixels = image.rows() * image.columns();
+
+ if (pixels == 0) {
+ return {Image{}, 0};
+ }
+
+ Label * image_buffer = buffer.get(pixels);
+ Image labels(image.rows(), image.columns(), image_buffer, image.columns());
+ label_trees.reset(image.rows(), image.columns(), connectivity);
+ const Label total_labels = connectedComponentsImpl(
+ image, labels, label_trees,
+ is_background);
+ return std::make_pair(labels, total_labels);
+}
+
+} // namespace nav2_costmap_2d
+
+#endif // NAV2_COSTMAP_2D__DENOISE__IMAGE_PROCESSING_HPP_
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 0000000000..93a6613af2
--- /dev/null
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise_layer.hpp
@@ -0,0 +1,131 @@
+// Copyright (c) 2023 Andrey Ryzhikov
+//
+// Licensed 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.
+
+#ifndef NAV2_COSTMAP_2D__DENOISE_LAYER_HPP_
+#define NAV2_COSTMAP_2D__DENOISE_LAYER_HPP_
+
+#include "nav2_costmap_2d/layer.hpp"
+#include "nav2_costmap_2d/denoise/image_processing.hpp"
+
+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
+
+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.
+ * @throw std::logic_error in case inner logic errors
+ */
+ void denoise(Image & 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)
+ * @throw std::logic_error in case inner logic errors
+ * @warning If image.empty() the behavior is undefined
+ */
+ void removeGroups(Image & image) const;
+
+ /**
+ * @brief Removes from the image freestanding single white pixels
+ * Works similarly to removeGroups with minimal_group_size_ = 2, but about 10x faster
+ * @throw std::logic_error in case inner logic errors
+ * @warning If image.empty() the behavior is undefined
+ */
+ void removeSinglePixels(Image & image) const;
+ /**
+ * @brief Separates image pixels into objects and background
+ * @return true if the pixel value is not an obstacle code. False in other case
+ */
+ bool isBackground(uint8_t pixel) const;
+
+private:
+ // The border value of group size. Groups of this and larger size will be kept
+ size_t minimal_group_size_{};
+ // Pixels connectivity type. Determines how pixels belonging to the same group can be arranged
+ ConnectivityType group_connectivity_type_{ConnectivityType::Way8};
+ // Memory buffer for temporal image
+ mutable MemoryBuffer buffer_;
+ // Implementing the removal of grouped noise
+ imgproc_impl::GroupsRemover groups_remover_;
+ // Interpret NO_INFORMATION code as obstacle
+ bool no_information_is_obstacle_{};
+};
+
+} // 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 0000000000..b8f6c427a6
--- /dev/null
+++ b/nav2_costmap_2d/plugins/denoise_layer.cpp
@@ -0,0 +1,211 @@
+// Copyright (c) 2023 Andrey Ryzhikov
+//
+// Licensed 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.
+
+#include "nav2_costmap_2d/denoise_layer.hpp"
+
+#include
+#include
+#include
+#include
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_costmap_2d
+{
+
+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));
+
+ const auto node = node_.lock();
+
+ if (!node) {
+ throw std::runtime_error("DenoiseLayer::onInitialize: Failed to lock node");
+ }
+ 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);
+ minimal_group_size_ = 1;
+ } else {
+ 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) {
+ group_connectivity_type_ = ConnectivityType::Way4;
+ } else {
+ group_connectivity_type_ = ConnectivityType::Way8;
+
+ if (group_connectivity_type_param != 8) {
+ 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);
+ }
+ }
+
+ 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;
+ }
+
+ if (min_x >= max_x || min_y >= max_y) {
+ return;
+ }
+ no_information_is_obstacle_ = master_grid.getDefaultValue() != NO_INFORMATION;
+
+ // wrap roi_image over existing costmap2d buffer
+ unsigned char * master_array = master_grid.getCharMap();
+ const int step = static_cast(master_grid.getSizeInCellsX());
+
+ const size_t width = max_x - min_x;
+ const size_t height = max_y - min_y;
+ Image roi_image(height, width, master_array + min_y * step + 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(Image & image) const
+{
+ 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(Image & image) const
+{
+ groups_remover_.removeGroups(
+ image, buffer_, group_connectivity_type_, minimal_group_size_,
+ [this](uint8_t pixel) {return isBackground(pixel);});
+}
+
+void
+DenoiseLayer::removeSinglePixels(Image & image) const
+{
+ // Building a map of 4 or 8-connected neighbors.
+ // The pixel of the map is 255 if there is an obstacle nearby
+ uint8_t * buf = buffer_.get(image.rows() * image.columns());
+ Image max_neighbors_image(image.rows(), image.columns(), buf, image.columns());
+
+ // If NO_INFORMATION (=255) isn't obstacle, we can't use a simple max() to check
+ // any obstacle nearby. In this case, we interpret NO_INFORMATION as an empty space.
+ if (!no_information_is_obstacle_) {
+ auto replace_to_free = [](uint8_t v) {
+ return v == NO_INFORMATION ? FREE_SPACE : v;
+ };
+ auto max = [&](const std::initializer_list lst) {
+ std::array buf = {
+ replace_to_free(*lst.begin()),
+ replace_to_free(*(lst.begin() + 1)),
+ replace_to_free(*(lst.begin() + 2))
+ };
+ return *std::max_element(buf.begin(), buf.end());
+ };
+ dilate(image, max_neighbors_image, group_connectivity_type_, max);
+ } else {
+ auto max = [](const std::initializer_list lst) {
+ return std::max(lst);
+ };
+ dilate(image, max_neighbors_image, group_connectivity_type_, max);
+ }
+
+ max_neighbors_image.convert(
+ image, [this](uint8_t maxNeighbor, uint8_t & img) {
+ if (!isBackground(img) && isBackground(maxNeighbor)) {
+ img = FREE_SPACE;
+ }
+ });
+}
+
+bool DenoiseLayer::isBackground(uint8_t pixel) const
+{
+ bool is_obstacle =
+ pixel == LETHAL_OBSTACLE ||
+ pixel == INSCRIBED_INFLATED_OBSTACLE ||
+ (pixel == NO_INFORMATION && no_information_is_obstacle_);
+ return !is_obstacle;
+}
+
+} // 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 bad904023c..19c2edb56f 100644
--- a/nav2_costmap_2d/test/unit/CMakeLists.txt
+++ b/nav2_costmap_2d/test/unit/CMakeLists.txt
@@ -50,3 +50,8 @@ ament_add_gtest(costmap_filter_service_test costmap_filter_service_test.cpp)
target_link_libraries(costmap_filter_service_test
nav2_costmap_2d_core
)
+
+ament_add_gtest(denoise_layer_test denoise_layer_test.cpp image_test.cpp image_processing_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 0000000000..2c4a2b9840
--- /dev/null
+++ b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp
@@ -0,0 +1,519 @@
+// Copyright (c) 2023 Andrey Ryzhikov
+//
+// Licensed 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.
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include "nav2_costmap_2d/denoise_layer.hpp"
+#include "image_tests_helper.hpp"
+
+namespace nav2_costmap_2d
+{
+/**
+ * @brief nav2_costmap_2d::DenoiseLayer class wrapper
+ *
+ * Provides access to DenoiseLayer private methods for testing them in isolation
+ */
+class DenoiseLayerTester : public ::testing::Test
+{
+public:
+ void removeSinglePixels(
+ Image & image, ConnectivityType connectivity,
+ bool no_information_is_obstacle = true)
+ {
+ denoise_.group_connectivity_type_ = connectivity;
+ denoise_.no_information_is_obstacle_ = no_information_is_obstacle;
+ denoise_.removeSinglePixels(image);
+ }
+
+ void removeGroups(
+ Image & image, ConnectivityType connectivity,
+ size_t minimal_group_size, bool no_information_is_obstacle = true)
+ {
+ denoise_.group_connectivity_type_ = connectivity;
+ denoise_.minimal_group_size_ = minimal_group_size;
+ denoise_.no_information_is_obstacle_ = no_information_is_obstacle;
+ denoise_.removeGroups(image);
+ }
+
+ void denoise(
+ Image & image, ConnectivityType connectivity,
+ size_t minimal_group_size, bool no_information_is_obstacle = true)
+ {
+ denoise_.group_connectivity_type_ = connectivity;
+ denoise_.minimal_group_size_ = minimal_group_size;
+ denoise_.no_information_is_obstacle_ = no_information_is_obstacle;
+ 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_);
+ }
+
+protected:
+ std::vector image_buffer_bytes;
+ std::vector image_buffer_bytes2;
+ std::vector image_buffer_bytes3;
+
+private:
+ nav2_costmap_2d::DenoiseLayer denoise_;
+};
+
+}
+
+using namespace nav2_costmap_2d;
+
+TEST_F(DenoiseLayerTester, removeSinglePixels4way) {
+ const auto in = imageFromString(
+ "x.x."
+ "x..x"
+ ".x.."
+ "xx.x", image_buffer_bytes);
+ const auto exp = imageFromString(
+ "x..."
+ "x..."
+ ".x.."
+ "xx..", image_buffer_bytes2);
+ auto out = clone(in, image_buffer_bytes3);
+ 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, removeSinglePixels4wayNoInformationIsEmpty) {
+ const std::map legend = {{'.', 0}, {'n', NO_INFORMATION}, {'x', LETHAL_OBSTACLE}};
+ const auto in = imageFromString(
+ "x.x."
+ "xnnx"
+ "nxnn"
+ "xx.x", image_buffer_bytes, legend);
+ const auto exp = imageFromString(
+ "x..."
+ "xnn."
+ "nxnn"
+ "xx..", image_buffer_bytes2, legend);
+ auto out = clone(in, image_buffer_bytes3);
+ removeSinglePixels(out, ConnectivityType::Way4, false);
+
+ 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 auto in = imageFromString(
+ "x.x."
+ "x..x"
+ ".x.."
+ "xx.x", image_buffer_bytes);
+ const auto exp = imageFromString(
+ "x.x."
+ "x..x"
+ ".x.."
+ "xx..", image_buffer_bytes2);
+
+ auto out = clone(in, image_buffer_bytes3);
+ 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 auto in = imageFromString(
+ "x", image_buffer_bytes);
+ const auto exp = imageFromString(
+ ".", image_buffer_bytes2);
+
+ auto out = clone(in, image_buffer_bytes3);
+ removeSinglePixels(out, ConnectivityType::Way8);
+
+ ASSERT_TRUE(isEqual(out, exp));
+ }
+
+ {
+ const auto in = imageFromString(
+ "x."
+ ".x", image_buffer_bytes);
+ const auto exp = imageFromString(
+ "x."
+ ".x", image_buffer_bytes2);
+
+ auto out = clone(in, image_buffer_bytes3);
+ removeSinglePixels(out, ConnectivityType::Way8);
+
+ ASSERT_TRUE(isEqual(out, exp));
+ }
+
+ {
+ const auto in = imageFromString(
+ "x."
+ ".x", image_buffer_bytes);
+ const auto exp = imageFromString(
+ ".."
+ "..", image_buffer_bytes2);
+
+ auto out = clone(in, image_buffer_bytes3);
+ removeSinglePixels(out, ConnectivityType::Way4);
+
+ ASSERT_TRUE(isEqual(out, exp));
+ }
+}
+
+TEST_F(DenoiseLayerTester, removeSinglePixelsFromNonBinary) {
+ // buffer for 9 pixels with neutral (between FREE_SPACE and INSCRIBED_INFLATED_OBSTACLE) value
+ image_buffer_bytes.assign(9, 250);
+ Image in = makeImage(3, 3, image_buffer_bytes);
+ in.row(1)[1] = NO_INFORMATION;
+ Image exp = clone(in, image_buffer_bytes2);
+ exp.row(1)[1] = FREE_SPACE;
+
+ auto out = clone(in, image_buffer_bytes3);
+ 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 auto in = imageFromString(
+ ".xx..xx"
+ "..x.x.."
+ "x..x..x"
+ "x......"
+ "...x.xx"
+ "xxx..xx"
+ "....xx.", image_buffer_bytes);
+ const auto exp = imageFromString(
+ ".xx...."
+ "..x...."
+ "......."
+ "......."
+ ".....xx"
+ "xxx..xx"
+ "....xx.", image_buffer_bytes2);
+
+ auto out = clone(in, image_buffer_bytes3);
+ 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, removePixelsGroup4wayNoInformationIsEmpty) {
+ const std::map legend = {{'.', 0}, {'n', NO_INFORMATION}, {'x', LETHAL_OBSTACLE}};
+ const auto in = imageFromString(
+ ".xxnnxx"
+ "..xnx.."
+ "x..x..x"
+ "x......"
+ "nnnxnxx"
+ "xxx..xx"
+ "....xx.", image_buffer_bytes, legend);
+ const auto exp = imageFromString(
+ ".xxnn.."
+ "..xn..."
+ "......."
+ "......."
+ "nnn.nxx"
+ "xxx..xx"
+ "....xx.", image_buffer_bytes2, legend);
+
+ auto out = clone(in, image_buffer_bytes3);
+ removeGroups(out, ConnectivityType::Way4, 3, false);
+
+ 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 auto in = imageFromString(
+ ".xx..xx"
+ "..x.x.."
+ "x..x..x"
+ "x......"
+ "...x.xx"
+ "xxx..xx"
+ "....xx.", image_buffer_bytes);
+ const auto exp = imageFromString(
+ ".xx..xx"
+ "..x.x.."
+ "...x..."
+ "......."
+ "...x.xx"
+ "xxx..xx"
+ "....xx.", image_buffer_bytes2);
+
+ auto out = clone(in, image_buffer_bytes3);
+ 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 auto in = imageFromString(
+ "x", image_buffer_bytes);
+ const auto exp = imageFromString(
+ ".", image_buffer_bytes2);
+
+ auto out = clone(in, image_buffer_bytes3);
+ removeGroups(out, ConnectivityType::Way8, 3);
+
+ ASSERT_TRUE(isEqual(out, exp));
+ }
+
+ {
+ const auto in = imageFromString(
+ "x."
+ ".x", image_buffer_bytes);
+ const auto exp = imageFromString(
+ ".."
+ "..", image_buffer_bytes2);
+
+ auto out = clone(in, image_buffer_bytes3);
+ removeGroups(out, ConnectivityType::Way8, 3);
+
+ ASSERT_TRUE(isEqual(out, exp));
+ }
+}
+
+TEST_F(DenoiseLayerTester, removePixelsGroupFromNonBinary) {
+ // buffer for 9 pixels with neutral (between FREE_SPACE and INSCRIBED_INFLATED_OBSTACLE) value
+ image_buffer_bytes.assign(9, 250);
+ Image in = makeImage(3, 3, image_buffer_bytes);
+ in.row(1)[1] = 255;
+
+ Image exp = clone(in, image_buffer_bytes2);
+ exp.row(1)[1] = 0;
+
+ auto out = clone(in, image_buffer_bytes3);
+ 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 auto in = imageFromString(
+ "xx."
+ "..."
+ "..x", image_buffer_bytes);
+ const auto exp = imageFromString(
+ "xx."
+ "..."
+ "...", image_buffer_bytes2);
+
+ auto out = clone(in, image_buffer_bytes3);
+ 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 auto in = imageFromString(
+ "xx."
+ "x.x"
+ "..x", image_buffer_bytes);
+ const auto exp = imageFromString(
+ "xx."
+ "x.."
+ "...", image_buffer_bytes2);
+
+ auto out = clone(in, image_buffer_bytes3);
+ 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, denoiseEmpty) {
+ Image in;
+
+ ASSERT_NO_THROW(denoise(in, ConnectivityType::Way4, 2));
+}
+
+TEST_F(DenoiseLayerTester, denoiseNothing) {
+ Image in = makeImage(1, 1, image_buffer_bytes);
+
+ 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.);
+ costmap.setCost(0, 0, NO_INFORMATION);
+ DenoiseLayerTester::configure(layer, ConnectivityType::Way4, 2);
+
+ layer.updateCosts(costmap, 0, 0, 1, 1);
+
+ ASSERT_EQ(costmap.getCost(0), FREE_SPACE);
+}
+
+// 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);
+ return layer;
+}
+
+TEST_F(DenoiseLayerTester, initializeDefault) {
+ auto layer = constructLayer();
+
+ DenoiseLayerTester::initialize(*layer);
+
+ ASSERT_EQ(
+ DenoiseLayerTester::getParameters(*layer),
+ std::make_tuple(true, 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, 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, ConnectivityType::Way8, 1));
+}
diff --git a/nav2_costmap_2d/test/unit/image_processing_test.cpp b/nav2_costmap_2d/test/unit/image_processing_test.cpp
new file mode 100644
index 0000000000..957fcf2732
--- /dev/null
+++ b/nav2_costmap_2d/test/unit/image_processing_test.cpp
@@ -0,0 +1,514 @@
+// Copyright (c) 2023 Andrey Ryzhikov
+//
+// Licensed 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.
+
+#include
+#include
+
+#include "nav2_costmap_2d/denoise/image_processing.hpp"
+#include "image_tests_helper.hpp"
+
+using namespace nav2_costmap_2d;
+using namespace imgproc_impl;
+
+struct ImageProcTester : public ::testing::Test
+{
+protected:
+ std::vector image_buffer_bytes_;
+ std::vector image_buffer_bytes2_;
+ std::vector image_buffer_bytes3_;
+ std::vector image_buffer_words_;
+};
+
+TEST(OutOfBounds, outOfBoundsAccess) {
+ // check access to nullptr row (up)
+ {
+ out_of_bounds_policy::ReplaceToZero c(nullptr, nullptr, 2);
+ uint8_t * any_non_null = reinterpret_cast(&c);
+ ASSERT_EQ(c.up(any_non_null), uint8_t(0));
+ }
+ // check out of bounds access
+ {
+ std::array data = {1, 2, 3};
+ out_of_bounds_policy::ReplaceToZero c(data.data(), data.data(), 2);
+ auto left_out_of_bounds = std::prev(data.data());
+ auto right_out_of_bounds = data.data() + data.size();
+
+ ASSERT_EQ(c.up(left_out_of_bounds), uint8_t(0));
+ ASSERT_EQ(c.up(right_out_of_bounds), uint8_t(0));
+
+ ASSERT_EQ(c.down(left_out_of_bounds), uint8_t(0));
+ ASSERT_EQ(c.down(right_out_of_bounds), uint8_t(0));
+ }
+}
+
+TEST_F(ImageProcTester, calculateHistogramWithoutTruncation) {
+ image_buffer_words_ = {0, 2, 1, 0, 3, 4, 1, 2, 0};
+ Image image = makeImage(3, 3, image_buffer_words_);
+ const uint16_t max_bin_size = 3; // three zeros
+ const uint16_t max_value = 4;
+ const auto hist = histogram(image, max_value, max_bin_size);
+
+ const std::array expected = {3, 2, 2, 1, 1};
+ ASSERT_EQ(hist.size(), expected.size());
+ ASSERT_TRUE(std::equal(expected.begin(), expected.end(), hist.begin()));
+}
+
+TEST_F(ImageProcTester, calculateHistogramWithTruncation) {
+ image_buffer_words_ = {0, 2, 1, 0, 3, 4, 1, 2, 0};
+ Image image = makeImage(3, 3, image_buffer_words_);
+ const uint16_t max_bin_size = 2;
+ const uint16_t max_value = 4;
+ const auto hist = histogram(image, max_value, max_bin_size);
+
+ const std::array expected = {2, 2, 2, 1, 1};
+ ASSERT_EQ(hist.size(), expected.size());
+ ASSERT_TRUE(std::equal(expected.begin(), expected.end(), hist.begin()));
+}
+
+TEST_F(ImageProcTester, calculateHistogramOfEmpty) {
+ const uint16_t max_bin_size = 1;
+ const uint16_t max_value = 0;
+ Image empty;
+
+ const auto hist = histogram(empty, max_value, max_bin_size);
+ ASSERT_TRUE(hist.empty());
+}
+
+
+TEST(EquivalenceLabelTrees, newLabelsTest) {
+ EquivalenceLabelTrees eq;
+ eq.reset(10, 10, ConnectivityType::Way4);
+ ASSERT_EQ(eq.makeLabel(), 1);
+ ASSERT_EQ(eq.makeLabel(), 2);
+ ASSERT_EQ(eq.makeLabel(), 3);
+}
+
+TEST(EquivalenceLabelTrees, unionTest) {
+ EquivalenceLabelTrees eq;
+ eq.reset(10, 10, ConnectivityType::Way4);
+
+ // create 5 single nodes
+ for (size_t i = 1; i < 6; ++i) {
+ eq.makeLabel();
+ }
+ ASSERT_EQ(eq.unionTrees(4, 3), 3);
+ ASSERT_EQ(eq.unionTrees(5, 3), 3);
+ ASSERT_EQ(eq.unionTrees(4, 1), 1);
+ ASSERT_EQ(eq.unionTrees(2, 5), 1);
+}
+
+struct ConnectedComponentsTester : public ImageProcTester
+{
+protected:
+ template
+ bool fingerTest();
+
+ template
+ bool spiralTest();
+
+ inline static bool isBackground(uint8_t pixel)
+ {
+ return pixel == BACKGROUND_CODE;
+ }
+
+ inline Image makeChessboardLikeImage(
+ size_t rows, size_t cols,
+ std::vector & buffer) const;
+
+protected:
+ MemoryBuffer buffer_;
+ imgproc_impl::EquivalenceLabelTrees label_trees_;
+ static const uint8_t BACKGROUND_CODE = 0;
+ static const uint8_t FOREGROUND_CODE = 255;
+};
+
+Image ConnectedComponentsTester::makeChessboardLikeImage(
+ size_t rows, size_t cols,
+ std::vector & buffer) const
+{
+ Image image = makeImage(rows, cols, buffer, cols * 3);
+
+ auto inverse = [this](uint8_t v) {
+ return (v == BACKGROUND_CODE) ? FOREGROUND_CODE : BACKGROUND_CODE;
+ };
+
+ uint8_t current_value = FOREGROUND_CODE;
+ for (size_t j = 0; j < cols; ++j) {
+ *(image.row(0) + j) = current_value;
+ current_value = inverse(current_value);
+ }
+
+ for (size_t i = 1; i < rows; ++i) {
+ auto up = image.row(i - 1);
+ auto current = image.row(i);
+ for (size_t j = 0; j < cols; ++j, ++up, ++current) {
+ *current = inverse(*up);
+ }
+ }
+ return image;
+}
+
+TEST_F(ConnectedComponentsTester, way4EmptyTest) {
+ Image empty;
+ const auto result = connectedComponents(
+ empty, buffer_, label_trees_,
+ isBackground);
+ ASSERT_EQ(result.second, uint8_t(0));
+}
+
+TEST_F(ConnectedComponentsTester, way4SinglePixelTest) {
+ Image input = makeImage(1, 1, image_buffer_bytes_);
+ {
+ input.row(0)[0] = BACKGROUND_CODE;
+
+ const auto result = connectedComponents(
+ input, buffer_, label_trees_,
+ isBackground);
+
+ ASSERT_EQ(result.first.row(0)[0], 0);
+ ASSERT_EQ(result.second, 1);
+ }
+ {
+ input.row(0)[0] = FOREGROUND_CODE;
+
+ const auto result = connectedComponents(
+ input, buffer_, label_trees_,
+ isBackground);
+
+ ASSERT_EQ(result.first.row(0)[0], 1);
+ ASSERT_EQ(result.second, 2);
+ }
+}
+
+TEST_F(ConnectedComponentsTester, way4ImageSmallTest) {
+ {
+ Image input = makeImage(1, 2, image_buffer_bytes_);
+ input.row(0)[0] = BACKGROUND_CODE;
+ input.row(0)[1] = FOREGROUND_CODE;
+
+ const auto result = connectedComponents