Skip to content

Commit

Permalink
Fix obstacle codes
Browse files Browse the repository at this point in the history 
interpret code LETHAL_OBSTACLE as an obstacle, just like NO_INFORMATION

Signed-off-by: ryzhikovas <ryzhikovas@gmail.com>
  • Loading branch information
@ryzhikovas
ryzhikovas committed Nov 24, 2021
1 parent ae847a7 commit 5d1abd1
Show file tree
Hide file tree
Showing 4 changed files with 73 additions and 43 deletions.
42 changes: 23 additions & 19 deletions nav2_costmap_2d/include/nav2_costmap_2d/image_processing.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -162,17 +162,22 @@ class ConnectedComponents
* 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<Image<Label>, Label> detect(const Image<uint8_t> & image, MemoryBuffer & buffer);
template<class IsBg>
static std::pair<Image<Label>, Label> detect(
const Image<uint8_t> & image, MemoryBuffer & buffer,
IsBg && is_background);
/**
* @brief Return the upper bound of the memory buffer for detecting connected components
* @param image input image
*/
static size_t optimalBufferSize(const Image<uint8_t> & image);

private:
template<class IsBg>
static Label detectImpl(
const Image<uint8_t> & image, Image<Label> & labels,
imgproc_impl::EquivalenceLabelTrees<Label> & label_trees);
imgproc_impl::EquivalenceLabelTrees<Label> & label_trees,
IsBg && is_background);
};

// Implementation
Expand Down Expand Up @@ -526,12 +531,6 @@ class EquivalenceLabelTrees
Label next_free;
};

/// @brief Return true if pixel_value is background code
inline bool is_bg(uint8_t pixel_value)
{
return pixel_value < 255;
}

/// @brief The specializations of this class provide the definition of the pixel label
template<ConnectivityType connectivity>
struct ProcessPixel;
Expand All @@ -549,9 +548,10 @@ struct ProcessPixel<ConnectivityType::Way8>
* @param label output label window
* @param eqTrees union-find structure
*/
template<class ImageWindow, class LabelsWindow, class Label>
template<class ImageWindow, class LabelsWindow, class Label, class IsBg>
static void pass(
ImageWindow & image, LabelsWindow & label, EquivalenceLabelTrees<Label> & eqTrees)
ImageWindow & image, LabelsWindow & label, EquivalenceLabelTrees<Label> & eqTrees,
IsBg && is_bg)
{
Label & current = label.e();

Expand Down Expand Up @@ -601,9 +601,10 @@ struct ProcessPixel<ConnectivityType::Way4>
* @param label output label window
* @param eqTrees union-find structure
*/
template<class ImageWindow, class LabelsWindow, class Label>
template<class ImageWindow, class LabelsWindow, class Label, class IsBg>
static void pass(
ImageWindow & image, LabelsWindow & label, EquivalenceLabelTrees<Label> & eqTrees)
ImageWindow & image, LabelsWindow & label, EquivalenceLabelTrees<Label> & eqTrees,
IsBg && is_bg)
{
Label & current = label.e();

Expand Down Expand Up @@ -765,8 +766,9 @@ Image<uint8_t> getShape(ConnectivityType connectivity)
} // namespace imgproc_impl

template<ConnectivityType connectivity, class Label>
template<class IsBg>
std::pair<Image<Label>, Label> ConnectedComponents<connectivity, Label>::detect(
const Image<uint8_t> & image, MemoryBuffer & buffer)
const Image<uint8_t> & image, MemoryBuffer & buffer, IsBg && is_background)
{
using namespace imgproc_impl;
const size_t pixels = image.rows() * image.columns();
Expand All @@ -791,15 +793,17 @@ std::pair<Image<Label>, Label> ConnectedComponents<connectivity, Label>::detect(
Label * labels_block = buffer_block + pixels;
Image<Label> labels(image.rows(), image.columns(), image_buffer, image.columns());
EquivalenceLabelTrees<Label> label_trees(labels_block, labels_buffer_size);
const Label total_labels = detectImpl(image, labels, label_trees);
const Label total_labels = detectImpl(image, labels, label_trees, is_background);
return std::make_pair(labels, total_labels);
}

template<ConnectivityType connectivity, class Label>
template<class IsBg>
Label
ConnectedComponents<connectivity, Label>::detectImpl(
const Image<uint8_t> & image, Image<Label> & labels,
imgproc_impl::EquivalenceLabelTrees<Label> & label_trees)
imgproc_impl::EquivalenceLabelTrees<Label> & label_trees,
IsBg && is_background)
{
using namespace imgproc_impl;
using PixelPass = ProcessPixel<connectivity>;
Expand All @@ -811,7 +815,7 @@ ConnectedComponents<connectivity, Label>::detectImpl(
auto lbl = makeSafeWindow<Label>(nullptr, labels.row(0), image.columns());

for (; &img.e() < image.row(0) + image.columns(); img.next(), lbl.next()) {
PixelPass::pass(img, lbl, label_trees);
PixelPass::pass(img, lbl, label_trees, is_background);
}
}

Expand All @@ -826,7 +830,7 @@ ConnectedComponents<connectivity, Label>::detectImpl(
// scan column 0
{
auto img = makeSafeWindow(up, current, image.columns());
PixelPass::pass(img, label_mask, label_trees);
PixelPass::pass(img, label_mask, label_trees, is_background);
}

// scan columns 1, 2... image.columns() - 2
Expand All @@ -836,7 +840,7 @@ ConnectedComponents<connectivity, Label>::detectImpl(
const uint8_t * current_line_last = current + image.columns() - 1;

for (; &img.e() < current_line_last; img.next(), label_mask.next()) {
PixelPass::pass(img, label_mask, label_trees);
PixelPass::pass(img, label_mask, label_trees, is_background);
}

// scan last column
Expand All @@ -845,7 +849,7 @@ ConnectedComponents<connectivity, Label>::detectImpl(
auto last_label = makeSafeWindow(
labels.row(row), labels.row(row + 1),
image.columns(), image.columns() - 1);
PixelPass::pass(last_img, last_label, label_trees);
PixelPass::pass(last_img, last_label, label_trees, is_background);
}
}

Expand Down
26 changes: 15 additions & 11 deletions nav2_costmap_2d/plugins/denoise_layer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,10 @@

namespace nav2_costmap_2d
{
static constexpr unsigned char OBSTACLE_CELL = 255;
bool isBackground(uint8_t pixel)
{
return pixel != LETHAL_OBSTACLE && pixel != NO_INFORMATION;
}

void
DenoiseLayer::onInitialize()
Expand Down Expand Up @@ -160,7 +163,7 @@ template<ConnectivityType connectivity, class Label>
void removeGroupsImpl(Image<uint8_t> & image, MemoryBuffer & buffer, size_t minimal_group_size)
{
// Creates an image labels in which each obstacles group is labeled with a unique code
auto components = ConnectedComponents<connectivity, Label>::detect(image, buffer);
auto components = ConnectedComponents<connectivity, Label>::detect(image, buffer, isBackground);
const Label groups_count = components.second;
const Image<Label> & labels = components.first;

Expand All @@ -175,18 +178,20 @@ void removeGroupsImpl(Image<uint8_t> & image, MemoryBuffer & buffer, size_t mini
// 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

// lookup_table[i] = OBSTACLE_CELL if groups_sizes[i] >= minimal_group_size, FREE_SPACE in other case
std::vector<uint8_t> lookup_table(groups_sizes.size());
// noise_labels_table[i] = true if group with label i is noise
std::vector<bool> noise_labels_table(groups_sizes.size());
auto transform_fn = [&minimal_group_size](size_t bin_value) {
return bin_value < minimal_group_size ? FREE_SPACE : OBSTACLE_CELL;
return bin_value < minimal_group_size;
};
std::transform(groups_sizes.begin(), groups_sizes.end(), lookup_table.begin(), transform_fn);
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, [&lookup_table](Label src, uint8_t & trg) {
if (trg == OBSTACLE_CELL) { // This check is required for non-binary input image
trg = lookup_table[src];
image, [&noise_labels_table](Label src, uint8_t & trg) {
if (!isBackground(trg) && noise_labels_table[src]) {
trg = 0;
}
});
}
Expand Down Expand Up @@ -225,8 +230,7 @@ DenoiseLayer::removeSinglePixels(Image<uint8_t> & image) const

max_neighbors_image.convert(
image, [this](uint8_t maxNeighbor, uint8_t & img) {
// img == OBSTACLE_CELL is required for non-binary input image
if (maxNeighbor != OBSTACLE_CELL && img == OBSTACLE_CELL) {
if (!isBackground(img) && isBackground(maxNeighbor)) {
img = FREE_SPACE;
}
});
Expand Down
4 changes: 2 additions & 2 deletions nav2_costmap_2d/test/unit/denoise_layer_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,7 @@ TEST_F(DenoiseLayerTester, removeSinglePixelsFromExtremelySmallImage) {

TEST_F(DenoiseLayerTester, removeSinglePixelsFromNonBinary) {
Image<uint8_t> in(3, 3);
in.fill(254);
in.fill(253);
in.at(1, 1) = 255;

Image<uint8_t> exp = in.clone();
Expand Down Expand Up @@ -282,7 +282,7 @@ TEST_F(DenoiseLayerTester, removePixelsGroupFromExtremelySmallImage) {

TEST_F(DenoiseLayerTester, removePixelsGroupFromNonBinary) {
Image<uint8_t> in(3, 3);
in.fill(254);
in.fill(253);
in.at(1, 1) = 255;

Image<uint8_t> exp = in.clone();
Expand Down
44 changes: 33 additions & 11 deletions nav2_costmap_2d/test/unit/image_processing_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#include <gtest/gtest.h>
#include <cmath>

#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_costmap_2d/image_processing.hpp"
#include "image_tests_helper.hpp"

Expand Down Expand Up @@ -118,9 +119,23 @@ struct ConnectedComponentsTester : public ::testing::Test
MemoryBuffer buffer;
};

bool isBackground(uint8_t pixel)
{
return pixel != LETHAL_OBSTACLE && pixel != NO_INFORMATION;
}

TEST_F(ConnectedComponentsTester, smallBufferTest) {
Image<uint8_t> input(3, 3);
MemoryBuffer small_buffer(1);
ASSERT_THROW(
(ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(
input, small_buffer, isBackground)), std::logic_error);
}

TEST_F(ConnectedComponentsTester, way4EmptyTest) {
Image<uint8_t> input;
const auto result = ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(input, buffer);
const auto result = ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(
input, buffer, isBackground);
ASSERT_EQ(result.second, uint8_t(0));
}

Expand All @@ -129,7 +144,8 @@ TEST_F(ConnectedComponentsTester, way4SinglePixelTest) {
Image<uint8_t> input(1, 1);
input.at(0, 0) = 0;

const auto result = ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(input, buffer);
const auto result = ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(
input, buffer, isBackground);

ASSERT_EQ(result.first.at(0, 0), 0);
ASSERT_EQ(result.second, 1);
Expand All @@ -138,7 +154,8 @@ TEST_F(ConnectedComponentsTester, way4SinglePixelTest) {
Image<uint8_t> input(1, 1);
input.at(0, 0) = 255;

const auto result = ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(input, buffer);
const auto result = ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(
input, buffer, isBackground);

ASSERT_EQ(result.first.at(0, 0), 1);
ASSERT_EQ(result.second, 2);
Expand All @@ -151,7 +168,8 @@ TEST_F(ConnectedComponentsTester, way4ImageSmallTest) {
input.at(0, 0) = 0;
input.at(0, 1) = 255;

const auto result = ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(input, buffer);
const auto result = ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(
input, buffer, isBackground);

ASSERT_EQ(result.second, uint8_t(2));
ASSERT_EQ(result.first.at(0, 0), 0);
Expand All @@ -162,7 +180,8 @@ TEST_F(ConnectedComponentsTester, way4ImageSmallTest) {
input.at(0, 0) = 0;
input.at(1, 0) = 255;

const auto result = ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(input, buffer);
const auto result = ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(
input, buffer, isBackground);

ASSERT_EQ(result.second, uint8_t(2));
ASSERT_EQ(result.first.at(0, 0), 0);
Expand All @@ -186,7 +205,7 @@ TEST_F(ConnectedComponentsTester, way4LabelsOverflowTest) {

ASSERT_THROW(
(ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(
input, buffer)),
input, buffer, isBackground)),
std::logic_error);
}

Expand Down Expand Up @@ -234,7 +253,8 @@ TEST_F(ConnectedComponentsTester, way4ImageStepsTest) {
".xx."
"xx.."
"....");
auto result = ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(input, buffer);
auto result = ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(
input, buffer, isBackground);

ASSERT_EQ(result.second, uint8_t(2));
ASSERT_TRUE(isEqualLabels(result.first, expected_labels));
Expand Down Expand Up @@ -267,7 +287,8 @@ TEST_F(ConnectedComponentsTester, way8ImageStepsTest) {
".....b"
"....b.", makeLabelsMap('b'));

auto result = ConnectedComponents<ConnectivityType::Way8, uint8_t>::detect(input, buffer);
auto result = ConnectedComponents<ConnectivityType::Way8, uint8_t>::detect(
input, buffer, isBackground);

ASSERT_EQ(result.second, uint8_t(3));
ASSERT_TRUE(isEqualLabels(result.first, expected_labels));
Expand All @@ -287,7 +308,8 @@ TEST_F(ConnectedComponentsTester, way4ImageSieveTest) {
".i.j."
"k.l.m", makeLabelsMap('m'));

auto result = ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(input, buffer);
auto result = ConnectedComponents<ConnectivityType::Way4, uint8_t>::detect(
input, buffer, isBackground);

ASSERT_EQ(result.second, uint8_t(14));
ASSERT_TRUE(isEqualLabels(result.first, expected_labels));
Expand All @@ -309,7 +331,7 @@ bool fingerTest(MemoryBuffer & buffer)
"a.b.c"
"a.b.c", makeLabelsMap('c'));

auto result = ConnectedComponents<connectivity, uint8_t>::detect(input, buffer);
auto result = ConnectedComponents<connectivity, uint8_t>::detect(input, buffer, isBackground);

return result.second == 4 && isEqualLabels(result.first, expected_labels);
}
Expand Down Expand Up @@ -342,7 +364,7 @@ bool spiralTest(MemoryBuffer & buffer)
".x....x"
".xxxxxx");

auto result = ConnectedComponents<connectivity, uint8_t>::detect(input, buffer);
auto result = ConnectedComponents<connectivity, uint8_t>::detect(input, buffer, isBackground);
return result.second == 2 && isEqualLabels(result.first, expected_labels);
}

Expand Down

0 comments on commit 5d1abd1

Please sign in to comment.