Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[ROS 2][grid_map_cv] Fix grid_map_cv conversion 688dc4f #509

Open
wants to merge 1 commit into
base: rolling
Choose a base branch
from
Open

[ROS 2][grid_map_cv] Fix grid_map_cv conversion 688dc4f #509

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 9 additions & 3 deletions grid_map_core/include/grid_map_core/gtest_eigen.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,9 @@ void assertEqual(const M1 & A, const M2 & B, std::string const & message = "")
for (int r = 0; r < A.rows(); r++) {
for (int c = 0; c < A.cols(); c++) {
if (std::isnan(A(r, c))) {
ASSERT_TRUE(std::isnan(B(r, c)));
ASSERT_TRUE(std::isnan(B(r, c))) << message
<< "\nNaN check failed at (" << r << "," << c << ")\n"
<< "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
} else {
ASSERT_EQ(
A(r, c),
Expand Down Expand Up @@ -78,7 +80,9 @@ void assertNear(const M1 & A, const M2 & B, T tolerance, std::string const & mes
for (int r = 0; r < A.rows(); r++) {
for (int c = 0; c < A.cols(); c++) {
if (std::isnan(A(r, c))) {
ASSERT_TRUE(std::isnan(B(r, c)));
ASSERT_TRUE(std::isnan(B(r, c))) << message
<< "\nNaN check failed at (" << r << "," << c << ")\n"
<< "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
} else {
ASSERT_NEAR(
A(r, c), B(r, c),
Expand All @@ -105,7 +109,9 @@ void expectNear(const M1 & A, const M2 & B, T tolerance, std::string const & mes
for (int r = 0; r < A.rows(); r++) {
for (int c = 0; c < A.cols(); c++) {
if (std::isnan(A(r, c))) {
EXPECT_TRUE(std::isnan(B(r, c)));
ASSERT_TRUE(std::isnan(B(r, c))) << message
<< "\nNaN check failed at (" << r << "," << c << ")\n"
<< "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
} else {
EXPECT_NEAR(
A(r, c), B(r, c),
Expand Down
19 changes: 10 additions & 9 deletions grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -115,20 +115,21 @@ class GridMapCvConverter
grid_map::Matrix & data = gridMap[layer];

for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
const Index index(*iterator);
const grid_map::Index gridMapIndex = *iterator;
const grid_map::Index imageIndex = iterator.getUnwrappedIndex();

// Check for alpha layer.
if (hasAlpha) {
const Type_ alpha =
image.at<cv::Vec<Type_, NChannels_>>(index(0), index(1))[NChannels_ - 1];
const Type_ alpha = image.at<cv::Vec<Type_,
NChannels_>>(imageIndex(0), imageIndex(1))[NChannels_ - 1];
if (alpha < alphaTreshold) {continue;}
}

// Compute value.
const Type_ imageValue = imageMono.at<Type_>(index(0), index(1));
const float mapValue = lowerValue + mapValueDifference *
(static_cast<float>(imageValue) / maxImageValue);
data(index(0), index(1)) = mapValue;
const Type_ imageValue = imageMono.at<Type_>(imageIndex(0), imageIndex(1));
const float mapValue =
lowerValue + mapValueDifference * (static_cast<float>(imageValue) / maxImageValue);
data(gridMapIndex(0), gridMapIndex(1)) = mapValue;
}

return true;
Expand Down Expand Up @@ -245,8 +246,8 @@ class GridMapCvConverter

for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
const Index index(*iterator);
if (std::isfinite(data(index(0), index(1)))) {
const float & value = data(index(0), index(1));
const float & value = data(index(0), index(1));
if (std::isfinite(value)) {
const Type_ imageValue =
(Type_) (((value - lowerValue) / (upperValue - lowerValue)) *
static_cast<float>(imageMax));
Expand Down
41 changes: 33 additions & 8 deletions grid_map_cv/test/GridMapCvTest.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,15 +19,29 @@

#include "grid_map_cv/grid_map_cv.hpp"

void replaceNan(grid_map::Matrix & m, const double newValue)
{
for(int r = 0; r < m.rows(); r++) {
for(int c = 0; c < m.cols(); c++) {
if (std::isnan(m(r, c))) {
m(r, c) = newValue;
}
}
}
}

TEST(ImageConversion, roundTrip8UC3)
{
// Create grid map.
grid_map::GridMap mapIn({"layer"});
mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01);
mapIn["layer"].setRandom();
mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.1);
mapIn["layer"].setRandom(); // Sets the layer to random values in [-1.0, 1.0].
mapIn.move(grid_map::Position(0.5, -0.2));
const float minValue = -1.0;
const float maxValue = 1.0;
// When we move `mapIn`, new areas are filled with NaN.
// As `toImage` does not support NaN, we replace NaN with `minValue` instead.
replaceNan(mapIn.get("layer"), minValue);

// Convert to image.
cv::Mat image;
Expand Down Expand Up @@ -55,10 +69,13 @@ TEST(ImageConversion, roundTrip8UC4)
// Create grid map.
grid_map::GridMap mapIn({"layer"});
mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.1);
mapIn["layer"].setRandom();
mapIn["layer"](1, 2) = NAN; // To check for transparnecy/nan handling.
mapIn["layer"].setRandom(); // Sets the layer to random values in [-1.0, 1.0].
mapIn.move(grid_map::Position(0.5, -0.2));
const float minValue = -1.0;
const float maxValue = 1.0;
// When we move `mapIn`, new areas are filled with NaN.
// As `toImage` does not support NaN, we replace NaN with `minValue` instead.
replaceNan(mapIn.get("layer"), minValue);

// Convert to image.
cv::Mat image;
Expand All @@ -85,10 +102,14 @@ TEST(ImageConversion, roundTrip16UC1)
{
// Create grid map.
grid_map::GridMap mapIn({"layer"});
mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01);
mapIn["layer"].setRandom();
mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.1);
mapIn["layer"].setRandom(); // Sets the layer to random values in [-1.0, 1.0].
mapIn.move(grid_map::Position(0.5, -0.2));
const float minValue = -1.0;
const float maxValue = 1.0;
// When we move `mapIn`, new areas are filled with NaN.
// As `toImage` does not support NaN, we replace NaN with `minValue` instead.
replaceNan(mapIn.get("layer"), minValue);

// Convert to image.
cv::Mat image;
Expand All @@ -115,10 +136,14 @@ TEST(ImageConversion, roundTrip32FC1)
{
// Create grid map.
grid_map::GridMap mapIn({"layer"});
mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01);
mapIn["layer"].setRandom();
mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.1);
mapIn["layer"].setRandom(); // Sets the layer to random values in [-1.0, 1.0].
mapIn.move(grid_map::Position(0.5, -0.2));
const float minValue = -1.0;
const float maxValue = 1.0;
// When we move `mapIn`, new areas are filled with NaN.
// As `toImage` does not support NaN, we replace NaN with `minValue` instead.
replaceNan(mapIn.get("layer"), minValue);

// Convert to image.
cv::Mat image;
Expand Down
Loading