Merge branch 'fix/grid_map_cv_conversion' into 'master'

[grid_map_cv] Fix open cv to moved grid map conversion.

GitOrigin-RevId: 50016100e952afe7990ecf6fea2489ee99baf039

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),

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<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;
@@ -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));

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;