From 652354ed20216621e701ee6359e70ef5de3db77f Mon Sep 17 00:00:00 2001 From: ryzhikovas Date: Wed, 27 Oct 2021 18:43:48 +0300 Subject: [PATCH] Migration from OpenCV Signed-off-by: ryzhikovas --- nav2_costmap_2d/CMakeLists.txt | 3 - .../include/nav2_costmap_2d/denoise_layer.hpp | 134 +-- .../include/nav2_costmap_2d/image.hpp | 296 ++++++ .../nav2_costmap_2d/image_processing.hpp | 876 ++++++++++++++++++ nav2_costmap_2d/plugins/denoise_layer.cpp | 148 ++- nav2_costmap_2d/test/unit/CMakeLists.txt | 2 +- .../test/unit/denoise_layer_test.cpp | 308 ++---- .../test/unit/image_processing_test.cpp | 437 +++++++++ nav2_costmap_2d/test/unit/image_test.cpp | 145 +++ .../test/unit/image_tests_helper.hpp | 99 ++ 10 files changed, 1984 insertions(+), 464 deletions(-) create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/image.hpp create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/image_processing.hpp create mode 100644 nav2_costmap_2d/test/unit/image_processing_test.cpp create mode 100644 nav2_costmap_2d/test/unit/image_test.cpp create mode 100644 nav2_costmap_2d/test/unit/image_tests_helper.hpp diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 2fd20e910e4..0b87cba7094 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -23,7 +23,6 @@ find_package(tf2_ros REQUIRED) find_package(tf2_sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(angles REQUIRED) -find_package(OpenCV REQUIRED) remove_definitions(-DDISABLE_LIBUSB-1.0) find_package(Eigen3 REQUIRED) @@ -33,7 +32,6 @@ nav2_package() include_directories( include ${EIGEN3_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} ) add_definitions(${EIGEN3_DEFINITIONS}) @@ -94,7 +92,6 @@ ament_target_dependencies(layers ) target_link_libraries(layers nav2_costmap_2d_core - ${OpenCV_LIBS} ) add_library(filters SHARED 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 50919c10d01..406675d6899 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise_layer.hpp @@ -15,10 +15,11 @@ #ifndef NAV2_COSTMAP_2D__DENOISE_LAYER_HPP_ #define NAV2_COSTMAP_2D__DENOISE_LAYER_HPP_ -#include #include +#include #include "nav2_costmap_2d/layer.hpp" +#include "nav2_costmap_2d/image_processing.hpp" namespace nav2_costmap_2d { @@ -31,17 +32,6 @@ class DenoiseLayer : public Layer { friend class DenoiseLayerTester; // For test some private methods using gtest -private: - /// Pixels connectivity type (is the way in which pixels in image relate to - /// their neighbors) - enum class ConnectivityType: int - { - /// neighbors pixels are connected horizontally and vertically - Way4 = 4, - /// neighbors pixels are connected horizontally, vertically and diagonally - Way8 = 8 - }; - public: DenoiseLayer() = default; ~DenoiseLayer() = default; @@ -100,142 +90,36 @@ class DenoiseLayer : public Layer * (all standalone obstacles will be preserved, since it satisfies this condition). * If minimal_group_size_ equals 2, performs fast filtering based on the dilation operation. * Otherwise, it performs a slower segmentation-based operation. - * - * @param image source single channel image with depth CV_8U. * @throw std::logic_error in case inner logic errors */ - void denoise(cv::Mat & image) const; + 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) - * @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 + * @warning If image.empty() the behavior is undefined */ - void removeGroups(cv::Mat & image) const; + 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 - * @param image source single channel binary image with depth CV_8U * @throw std::logic_error in case inner logic errors - * @warning If image.empty() or image.type() != CV_8UC1, the behavior is undefined - */ - void removeSinglePixels(cv::Mat & image) const; - - /** - * @brief Calculate truncated histogram of image. - * - * Creates a histogram of image_max bins. - * Bin with index i keep min of (, bin_max). - * This truncation avoids overflow and is acceptable in the problem being solved. - * For example, the image may have 100'000 pixels equal to 0 - * (100'000 > std::numeric_limits). In this case, an overflow will occur when - * calculating a traditional histogram with bins of the uint16_t type. But in this function, - * the bin value will increase to the bin_max value, then stop. Overflow will not happen. - * - * Faster (because simpler) than cv::calcHist - * @param image source single channel image with depth CV_16U - * @param image_max max image pixel value - * @param bin_max max histogram bin value - * @return vector of histogram bins - * @throw std::logic_error if image.type() != CV_16UC1 - * @warning If source contains a pixel with a value, large then image_max, - * the behavior is undefined - */ - std::vector calculateHistogram( - const cv::Mat & image, uint16_t image_max, uint16_t bin_max) const; - - /** - * @brief Convert each pixel of source image to target by lookup table - * Perform target[i,j] = table[ source[i,j] ] - * @param source source single channel image with depth CV_16UC1 - * @param target source single channel image with depth CV_8UC1 - * @param table lookup table - * @throw std::logic_error if the source and target of different sizes - * @throw std::logic_error if source.type() != CV_16UC1 or target.type() != CV_8UC1 - * @warning If table.size() not more then max pixel value of source image, - * the behavior is undefined - */ - void applyLookupTable( - const cv::Mat & source, cv::Mat & target, const std::vector & table) const; - - /** - * @brief Creates a lookup table for binary thresholding - * The table size is equal to groups_sizes.size() - * Lookup table[i] = OBSTACLE_CELL if groups_sizes[i] >= threshold, - * FREE_SPACE in other case + * @warning If image.empty() the behavior is undefined */ - std::vector makeLookupTable( - const std::vector & groups_sizes, uint16_t threshold) const; - - /** - * @brief Convert each pixel of source to corresponding pixel of target using a custom function - * - * The source and target must be the same size. - * For calculation of new target value the operation can use source value and - * an optionally current target value. - * This function call operation(source[i, j], target[i, j]) for each pixel - * where target[i, j] is mutable - * @tparam SourceElement type of source pixel - * @tparam TargetElement type of target pixel - * @tparam Converter function object. - * Signature should be equivalent to the following: - * void fn(const SourceElement& src, TargetElement& trg) - * @param source input image with SourceElement-type pixels - * @param target output image with TargetElement-type pixels - * @param operation the binary operation op is applied to pairs of pixels: - * first (const) from source and second (mutable) from target - * @throw std::logic_error if the source and target of different sizes - * @warning If SourceElement/TargetElement type does not match the source/target image type, - * the behavior is undefined - */ - template - void convert(const cv::Mat & source, cv::Mat & target, Converter operation) const; - - /** - * @brief Checks that the image type is as expected - * @param image the image to be checked - * @param cv_type expected image type - * @param error_prefix prefix of the exception text that will be thrown is types are different - * @throw std::logic_error if the image type and cv_type are different - * @warning If error_prefix is nullptr, the behavior is undefined - */ - void checkImageType(const cv::Mat & image, int cv_type, const char * error_prefix) const; - - /** - * @brief Checks that the a.size() == b.size() - * @param error_prefix prefix of the exception text that will be thrown is sizes are different - * @throw std::logic_error if the images sizes are different - * @warning If error_prefix is nullptr, the behavior is undefined - */ - void checkImagesSizesEqual(const cv::Mat & a, const cv::Mat & b, const char * error_prefix) const; + void removeSinglePixels(Image & image) const; private: // Pixels connectivity type. Determines how pixels belonging to the same group can be arranged size_t minimal_group_size_{}; // The border value of group size. Groups of this and larger size will be kept ConnectivityType group_connectivity_type_{ConnectivityType::Way8}; + // Additional memory buffer + mutable std::unique_ptr buffer; }; -template -void DenoiseLayer::convert(const cv::Mat & source, cv::Mat & target, Converter operation) const -{ - checkImagesSizesEqual(source, target, "DenoiseLayer::convert. The source and target"); - - for (int row = 0; row < source.rows; ++row) { - const auto src_begin = source.ptr(row); - const auto src_end = src_begin + source.cols; - auto trg = target.ptr(row); - - for (auto src = src_begin; src != src_end; ++src, ++trg) { - operation(*src, *trg); - } - } -} - } // namespace nav2_costmap_2d #endif // NAV2_COSTMAP_2D__DENOISE_LAYER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/image.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/image.hpp new file mode 100644 index 00000000000..94ba82f71c2 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/image.hpp @@ -0,0 +1,296 @@ +// Copyright (c) 2021 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 +#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 + * @param rows number of image rows + * @param columns number of image columns + */ + Image(size_t rows, size_t columns); + + /** + * @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; + + /** + * @brief Create copy of image + */ + Image clone() const; + + /** + * @brief Create new image referencing to this image part + * Share image data between new and old object. + * Changing data in a new object will affect the given one and vice versa + * @param top first row of image part + * @param left first column of image part + * @param rows number of part rows + * @param columns number of part columns + * @throw std::logic_error if part out of image bounds + */ + Image part(size_t top, size_t left, size_t rows, size_t columns); + + /// @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 image pixel + * @warning If row/column exceed the image bounds, the behavior is undefined + */ + T & at(size_t row, size_t column); + + /// @overload + const T & at(size_t row, size_t column) const; + + /** + * @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 Fill the image with pixels value + void fill(T value); + + /** + * @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: + std::shared_ptr data_; + 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} +{ + auto empty_deleter = [](T *) { /* do nothing. This object does not own data_ */}; + data_ = std::shared_ptr(data, empty_deleter); + data_start_ = data; +} + +template +Image::Image(size_t rows, size_t columns) +: rows_{rows}, columns_{columns}, step_{columns} +{ + // Before C++20 we can't call std::make_shared... + data_ = std::make_unique(rows * columns); // it's ok if rows * columns == 0 + data_start_ = data_.get(); +} + +template +Image::Image(Image & other) +: data_{other.data_}, data_start_{other.data_start_}, + rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {} + +template +Image::Image(Image && other) noexcept +: data_{std::move(other.data_)}, data_start_{other.data_start_}, + rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {} + +template +Image Image::clone() const +{ + Image clone(rows(), columns()); + convert( + clone, [](const T & src, T & trg) { + trg = src; + }); + return clone; +} + +template +Image Image::part(size_t top, size_t left, size_t rows, size_t columns) +{ + if (top + rows > this->rows() || left + columns > this->columns()) { + throw std::logic_error("Image::part. Part is out of image bounds"); + } + Image img(rows, columns, &at(top, left), step_); + img.data_ = this->data_; // overwrite shared pointer to this object member + return img; +} + +template +T & Image::at(size_t row, size_t column) +{ + return const_cast( static_cast &>(*this).at(row, column) ); +} + +template +const T & Image::at(size_t row, size_t column) const +{ + return *(this->row(row) + column); +} + +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 +void Image::fill(T value) +{ + forEach([&](T & v) {v = value;}); +} + +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__IMAGE_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/image_processing.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/image_processing.hpp new file mode 100644 index 00000000000..0b16d70b016 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/image_processing.hpp @@ -0,0 +1,876 @@ +// Copyright (c) 2021 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_PROCESSING_HPP_ +#define NAV2_COSTMAP_2D__IMAGE_PROCESSING_HPP_ + +#include "image.hpp" +#include + +namespace nav2_costmap_2d +{ + +/// Pixels connectivity type (is the way in which pixels in image relate to +/// their neighbors) +enum class ConnectivityType: int +{ + /// neighbors pixels are connected horizontally and vertically + Way4 = 4, + /// neighbors pixels are connected horizontally, vertically and diagonally + Way8 = 8 +}; + +/** + * @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); + +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; +} + +/** + * @brief A memory buffer that can grow to an upper-bounded capacity + */ +class MemoryBuffer +{ +public: + /** + * @brief Creates an object with the fixed maximum capacity. Doesn't perform allocations + * @param capacity buffer capacity in bytes + */ + inline MemoryBuffer(size_t capacity) + : capacity_{capacity} {} + /// @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::logic_error if buffer size limited and sizeof(T) * count > capacity + */ + template + T * get(std::size_t count); + /// @brief Return the buffer capacity in bytes + inline size_t capacity() {return capacity_;} + +private: + inline void reset(); + inline void allocate(size_t bytes); + +private: + void * data{}; + size_t size{}; + size_t capacity_{}; +}; + +// forward declarations +namespace imgproc_impl +{ +template +class EquivalenceLabelTrees; + +template +void morphology_operation( + const Image & input, Image & output, + const Image & shape, AggregateFn aggregate); +inline Image getShape(ConnectivityType connectivity); +} // namespace imgproc_impl + +/** + * @brief Perform morphological dilation + * @param input input image + * @param output output image + * @param connectivity selector for selecting structuring element (Way4-> cross, Way8-> rect) + */ +inline void dilate( + const Image & input, Image & output, + ConnectivityType connectivity) +{ + using namespace imgproc_impl; + auto aggregate = [](std::initializer_list lst) { + return std::max(lst); + }; + morphology_operation(input, output, getShape(connectivity), aggregate); +} + +template +class ConnectedComponents +{ +public: + /** + * @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 + * @param image input image. Pixels less than 255 will be interpreted as background + * @param buffer memory block that will be used to store the result (labeled image) + * and the internal buffer for labels trees + * @throw std::logic_error in case of insufficient memory on labels integer overflow + * @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 pixel equal to 255, the total number of labels == 2. + * Two labels (0, 1) have been counted, although label 0 is not used) + */ + static std::pair, Label> detect(const Image & image, MemoryBuffer & buffer); + /** + * @brief Return the upper bound of the memory buffer for detecting connected components + * @param image input image + */ + static size_t optimalBufferSize(const Image & image); + +private: + static Label detectImpl( + const Image & image, Image