Impl noise filtering layer in the costmap_2d

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}

nav2_costmap_2d/costmap_plugins.xml

@@ -15,6 +15,9 @@
  <class type="nav2_costmap_2d::RangeSensorLayer" base_class_type="nav2_costmap_2d::Layer">
  <description>A range-sensor (sonar, IR) based obstacle layer for costmap_2d</description>
  </class>
+ <class type="nav2_costmap_2d::DenoiseLayer" base_class_type="nav2_costmap_2d::Layer">
+ <description>Filters noise-induced freestanding obstacles or small obstacles groups</description>
+ </class>
  </library>
 
  <library path="filters">

nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp

@@ -0,0 +1,207 @@
+// Copyright (c) 2022 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__IMAGE_HPP_
+#define NAV2_COSTMAP_2D__IMAGE_HPP_
+
+#include <cstddef>
+#include <stdexcept>
+
+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 T>
+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<class Functor>
+ 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<class Functor>
+ 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<class TargetElement, class Converter>
+ void convert(Image<TargetElement> & target, Converter && converter) const;
+
+private:
+ T * data_start_{};
+ size_t rows_{};
+ size_t columns_{};
+ size_t step_{};
+};
+
+template<class T>
+Image<T>::Image(size_t rows, size_t columns, T * data, size_t step)
+: rows_{rows}, columns_{columns}, step_{step}
+{
+ data_start_ = data;
+}
+
+template<class T>
+Image<T>::Image(Image & other)
+: data_start_{other.data_start_},
+ rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {}
+
+template<class T>
+Image<T>::Image(Image && other) noexcept
+: data_start_{other.data_start_},
+ rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {}
+
+template<class T>
+T * Image<T>::row(size_t row)
+{
+ return const_cast<T *>( static_cast<const Image<T> &>(*this).row(row) );
+}
+
+template<class T>
+const T * Image<T>::row(size_t row) const
+{
+ return data_start_ + row * step_;
+}
+
+template<class T>
+template<class Functor>
+void Image<T>::forEach(Functor && fn)
+{
+ static_cast<const Image<T> &>(*this).forEach(
+ [&](const T & pixel) {
+ fn(const_cast<T &>(pixel));
+ });
+}
+
+template<class T>
+template<class Functor>
+void Image<T>::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<class T>
+template<class TargetElement, class Converter>
+void Image<T>::convert(Image<TargetElement> & 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__IMAGE_HPP_