From 98fe5f65306b3ab50cb4f7c3ab03e88c8cd06679 Mon Sep 17 00:00:00 2001 From: Khaled Jalloul Date: Wed, 4 Dec 2024 08:25:08 +0100 Subject: [PATCH] Merge branch 'fix/grid_map_cv_conversion' into 'master' [grid_map_cv] Fix open cv to moved grid map conversion. GitOrigin-RevId: 50016100e952afe7990ecf6fea2489ee99baf039 --- .../include/grid_map_core/gtest_eigen.hpp | 12 ++++-- .../grid_map_cv/GridMapCvConverter.hpp | 19 +++++---- grid_map_cv/test/GridMapCvTest.cpp | 41 +++++++++++++++---- 3 files changed, 52 insertions(+), 20 deletions(-) diff --git a/grid_map_core/include/grid_map_core/gtest_eigen.hpp b/grid_map_core/include/grid_map_core/gtest_eigen.hpp index f7599fd44..7b19f3f47 100644 --- a/grid_map_core/include/grid_map_core/gtest_eigen.hpp +++ b/grid_map_core/include/grid_map_core/gtest_eigen.hpp @@ -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), @@ -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), @@ -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), diff --git a/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp index 233c28a0d..484657544 100644 --- a/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp +++ b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp @@ -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>(index(0), index(1))[NChannels_ - 1]; + const Type_ alpha = image.at>(imageIndex(0), imageIndex(1))[NChannels_ - 1]; if (alpha < alphaTreshold) {continue;} } // Compute value. - const Type_ imageValue = imageMono.at(index(0), index(1)); - const float mapValue = lowerValue + mapValueDifference * - (static_cast(imageValue) / maxImageValue); - data(index(0), index(1)) = mapValue; + const Type_ imageValue = imageMono.at(imageIndex(0), imageIndex(1)); + const float mapValue = + lowerValue + mapValueDifference * (static_cast(imageValue) / maxImageValue); + data(gridMapIndex(0), gridMapIndex(1)) = mapValue; } return true; @@ -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(imageMax)); diff --git a/grid_map_cv/test/GridMapCvTest.cpp b/grid_map_cv/test/GridMapCvTest.cpp index 5205d2d1c..7475a3f75 100644 --- a/grid_map_cv/test/GridMapCvTest.cpp +++ b/grid_map_cv/test/GridMapCvTest.cpp @@ -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; @@ -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; @@ -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; @@ -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;