From 99cd204c63fc858b05d0810301e730cd51124267 Mon Sep 17 00:00:00 2001 From: Khaled Jalloul Date: Fri, 22 Nov 2024 11:10:36 +0100 Subject: [PATCH] Fix build and lint errors --- .../grid_map_cv/GridMapCvConverter.hpp | 10 +- .../grid_map_cv/GridMapCvProcessing.hpp | 9 +- grid_map_cv/src/GridMapCvProcessing.cpp | 33 +++-- grid_map_cv/test/GridMapCvProcessingTest.cpp | 133 ++++++++++++------ grid_map_cv/test/GridMapCvTest.cpp | 44 +++--- 5 files changed, 143 insertions(+), 86 deletions(-) diff --git a/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp index 32e5d31b..ecc33593 100644 --- a/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp +++ b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp @@ -119,13 +119,15 @@ class GridMapCvConverter // Check for alpha layer. if (hasAlpha) { - const Type_ alpha = image.at>(imageIndex(0), imageIndex(1))[NChannels_ - 1]; - if (alpha < alphaTreshold) continue; + const Type_ alpha = image.at>(imageIndex(0), imageIndex(1))[NChannels_ - 1]; + if (alpha < alphaTreshold) {continue;} } // Compute value. const Type_ imageValue = imageMono.at(imageIndex(0), imageIndex(1)); - const float mapValue = lowerValue + mapValueDifference * ((float) imageValue / maxImageValue); + const float mapValue = + lowerValue + mapValueDifference * (static_cast(imageValue) / maxImageValue); data(gridMapIndex(0), gridMapIndex(1)) = mapValue; } @@ -243,7 +245,7 @@ class GridMapCvConverter for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { const Index index(*iterator); - 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)) * diff --git a/grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp b/grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp index fc344066..f90849a3 100644 --- a/grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp +++ b/grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp @@ -8,6 +8,8 @@ #pragma once +#include + #include // OpenCV @@ -63,8 +65,11 @@ class GridMapCvProcessing * @throw std::out_of_range if no map layer with name `heightLayerName` is present. * @throw std::invalid_argument if the transform is not approximately z-aligned. */ - static GridMap getTransformedMap(GridMap&& gridMapSource, const Eigen::Isometry3d& transform, const std::string& heightLayerName, - const std::string& newFrameId); + static GridMap getTransformedMap( + GridMap && gridMapSource, + const Eigen::Isometry3d & transform, + const std::string & heightLayerName, + const std::string & newFrameId); }; } // namespace grid_map diff --git a/grid_map_cv/src/GridMapCvProcessing.cpp b/grid_map_cv/src/GridMapCvProcessing.cpp index e1f0a19c..fb917b38 100644 --- a/grid_map_cv/src/GridMapCvProcessing.cpp +++ b/grid_map_cv/src/GridMapCvProcessing.cpp @@ -82,11 +82,16 @@ bool GridMapCvProcessing::changeResolution( return true; } -GridMap GridMapCvProcessing::getTransformedMap(GridMap&& gridMapSource, const Eigen::Isometry3d& transform, - const std::string& heightLayerName, const std::string& newFrameId) { +GridMap GridMapCvProcessing::getTransformedMap( + GridMap && gridMapSource, + const Eigen::Isometry3d & transform, + const std::string & heightLayerName, + const std::string & newFrameId) +{ // Check if height layer is valid. if (!gridMapSource.exists(heightLayerName)) { - throw std::out_of_range("GridMap::getTransformedMap(...) : No map layer '" + heightLayerName + "' available."); + throw std::out_of_range("GridMap::getTransformedMap(...) : No map layer '" + + heightLayerName + "' available."); } // Check if transformation is z aligned. @@ -96,7 +101,9 @@ GridMap GridMapCvProcessing::getTransformedMap(GridMap&& gridMapSource, const Ei auto yawPitchRoll = transform.rotation().eulerAngles(2, 1, 0); // Double check convention! double rotationAngle = yawPitchRoll.x() * 180 / CV_PI; - if (std::abs(yawPitchRoll.y()) >= 3 && std::abs(yawPitchRoll.z()) >= 3) { // Resolve yaw ambiguity in euler angles. + if (std::abs(yawPitchRoll.y()) >= 3 && + std::abs(yawPitchRoll.z()) >= 3) + { // Resolve yaw ambiguity in euler angles. rotationAngle += 180; } @@ -113,7 +120,7 @@ GridMap GridMapCvProcessing::getTransformedMap(GridMap&& gridMapSource, const Ei cv::Rect2f boundingBox; bool firstLayer = true; - for (const auto& layer : gridMapSource.getLayers()) { + for (const auto & layer : gridMapSource.getLayers()) { cv::Mat imageSource, imageResult; // From gridMap to openCV image. Assumes defaultStartIndex. @@ -124,10 +131,11 @@ GridMap GridMapCvProcessing::getTransformedMap(GridMap&& gridMapSource, const Ei // Get rotation matrix for rotating the image around its center in pixel coordinates. See // https://answers.opencv.org/question/82708/rotation-center-confusion/ cv::Point2f imageCenter = - cv::Point2f((imageSource.cols - 1) / 2.0, (imageSource.rows - 1) / 2.0); + cv::Point2f((imageSource.cols - 1) / 2.0, (imageSource.rows - 1) / 2.0); imageRotationMatrix = cv::getRotationMatrix2D(imageCenter, rotationAngle, 1.0); - boundingBox = cv::RotatedRect(cv::Point2f(0, 0), imageSource.size(), rotationAngle).boundingRect2f(); + boundingBox = cv::RotatedRect(cv::Point2f(0, 0), + imageSource.size(), rotationAngle).boundingRect2f(); // Adjust transformation matrix. See // https://docs.opencv.org/3.4/da/d54/group__imgproc__transform.html#gafbbc470ce83812914a70abfb604f4326 and https://stackoverflow.com/questions/22041699/rotate-an-image-without-cropping-in-opencv-in-c @@ -135,17 +143,20 @@ GridMap GridMapCvProcessing::getTransformedMap(GridMap&& gridMapSource, const Ei imageRotationMatrix.at(1, 2) += boundingBox.height / 2.0 - imageSource.rows / 2.0; // Calculate the new center of the gridMap. - Position3 newCenter = transform * Position3{(gridMapSource.getPosition().x()), (gridMapSource.getPosition().y()), 0.0}; + Position3 newCenter = transform * Position3{(gridMapSource.getPosition().x()), + (gridMapSource.getPosition().y()), 0.0}; // Set the size of the rotated gridMap. - transformedMap.setGeometry({boundingBox.height * gridMapSource.getResolution(), boundingBox.width * gridMapSource.getResolution()}, - gridMapSource.getResolution(), Position(newCenter.x(), newCenter.y())); + transformedMap.setGeometry({boundingBox.height * gridMapSource.getResolution(), + boundingBox.width * gridMapSource.getResolution()}, + gridMapSource.getResolution(), Position(newCenter.x(), newCenter.y())); firstLayer = false; } // Rotate the layer. imageResult = cv::Mat(boundingBox.size(), CV_32F, std::numeric_limits::quiet_NaN()); - cv::warpAffine(imageSource, imageResult, imageRotationMatrix, boundingBox.size(), cv::INTER_NEAREST, cv::BORDER_TRANSPARENT); + cv::warpAffine(imageSource, imageResult, imageRotationMatrix, + boundingBox.size(), cv::INTER_NEAREST, cv::BORDER_TRANSPARENT); // Copy result into gridMapLayer. Assumes default start index. Matrix resultLayer; diff --git a/grid_map_cv/test/GridMapCvProcessingTest.cpp b/grid_map_cv/test/GridMapCvProcessingTest.cpp index efbaf96d..df0178e3 100644 --- a/grid_map_cv/test/GridMapCvProcessingTest.cpp +++ b/grid_map_cv/test/GridMapCvProcessingTest.cpp @@ -17,9 +17,6 @@ #include "grid_map_cv/grid_map_cv.hpp" -using namespace std; -using namespace grid_map; - TEST(GridMapCvProcessing, changeResolution) { // Create grid map. @@ -87,19 +84,24 @@ TEST(GridMapCvProcessing, changeResolutionForMovedMap) */ TEST(GridMap, TransformRotate90) { // Initial map. - GridMap map; + grid_map::GridMap map; const auto heightLayerName = "height"; - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0)); + map.setGeometry(grid_map::Length(1.0, 2.0), 0.1, grid_map::Position(0.0, 0.0)); map.add(heightLayerName, 0.0); map.setBasicLayers(map.getLayers()); map.get(heightLayerName)(0, 0) = 1.0; // Transformation (90° rotation). - Eigen::Isometry3d transform = Eigen::Translation3d(0, 0, 0) * Eigen::AngleAxisd(M_PI / 2, Vector3::UnitZ()); + Eigen::Isometry3d transform = + Eigen::Translation3d(0, 0, 0) * Eigen::AngleAxisd(M_PI / 2, grid_map::Vector3::UnitZ()); // Apply affine transformation. - const GridMap transformedMap = GridMapCvProcessing::getTransformedMap(GridMap(map), transform, heightLayerName, map.getFrameId()); + const grid_map::GridMap transformedMap = + grid_map::GridMapCvProcessing::getTransformedMap(grid_map::GridMap(map), + transform, + heightLayerName, + map.getFrameId()); // Check if map has been rotated by 90° about z // Check dimensions. @@ -131,19 +133,24 @@ TEST(GridMap, TransformRotate90) { */ TEST(GridMap, TransformRotate180) { // Initial map. - GridMap map; + grid_map::GridMap map; const auto heightLayerName = "height"; - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0)); + map.setGeometry(grid_map::Length(1.0, 2.0), 0.1, grid_map::Position(0.0, 0.0)); map.add(heightLayerName, 0.0); map.setBasicLayers(map.getLayers()); map.get(heightLayerName)(0, 0) = 1.0; // Transformation (180° rotation). - Eigen::Isometry3d transform = Eigen::Translation3d(0, 0, 0) * Eigen::AngleAxisd(M_PI, Vector3::UnitZ()); + Eigen::Isometry3d transform = + Eigen::Translation3d(0, 0, 0) * Eigen::AngleAxisd(M_PI, grid_map::Vector3::UnitZ()); // Apply affine transformation. - const GridMap transformedMap = GridMapCvProcessing::getTransformedMap(GridMap(map), transform, heightLayerName, map.getFrameId()); + const grid_map::GridMap transformedMap = + grid_map::GridMapCvProcessing::getTransformedMap(grid_map::GridMap(map), + transform, + heightLayerName, + map.getFrameId()); // Check if map has been rotated by 180° about z @@ -176,19 +183,24 @@ TEST(GridMap, TransformRotate180) { */ TEST(GridMap, TransformRotate270) { // Initial map. - GridMap map; + grid_map::GridMap map; const auto heightLayerName = "height"; - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0)); + map.setGeometry(grid_map::Length(1.0, 2.0), 0.1, grid_map::Position(0.0, 0.0)); map.add(heightLayerName, 0.0); map.setBasicLayers(map.getLayers()); map.get(heightLayerName)(0, 0) = 1.0; // Transformation (270° rotation). - Eigen::Isometry3d transform = Eigen::Translation3d(0, 0, 0) * Eigen::AngleAxisd(-M_PI / 2, Vector3::UnitZ()); + Eigen::Isometry3d transform = + Eigen::Translation3d(0, 0, 0) * Eigen::AngleAxisd(-M_PI / 2, grid_map::Vector3::UnitZ()); // Apply affine transformation. - const GridMap transformedMap = GridMapCvProcessing::getTransformedMap(GridMap(map), transform, heightLayerName, map.getFrameId()); + const grid_map::GridMap transformedMap = + grid_map::GridMapCvProcessing::getTransformedMap(grid_map::GridMap(map), + transform, + heightLayerName, + map.getFrameId()); // Check if map has been rotated by 270° about z @@ -222,19 +234,24 @@ TEST(GridMap, TransformRotate270) { */ TEST(GridMap, TransformRotate360) { // Initial map. - GridMap map; + grid_map::GridMap map; const auto heightLayerName = "height"; - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0)); + map.setGeometry(grid_map::Length(1.0, 2.0), 0.1, grid_map::Position(0.0, 0.0)); map.add(heightLayerName, 0.0); map.setBasicLayers(map.getLayers()); map.get(heightLayerName)(0, 0) = 1.0; // Transformation (360° rotation). - Eigen::Isometry3d transform = Eigen::Translation3d(0, 0, 0) * Eigen::AngleAxisd(M_PI * 2, Vector3::UnitZ()); + Eigen::Isometry3d transform = + Eigen::Translation3d(0, 0, 0) * Eigen::AngleAxisd(M_PI * 2, grid_map::Vector3::UnitZ()); // Apply affine transformation. - const GridMap transformedMap = GridMapCvProcessing::getTransformedMap(GridMap(map), transform, heightLayerName, map.getFrameId()); + const grid_map::GridMap transformedMap = + grid_map::GridMapCvProcessing::getTransformedMap(grid_map::GridMap(map), + transform, + heightLayerName, + map.getFrameId()); // Check if map has been rotated by 360° about z @@ -276,18 +293,23 @@ TEST(GridMap, TransformRotate360) { */ TEST(GridMap, TransformRotate45) { // Initial map. - GridMap map; + grid_map::GridMap map; const auto heightLayerName = "height"; double resolution = 0.02; - map.setGeometry(Length(1.0, 2.0), resolution, Position(0.0, 0.0)); + map.setGeometry(grid_map::Length(1.0, 2.0), resolution, grid_map::Position(0.0, 0.0)); map.add(heightLayerName, 1.0); map.setBasicLayers(map.getLayers()); // Transformation (45° rotation). - Eigen::Isometry3d transform = Eigen::Translation3d(0, 0, 0) * Eigen::AngleAxisd(M_PI / 4, Vector3::UnitZ()); + Eigen::Isometry3d transform = + Eigen::Translation3d(0, 0, 0) * Eigen::AngleAxisd(M_PI / 4, grid_map::Vector3::UnitZ()); // Apply affine transformation. - const GridMap transformedMap = GridMapCvProcessing::getTransformedMap(GridMap(map), transform, heightLayerName, map.getFrameId()); + const grid_map::GridMap transformedMap = + grid_map::GridMapCvProcessing::getTransformedMap(grid_map::GridMap(map), + transform, + heightLayerName, + map.getFrameId()); // Check if map has been rotated by 45° about z @@ -297,24 +319,30 @@ TEST(GridMap, TransformRotate45) { // Check that filled area stays the same. EXPECT_TRUE(transformedMap.get(heightLayerName).hasNaN()); - EXPECT_NEAR(transformedMap.get(heightLayerName).sumOfFinites() / map.get(heightLayerName).size(), 1.0, 1e-2); + EXPECT_NEAR(transformedMap.get(heightLayerName).sumOfFinites() / + map.get(heightLayerName).size(), 1.0, 1e-2); } TEST(GridMap, TransformTranslationXZY) { // Initial map. - GridMap map; + grid_map::GridMap map; const auto heightLayerName = "height"; const auto otherLayerName = "other_layer"; - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0)); + map.setGeometry(grid_map::Length(1.0, 2.0), 0.1, grid_map::Position(0.0, 0.0)); map.add(heightLayerName, 0.0); map.add(otherLayerName, 0.0); map.setBasicLayers(map.getLayers()); // Translation by (1, -0.5, 0.2): - Eigen::Isometry3d transform = Eigen::Translation3d(1, -0.5, 0.2) * Eigen::AngleAxisd(0, Vector3::UnitZ()); + Eigen::Isometry3d transform = + Eigen::Translation3d(1, -0.5, 0.2) * Eigen::AngleAxisd(0, grid_map::Vector3::UnitZ()); // Apply affine transformation. - const GridMap transformedMap = GridMapCvProcessing::getTransformedMap(GridMap(map), transform, heightLayerName, map.getFrameId()); + const grid_map::GridMap transformedMap = + grid_map::GridMapCvProcessing::getTransformedMap(grid_map::GridMap(map), + transform, + heightLayerName, + map.getFrameId()); // Check if map has still the same size EXPECT_NEAR(map.getLength().x(), transformedMap.getLength().x(), 1e-6); @@ -326,7 +354,8 @@ TEST(GridMap, TransformTranslationXZY) { EXPECT_NEAR(transformedMap.getPosition().y(), transform.translation().y(), 1e-6); // Check if height values were updated. - EXPECT_NEAR(map.get(heightLayerName)(0, 0) + transform.translation().z(), transformedMap.get(heightLayerName)(0, 0), 1e-6); + EXPECT_NEAR(map.get(heightLayerName)(0, 0) + transform.translation().z(), + transformedMap.get(heightLayerName)(0, 0), 1e-6); // Check that other layers were kept as before. EXPECT_NEAR(map.get(otherLayerName)(0, 0), transformedMap.get(otherLayerName)(0, 0), 1e-6); @@ -334,18 +363,22 @@ TEST(GridMap, TransformTranslationXZY) { TEST(GridMap, TransformUnaligned) { // Initial map. - GridMap map; + grid_map::GridMap map; const auto heightLayerName = "height"; - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0)); + map.setGeometry(grid_map::Length(1.0, 2.0), 0.1, grid_map::Position(0.0, 0.0)); map.add(heightLayerName, 0.0); map.setBasicLayers(map.getLayers()); // A small pitch rotation by 22.5° - Eigen::Isometry3d transform = Eigen::Translation3d(0, 0, 0) * Eigen::AngleAxisd(M_PI / 8, Vector3::UnitY()); + Eigen::Isometry3d transform = + Eigen::Translation3d(0, 0, 0) * Eigen::AngleAxisd(M_PI / 8, grid_map::Vector3::UnitY()); // Check that this unaligned transformation is not accepted. - ASSERT_ANY_THROW(const GridMap transformedMap = - GridMapCvProcessing::getTransformedMap(GridMap(map), transform, heightLayerName, map.getFrameId())); + ASSERT_ANY_THROW(const grid_map::GridMap transformedMap = + grid_map::GridMapCvProcessing::getTransformedMap(grid_map::GridMap(map), + transform, + heightLayerName, + map.getFrameId())); } /** @@ -355,33 +388,39 @@ TEST(GridMap, TransformUnaligned) { * --------------------------- * * Transforming a 2 x 2 m GridMap with a resolution of 2 cm by 45° took: - * avg: 1.03322ms, min: 0.946455ms, max 1.51534ms + * avg: 1.03322ms, min: 0.946455ms, max 1.51534ms */ TEST(GridMap, TransformComputationTime) { // Initial map. - GridMap map; + grid_map::GridMap map; const auto heightLayerName = "height"; - map.setGeometry(Length(4.0, 4.0), 0.02, Position(0.0, 0.0)); + map.setGeometry(grid_map::Length(4.0, 4.0), 0.02, grid_map::Position(0.0, 0.0)); map.add(heightLayerName, 0.0); map.setBasicLayers(map.getLayers()); // Translation by (1, -0.5, 0.2): - Eigen::Isometry3d transform = Eigen::Translation3d(1, -0.5, 0.2) * Eigen::AngleAxisd(M_PI / 4, Vector3::UnitZ()); + Eigen::Isometry3d transform = + Eigen::Translation3d(1, -0.5, 0.2) * Eigen::AngleAxisd(M_PI / 4, grid_map::Vector3::UnitZ()); // Setup timing metrics. - long sumTimes = 0; - long averageTime = 0; - long minTime = std::numeric_limits::max(); - long maxTime = std::numeric_limits::lowest(); + int64_t sumTimes = 0; + int64_t averageTime = 0; + int64_t minTime = std::numeric_limits::max(); + int64_t maxTime = std::numeric_limits::lowest(); // Benchmark the function. size_t numberOfSamples = 100; for (size_t sampleNumber = 0; sampleNumber < numberOfSamples; sampleNumber++) { auto timestampBefore = std::chrono::high_resolution_clock::now(); - const GridMap transformedMap = GridMapCvProcessing::getTransformedMap(GridMap(map), transform, heightLayerName, map.getFrameId()); + const grid_map::GridMap transformedMap = + grid_map::GridMapCvProcessing::getTransformedMap(grid_map::GridMap(map), + transform, + heightLayerName, + map.getFrameId()); auto timestampAfter = std::chrono::high_resolution_clock::now(); - long time = std::chrono::duration_cast(timestampAfter - timestampBefore).count(); + int64_t time = std::chrono::duration_cast(timestampAfter - + timestampBefore).count(); sumTimes += time; if (time < minTime) { minTime = time; @@ -392,6 +431,8 @@ TEST(GridMap, TransformComputationTime) { } averageTime = sumTimes / numberOfSamples; - std::cout << "Transforming a 2 x 2 m GridMap with a resolution of 2 cm by 45° took: " << std::endl - << "avg: " << averageTime * 1e-6f << "ms, \tmin: " << minTime * 1e-6f << "ms, \tmax " << maxTime * 1e-6f << "ms." << std::endl; + std::cout << "Transforming a 2 x 2 m GridMap with a resolution of 2 cm by 45° took: " + << std::endl + << "avg: " << averageTime * 1e-6f << "ms, \tmin: " << minTime * 1e-6f << "ms, \tmax " + << maxTime * 1e-6f << "ms." << std::endl; } diff --git a/grid_map_cv/test/GridMapCvTest.cpp b/grid_map_cv/test/GridMapCvTest.cpp index 617469b3..de291e03 100644 --- a/grid_map_cv/test/GridMapCvTest.cpp +++ b/grid_map_cv/test/GridMapCvTest.cpp @@ -19,18 +19,12 @@ #include "grid_map_cv/grid_map_cv.hpp" -using namespace std; -using namespace grid_map; - -void replaceNan(Matrix& m, const double newValue) +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; + 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; } } } @@ -41,11 +35,12 @@ TEST(ImageConversion, roundTrip8UC3) // Create grid map. grid_map::GridMap mapIn({"layer"}); 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)); + 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; - 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. + 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; @@ -73,11 +68,12 @@ 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(); // Sets the layer to random values in [-1.0, 1.0]. - mapIn.move(Position(0.5, -0.2)); + 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; - 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. + 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; @@ -105,11 +101,12 @@ TEST(ImageConversion, roundTrip16UC1) // Create grid map. grid_map::GridMap mapIn({"layer"}); 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)); + 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; - 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. + 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; @@ -137,11 +134,12 @@ TEST(ImageConversion, roundTrip32FC1) // Create grid map. grid_map::GridMap mapIn({"layer"}); 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)); + 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; - 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. + 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;