Skip to content

Commit

Permalink
Minor refactoring
Browse files Browse the repository at this point in the history 
Signed-off-by: ryzhikovas <ryzhikovas@gmail.com>
  • Loading branch information
@ryzhikovas
ryzhikovas committed Oct 7, 2021
1 parent f6fa0e5 commit bc2b780
Show file tree
Hide file tree
Showing 3 changed files with 73 additions and 80 deletions.
31 changes: 12 additions & 19 deletions nav2_costmap_2d/include/nav2_costmap_2d/denoise_layer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,6 @@ class DenoiseLayer : public Layer

/**
* @brief Reports that no expansion is required
*
* The method is called to ask the plugin: which area of costmap it needs to update.
* A layer is essentially a filter, so it never needs to expand bounds.
*/
Expand All @@ -69,14 +68,13 @@ class DenoiseLayer : public Layer

/**
* @brief Filters noise-induced obstacles in the selected region of the costmap
*
* The method is called when costmap recalculation is required.
* It updates the costmap within its window bounds.
* @param master_grid The master costmap grid to update
* \param min_x X min map coord of the window to update
* \param min_y Y min map coord of the window to update
* \param max_x X max map coord of the window to update
* \param max_y Y max map coord of the window to update
* @param min_x X min map coord of the window to update
* @param min_y Y min map coord of the window to update
* @param max_x X max map coord of the window to update
* @param max_y Y max map coord of the window to update
*/
void updateCosts(
nav2_costmap_2d::Costmap2D & master_grid,
Expand All @@ -85,7 +83,6 @@ class DenoiseLayer : public Layer
protected:
/**
* @brief Initializes the layer on startup
*
* This method is called at the end of plugin initialization.
* Reads plugin parameters from a config file
*/
Expand All @@ -96,12 +93,12 @@ class DenoiseLayer : public Layer
* @brief Removes from the image single obstacles (white pixels) or small obstacles groups
*
* Pixels less than 255 will be interpreted as background (free space), 255 - as obstacles.
* Replaces groups of obstacles smaller than minimal_group_size to free space.
* Replaces groups of obstacles smaller than minimal_group_size_ to free space.
* Groups connectivity type is determined by the connectivity parameter.
*
* If minimal_group_size is 1 or 0, it does nothing
* If minimal_group_size_ is 1 or 0, it does nothing
* (all standalone obstacles will be preserved, since it satisfies this condition).
* If minimal_group_size equals 2, performs fast filtering based on the dilation operation.
* If minimal_group_size_ equals 2, performs fast filtering based on the dilation operation.
* Otherwise, it performs a slower segmentation-based operation.
*
* @param image source single channel image with depth CV_8U.
Expand All @@ -110,10 +107,9 @@ class DenoiseLayer : public Layer
void denoise(cv::Mat & image) const;

/**
* @brief Removes from the image groups of white pixels smaller than minimal_group_size
*
* @brief Removes from the image groups of white pixels smaller than minimal_group_size_
* Segments the image into groups of connected pixels
* Replace pixels in groups whose size smaller than minimal_group_size to zero value (background)
* Replace pixels in groups whose size smaller than minimal_group_size_ to zero value (background)
* @param image source single channel binary image with depth CV_8U
* @throw std::logic_error in case inner logic errors
* @warning If image.empty() or image.type() != CV_8UC1, the behavior is undefined
Expand All @@ -122,8 +118,7 @@ class DenoiseLayer : public Layer

/**
* @brief Removes from the image freestanding single white pixels
*
* Works similarly to removeGroups with minimal_group_size = 2, but about 10x faster
* Works similarly to removeGroups with minimal_group_size_ = 2, but about 10x faster
* @param image source single channel binary image with depth CV_8U
* @throw std::logic_error in case inner logic errors
* @warning If image.empty() or image.type() != CV_8UC1, the behavior is undefined
Expand Down Expand Up @@ -155,7 +150,6 @@ class DenoiseLayer : public Layer

/**
* @brief Convert each pixel of source image to target by lookup table
*
* Perform target[i,j] = table[ source[i,j] ]
* @param source source single channel image with depth CV_16UC1
* @param target source single channel image with depth CV_8UC1
Expand All @@ -170,7 +164,6 @@ class DenoiseLayer : public Layer

/**
* @brief Creates a lookup table for binary thresholding
*
* The table size is equal to groups_sizes.size()
* Lookup table[i] = OBSTACLE_CELL if groups_sizes[i] >= threshold,
* FREE_SPACE in other case
Expand Down Expand Up @@ -222,9 +215,9 @@ class DenoiseLayer : public Layer

private:
// Pixels connectivity type. Determines how pixels belonging to the same group can be arranged
size_t minimal_group_size{};
size_t minimal_group_size_{};
// The border value of group size. Groups of this and larger size will be kept
ConnectivityType group_connectivity_type{ConnectivityType::Way8};
ConnectivityType group_connectivity_type_{ConnectivityType::Way8};
};

template<class SourceElement, class TargetElement, class Converter>
Expand Down
106 changes: 53 additions & 53 deletions nav2_costmap_2d/plugins/denoise_layer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,47 +25,6 @@ namespace nav2_costmap_2d
{
static constexpr unsigned char OBSTACLE_CELL = 255;

void
DenoiseLayer::reset()
{
current_ = false;
}

bool
DenoiseLayer::isClearable()
{
return false;
}

void
DenoiseLayer::updateBounds(
double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/,
double * /*min_x*/, double * /*min_y*/,
double * /*max_x*/, double * /*max_y*/) {}

void
DenoiseLayer::updateCosts(
nav2_costmap_2d::Costmap2D & master_grid, int min_x, int min_y, int max_x, int max_y)
{
if (!enabled_) {
return;
}

unsigned char * master_array = master_grid.getCharMap();
const int step = static_cast<int>(master_grid.getSizeInCellsX());

const cv::Size roi_size {max_x - min_x, max_y - min_y};
cv::Mat roi_image(roi_size, CV_8UC1, static_cast<void *>(master_array + min_x), step);

try {
denoise(roi_image);
} catch (std::exception & ex) {
RCLCPP_ERROR(logger_, (std::string("Inner error: ") + ex.what()).c_str());
}

current_ = true;
}

void
DenoiseLayer::onInitialize()
{
Expand Down Expand Up @@ -97,43 +56,84 @@ DenoiseLayer::onInitialize()
"DenoiseLayer::onInitialize(): param minimal_group_size: %i."
" A value of 1 or less means that all map cells will be left as they are.",
minimal_group_size_param);
this->minimal_group_size = 1;
this->minimal_group_size_ = 1;
} else {
this->minimal_group_size = static_cast<size_t>(minimal_group_size_param);
this->minimal_group_size_ = static_cast<size_t>(minimal_group_size_param);
}

const int group_connectivity_type_param = getInt("group_connectivity_type");

if (group_connectivity_type_param == 4) {
this->group_connectivity_type = ConnectivityType::Way4;
this->group_connectivity_type_ = ConnectivityType::Way4;
} else if (group_connectivity_type_param == 8) {
this->group_connectivity_type = ConnectivityType::Way8;
this->group_connectivity_type_ = ConnectivityType::Way8;
} else {
RCLCPP_WARN(
logger_, "DenoiseLayer::onInitialize(): param group_connectivity_type: %i."
" Possible values are 4 (neighbors pixels are connected horizontally and vertically) "
"or 8 (neighbors pixels are connected horizontally, vertically and diagonally)."
"The default value 8 will be used",
group_connectivity_type_param);
this->group_connectivity_type = ConnectivityType::Way8;
this->group_connectivity_type_ = ConnectivityType::Way8;
}
current_ = true;
}

void
DenoiseLayer::reset()
{
current_ = false;
}

bool
DenoiseLayer::isClearable()
{
return false;
}

void
DenoiseLayer::updateBounds(
double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/,
double * /*min_x*/, double * /*min_y*/,
double * /*max_x*/, double * /*max_y*/) {}

void
DenoiseLayer::updateCosts(
nav2_costmap_2d::Costmap2D & master_grid, int min_x, int min_y, int max_x, int max_y)
{
if (!enabled_) {
return;
}

unsigned char * master_array = master_grid.getCharMap();
const int step = static_cast<int>(master_grid.getSizeInCellsX());

const cv::Size roi_size {max_x - min_x, max_y - min_y};
cv::Mat roi_image(roi_size, CV_8UC1, static_cast<void *>(master_array + min_x), step);

try {
denoise(roi_image);
} catch (std::exception & ex) {
RCLCPP_ERROR(logger_, (std::string("Inner error: ") + ex.what()).c_str());
}

current_ = true;
}

void
DenoiseLayer::denoise(cv::Mat & image) const
{
checkImageType(image, CV_8UC1, "DenoiseLayer::denoise_");
checkImageType(image, CV_8UC1, "DenoiseLayer::denoise");

if (image.empty()) {
return;
}

if (minimal_group_size <= 1) {
if (minimal_group_size_ <= 1) {
return; // A smaller group cannot exist. No one pixel will be changed
}

if (minimal_group_size == 2) {
if (minimal_group_size_ == 2) {
// Performs fast filtration based on erosion function
removeSinglePixels(image);
} else {
Expand All @@ -157,21 +157,21 @@ DenoiseLayer::removeGroups(cv::Mat & image) const
// There is an simple alternative: connectedComponentsWithStats.
// But cv::connectedComponents + calculateHistogram is about 20% faster
const uint16_t groups_count = static_cast<uint16_t>(
cv::connectedComponents(binary, labels, static_cast<int>(group_connectivity_type), CV_16U)
cv::connectedComponents(binary, labels, static_cast<int>(group_connectivity_type_), CV_16U)
);

// Calculates the size of each group.
// Group size is equal to the number of pixels with the same label
const auto max_label_value = groups_count - 1; // It's safe. groups_count always non-zero
std::vector<uint16_t> groups_sizes = calculateHistogram(
labels, max_label_value, minimal_group_size + 1);
labels, max_label_value, minimal_group_size_ + 1);
// The group of pixels labeled 0 corresponds to empty map cells.
// Zero bin of the histogram is equal to the number of pixels in this group.
// Because the values of empty map cells should not be changed, we will reset this bin
groups_sizes.front() = 0; // don't change image background value

// Replace the pixel values from the small groups to background code
const std::vector<uint8_t> lookup_table = makeLookupTable(groups_sizes, minimal_group_size);
const std::vector<uint8_t> lookup_table = makeLookupTable(groups_sizes, minimal_group_size_);
convert<uint16_t, uint8_t>(
labels, image, [&lookup_table, this](uint16_t src, uint8_t & trg) {
if (trg == OBSTACLE_CELL) { // This check is required for non-binary input image
Expand Down Expand Up @@ -243,7 +243,7 @@ DenoiseLayer::makeLookupTable(const std::vector<uint16_t> & groups_sizes, uint16
void
DenoiseLayer::removeSinglePixels(cv::Mat & image) const
{
const int shape_code = group_connectivity_type == ConnectivityType::Way4 ?
const int shape_code = group_connectivity_type_ == ConnectivityType::Way4 ?
cv::MorphShapes::MORPH_CROSS : cv::MorphShapes::MORPH_RECT;
cv::Mat shape = cv::getStructuringElement(shape_code, {3, 3});
shape.at<uint8_t>(1, 1) = 0;
Expand Down
16 changes: 8 additions & 8 deletions nav2_costmap_2d/test/unit/denoise_layer_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,21 +63,21 @@ class DenoiseLayerTester : public ::testing::Test

void removeSinglePixels(cv::Mat & image, ConnectivityType connectivity)
{
denoise_.group_connectivity_type = connectivity;
denoise_.group_connectivity_type_ = connectivity;
denoise_.removeSinglePixels(image);
}

void removeGroups(cv::Mat & image, ConnectivityType connectivity, size_t minimal_group_size)
{
denoise_.group_connectivity_type = connectivity;
denoise_.minimal_group_size = minimal_group_size;
denoise_.group_connectivity_type_ = connectivity;
denoise_.minimal_group_size_ = minimal_group_size;
denoise_.removeGroups(image);
}

void denoise(cv::Mat & image, ConnectivityType connectivity, size_t minimal_group_size)
{
denoise_.group_connectivity_type = connectivity;
denoise_.minimal_group_size = minimal_group_size;
denoise_.group_connectivity_type_ = connectivity;
denoise_.minimal_group_size_ = minimal_group_size;
denoise_.denoise(image);
}

Expand All @@ -102,14 +102,14 @@ class DenoiseLayerTester : public ::testing::Test
nav2_costmap_2d::DenoiseLayer & d, ConnectivityType connectivity, size_t minimal_group_size)
{
d.enabled_ = true;
d.group_connectivity_type = connectivity;
d.minimal_group_size = minimal_group_size;
d.group_connectivity_type_ = connectivity;
d.minimal_group_size_ = minimal_group_size;
}

static std::tuple<bool, DenoiseLayerTester::ConnectivityType, size_t> getParameters(
const nav2_costmap_2d::DenoiseLayer & d)
{
return std::make_tuple(d.enabled_, d.group_connectivity_type, d.minimal_group_size);
return std::make_tuple(d.enabled_, d.group_connectivity_type_, d.minimal_group_size_);
}

private:
Expand Down

0 comments on commit bc2b780

Please sign in to comment.