Merge Master into ROS2

grid_map_core/include/grid_map_core/gtest_eigen.hpp

@@ -40,7 +40,8 @@ namespace grid_map {
  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),B(r,c)) << message << "\nEquality comparison failed at (" << r << "," << c << ")\n"
  << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
@@ -63,7 +64,8 @@ namespace grid_map {
  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),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n"
  << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
@@ -83,7 +85,8 @@ namespace grid_map {
  for(int c = 0; c < A.cols(); c++)
  {
  if (std::isnan(A(r,c))) {
- EXPECT_TRUE(std::isnan(B(r,c)));
+ EXPECT_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),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n"
  << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;

grid_map_core/include/grid_map_core/iterators/LineIterator.hpp

@@ -109,10 +109,10 @@ class LineIterator
  Index end_;
 
  //! Current cell number.
- unsigned int iCell_;
+ unsigned int iCell_ = 0;
 
  //! Number of cells in the line.
- unsigned int nCells_;
+ unsigned int nCells_ = 0;
 
  //! Helper variables for Bresenham Line Drawing algorithm.
  Size increment1_, increment2_;

grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp

@@ -100,18 +100,19 @@ 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 Type_ imageValue = imageMono.at<Type_>(imageIndex(0), imageIndex(1));
  const float mapValue = lowerValue + mapValueDifference * ((float) imageValue / maxImageValue);
- data(index(0), index(1)) = mapValue;
+ data(gridMapIndex(0), gridMapIndex(1)) = mapValue;
  }
 
  return true;
@@ -225,9 +226,9 @@ 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 Type_ imageValue = (Type_) (((value - lowerValue) / (upperValue - lowerValue)) * (float) imageMax);
+ const float& value = data(index(0), index(1));
+ if (std::isfinite(value)) {
+ const Type_ imageValue = (Type_)(((value - lowerValue) / (upperValue - lowerValue)) * (float)imageMax);
  const Index imageIndex(iterator.getUnwrappedIndex());
  unsigned int channel = 0;
  image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[channel] = imageValue;

grid_map_cv/test/GridMapCvTest.cpp

@@ -20,14 +20,30 @@
 using namespace std;
 using namespace grid_map;
 
+void replaceNan(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.
  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(Position(0.5, -0.2));
  const float minValue = -1.0;
  const float maxValue = 1.0;
+ replaceNan(mapIn.get("layer"), minValue); // When we move `mapIn`, new areas are filled with NaN. As `toImage` does not support NaN, we replace NaN with `minValue` instead.
 
  // Convert to image.
  cv::Mat image;
@@ -50,10 +66,11 @@ TEST(ImageConversion, roundTrip8UC4)
  // Create 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(Position(0.5, -0.2));
  const float minValue = -1.0;
  const float maxValue = 1.0;
+ replaceNan(mapIn.get("layer"), minValue); // When we move `mapIn`, new areas are filled with NaN. As `toImage` does not support NaN, we replace NaN with `minValue` instead.
 
  // Convert to image.
  cv::Mat image;
@@ -75,10 +92,12 @@ TEST(ImageConversion, roundTrip16UC1)
 {
  // Create 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(Position(0.5, -0.2));
  const float minValue = -1.0;
  const float maxValue = 1.0;
+ replaceNan(mapIn.get("layer"), minValue); // When we move `mapIn`, new areas are filled with NaN. As `toImage` does not support NaN, we replace NaN with `minValue` instead.
 
  // Convert to image.
  cv::Mat image;
@@ -100,10 +119,12 @@ TEST(ImageConversion, roundTrip32FC1)
 {
  // Create 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(Position(0.5, -0.2));
  const float minValue = -1.0;
  const float maxValue = 1.0;
+ replaceNan(mapIn.get("layer"), minValue); // When we move `mapIn`, new areas are filled with NaN. As `toImage` does not support NaN, we replace NaN with `minValue` instead.
 
  // Convert to image.
  cv::Mat image;