From 383219d4e9879129ed9cbe4f57c303888dde2252 Mon Sep 17 00:00:00 2001 From: Khaled Jalloul Date: Wed, 4 Dec 2024 07:45:48 +0100 Subject: [PATCH] Merge branch 'feature/optimize_get_transformed_map' into 'master' [grid_map] Optimize get transformed map Closes #UNKNOWN GitOrigin-RevId: c468aac9cdbae996e989a62d89603e09cad89140 --- grid_map_cv/CHANGELOG.rst | 4 + .../grid_map_cv/GridMapCvProcessing.hpp | 27 +- grid_map_cv/package.xml | 1 + grid_map_cv/src/GridMapCvProcessing.cpp | 93 ++++- grid_map_cv/test/GridMapCvProcessingTest.cpp | 368 ++++++++++++++++++ 5 files changed, 491 insertions(+), 2 deletions(-) diff --git a/grid_map_cv/CHANGELOG.rst b/grid_map_cv/CHANGELOG.rst index 1adb26428..add3d0382 100644 --- a/grid_map_cv/CHANGELOG.rst +++ b/grid_map_cv/CHANGELOG.rst @@ -36,6 +36,10 @@ Changelog for package grid_map_cv * Initial ROS2 port * Contributors: Maximilian Wulf, Steve Macenski +1.6.3 (2020-09-30) +------------------ +* Added getTransformedMap. + 1.6.2 (2019-10-14) ------------------ diff --git a/grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp b/grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp index 137d5c77d..e3ca79b46 100644 --- a/grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp +++ b/grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp @@ -2,13 +2,15 @@ * GridMapCvProcessing.hpp * * Created on: Apr 15, 2016 - * Author: Péter Fankhauser + * Author: Péter Fankhauser, Magnus Gärtner * Institute: ETH Zurich, ANYbotics */ #ifndef GRID_MAP_CV__GRIDMAPCVPROCESSING_HPP_ #define GRID_MAP_CV__GRIDMAPCVPROCESSING_HPP_ +#include + #include // OpenCV @@ -46,6 +48,29 @@ class GridMapCvProcessing grid_map::GridMap & gridMapResult, const double resolution, const int interpolationAlgorithm = cv::INTER_CUBIC); + + /*! + * Apply isometric transformation (rotation + offset) to grid map and returns the transformed map. + * Note: The returned map may not have the same length since it's geometric description contains + * the original map. + * Also note that the map treats the height layer differently from other layers. I.e, the translation in z-direction is applied only to + * the height layer. For layers other than the height layer that contain geometric information this function might compute unexpected + * results. E.g transforming the layers normals_x/y/z will only represent the same values at the transformed location but will not + * transform the normal vectors into the new coordinate system. + * + * @param[consume] gridMapSource The map to transform. + * @param[in] transform the requested transformation to apply. + * @param[in] heightLayerName the height layer of the map. + * @param[in] newFrameId frame index of the new map. + * @return transformed map. + * @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); }; } // namespace grid_map diff --git a/grid_map_cv/package.xml b/grid_map_cv/package.xml index 267f16027..f553c57ee 100644 --- a/grid_map_cv/package.xml +++ b/grid_map_cv/package.xml @@ -10,6 +10,7 @@ http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Péter Fankhauser + Magnus Gärtner ament_cmake grid_map_cmake_helpers diff --git a/grid_map_cv/src/GridMapCvProcessing.cpp b/grid_map_cv/src/GridMapCvProcessing.cpp index 746e09196..fb917b387 100644 --- a/grid_map_cv/src/GridMapCvProcessing.cpp +++ b/grid_map_cv/src/GridMapCvProcessing.cpp @@ -2,13 +2,16 @@ * GridMapCvProcessing.cpp * * Created on: Apr 15, 2016 - * Author: Péter Fankhauser + * Author: Péter Fankhauser, Magnus Gärtner * Institute: ETH Zurich, ANYbotics */ #include "grid_map_cv/GridMapCvProcessing.hpp" #include "grid_map_cv/GridMapCvConverter.hpp" +#include +#include + namespace grid_map { @@ -79,4 +82,92 @@ bool GridMapCvProcessing::changeResolution( return true; } +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."); + } + + // Check if transformation is z aligned. + if (std::abs(transform(2, 2) - 1) >= 0.05) { + throw std::invalid_argument("The given transform is not Z aligned!"); + } + + 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. + rotationAngle += 180; + } + + gridMapSource.convertToDefaultStartIndex(); + + // Create the rotated gridMap. + GridMap transformedMap(gridMapSource.getLayers()); + transformedMap.setBasicLayers(gridMapSource.getBasicLayers()); + transformedMap.setTimestamp(gridMapSource.getTimestamp()); + transformedMap.setFrameId(newFrameId); + + // openCV rotation parameters, initalized on first layer. + cv::Mat imageRotationMatrix; + cv::Rect2f boundingBox; + + bool firstLayer = true; + for (const auto & layer : gridMapSource.getLayers()) { + cv::Mat imageSource, imageResult; + + // From gridMap to openCV image. Assumes defaultStartIndex. + cv::eigen2cv(gridMapSource[layer], imageSource); + + // Calculate transformation matrix and update geometry of the resulting grid map. + if (firstLayer) { + // 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); + + imageRotationMatrix = cv::getRotationMatrix2D(imageCenter, rotationAngle, 1.0); + 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 + imageRotationMatrix.at(0, 2) += boundingBox.width / 2.0 - imageSource.cols / 2.0; + 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}; + + // Set the size of the rotated gridMap. + 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); + + // Copy result into gridMapLayer. Assumes default start index. + Matrix resultLayer; + cv::cv2eigen(imageResult, resultLayer); + transformedMap.add(layer, resultLayer); + } + + // Add height translation. + grid_map::Matrix heightLayer = transformedMap[heightLayerName]; + transformedMap[heightLayerName] = heightLayer.array() + transform.translation().z(); + + return transformedMap; +} } // namespace grid_map diff --git a/grid_map_cv/test/GridMapCvProcessingTest.cpp b/grid_map_cv/test/GridMapCvProcessingTest.cpp index 261856ad3..ab5c94021 100644 --- a/grid_map_cv/test/GridMapCvProcessingTest.cpp +++ b/grid_map_cv/test/GridMapCvProcessingTest.cpp @@ -67,3 +67,371 @@ TEST(GridMapCvProcessing, changeResolutionForMovedMap) mapIn.atPosition("layer", mapIn.getPosition()), mapOut.atPosition("layer", mapOut.getPosition())); // Center. } + +/** + * We check that the marker we set in the top left corner is rotated accordingly. + * + * Rotate by 90° counter-clockwise. + * +------+ + * |1 | + * | | +------------+ + * | | | | + * | | +---> |1 | + * | | +------------+ + * | | + * +------+ + */ +TEST(GridMap, TransformRotate90) { + // Initial map. + grid_map::GridMap map; + const auto heightLayerName = "height"; + + 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, grid_map::Vector3::UnitZ()); + + // Apply affine transformation. + 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. + EXPECT_NEAR(map.getLength().x(), transformedMap.getLength().y(), 1e-6); + EXPECT_NEAR(map.getLength().y(), transformedMap.getLength().x(), 1e-6); + EXPECT_EQ(map.get(heightLayerName).size(), transformedMap.get(heightLayerName).size()); + + // Check if marker was rotated. + EXPECT_DOUBLE_EQ(map.get(heightLayerName)(0, 0), transformedMap.get(heightLayerName)(19, 0)); + EXPECT_DOUBLE_EQ(transformedMap.get(heightLayerName)(19, 0), 1.0); + + // Check map position. + EXPECT_DOUBLE_EQ(map.getPosition().x(), transformedMap.getPosition().x()); + EXPECT_DOUBLE_EQ(map.getPosition().y(), transformedMap.getPosition().y()); +} + +/** + * We check that the marker we set in the top left corner is rotated accordingly. + * + * Rotate by 180° counter-clockwise. + * +------+ +------+ + * |1 | | | + * | | | | + * | | | | + * | | +---> | | + * | | | | + * | | | 1| + * +------+ +------+ + */ +TEST(GridMap, TransformRotate180) { + // Initial map. + grid_map::GridMap map; + const auto heightLayerName = "height"; + + 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, grid_map::Vector3::UnitZ()); + + // Apply affine transformation. + 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 + + // Check dimensions. + EXPECT_NEAR(map.getLength().x(), transformedMap.getLength().x(), 1e-6); + EXPECT_NEAR(map.getLength().y(), transformedMap.getLength().y(), 1e-6); + EXPECT_EQ(map.get(heightLayerName).size(), transformedMap.get(heightLayerName).size()); + + // Check if marker was rotated. + EXPECT_DOUBLE_EQ(map.get(heightLayerName)(0, 0), transformedMap.get(heightLayerName)(9, 19)); + EXPECT_DOUBLE_EQ(transformedMap.get(heightLayerName)(9, 19), 1.0); + + // Check map position. + EXPECT_DOUBLE_EQ(map.getPosition().x(), transformedMap.getPosition().x()); + EXPECT_DOUBLE_EQ(map.getPosition().y(), transformedMap.getPosition().y()); +} + +/** + * We check that the marker we set in the top left corner is rotated accordingly. + * + * Rotate by 270° counter-clockwise / 90° clockwise. + * +------+ + * |1 | + * | | +------------+ + * | | | 1| + * | | +---> | | + * | | +------------+ + * | | + * +------+ + */ +TEST(GridMap, TransformRotate270) { + // Initial map. + grid_map::GridMap map; + const auto heightLayerName = "height"; + + 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, grid_map::Vector3::UnitZ()); + + // Apply affine transformation. + 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 + + // Check dimensions. + EXPECT_NEAR(map.getLength().x(), transformedMap.getLength().y(), 1e-6); + EXPECT_NEAR(map.getLength().y(), transformedMap.getLength().x(), 1e-6); + EXPECT_EQ(map.get(heightLayerName).size(), transformedMap.get(heightLayerName).size()); + + // Check if marker was rotated. + EXPECT_DOUBLE_EQ(map.get(heightLayerName)(0, 0), transformedMap.get(heightLayerName)(0, 9)); + EXPECT_DOUBLE_EQ(transformedMap.get(heightLayerName)(0, 9), 1.0); + + // Check map position. + EXPECT_DOUBLE_EQ(map.getPosition().x(), transformedMap.getPosition().x()); + EXPECT_DOUBLE_EQ(map.getPosition().y(), transformedMap.getPosition().y()); +} + + +/** + * We check that the marker we set in the top left corner is rotated accordingly. + * + * Rotate by 36° counter-clockwise. + * +------+ +------+ + * |1 | |1 | + * | | | | + * | | | | + * | | +---> | | + * | | | | + * | | | | + * +------+ +------+ + */ +TEST(GridMap, TransformRotate360) { + // Initial map. + grid_map::GridMap map; + const auto heightLayerName = "height"; + + 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, grid_map::Vector3::UnitZ()); + + // Apply affine transformation. + 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 + + // Check dimensions + EXPECT_NEAR(map.getLength().x(), transformedMap.getLength().x(), 1e-6); + EXPECT_NEAR(map.getLength().y(), transformedMap.getLength().y(), 1e-6); + EXPECT_EQ(map.get(heightLayerName).size(), transformedMap.get(heightLayerName).size()); + + // Check if marker was rotated. + EXPECT_DOUBLE_EQ(map.get(heightLayerName)(0, 0), transformedMap.get(heightLayerName)(0, 0)); + EXPECT_DOUBLE_EQ(transformedMap.get(heightLayerName)(0, 0), 1.0); + + // Check map position. + EXPECT_DOUBLE_EQ(map.getPosition().x(), transformedMap.getPosition().x()); + EXPECT_DOUBLE_EQ(map.getPosition().y(), transformedMap.getPosition().y()); +} + +/** + * Expected Result: + * ---------------- + * + * map rotate by 45° transformedMap + * + * + * +---------------------+ + * +--------+ | XXX | + * |XXXXXXXX| | XXXXXXX | + * |XXXXXXXX| | XXXXXXXXXXXX | + * |XXXXXXXX| | XXXXXXXXXXXXXXX | + * |XXXXXXXX| |XXXXXXXXXXXXXXXXX | + * 2m |XXXXXXXX| +-----> | XXXXXXXXXXXXXXXXXX | sin(45°) * 1m + cos(45°) * 2m + * |XXXXXXXX| | XXXXXXXXXXXXXXXXXX| + * |XXXXXXXX| | XXXXXXXXXXXXXXX| = 2.121m + * |XXXXXXXX| | XXXXXXXXXXXXX| + * |XXXXXXXX| | XXXXXXXX | + * +--------+ | XXX | + * 1m +---------------------+ + * 2.121m + */ +TEST(GridMap, TransformRotate45) { + // Initial map. + grid_map::GridMap map; + const auto heightLayerName = "height"; + double resolution = 0.02; + 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, grid_map::Vector3::UnitZ()); + + // Apply affine transformation. + 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 + + // Check dimensions. + EXPECT_NEAR(transformedMap.getLength().x(), 2.121, resolution); + EXPECT_NEAR(transformedMap.getLength().y(), 2.121, resolution); + + // 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); +} + +TEST(GridMap, TransformTranslationXZY) { + // Initial map. + grid_map::GridMap map; + const auto heightLayerName = "height"; + const auto otherLayerName = "other_layer"; + 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, grid_map::Vector3::UnitZ()); + + // Apply affine transformation. + 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); + EXPECT_NEAR(map.getLength().y(), transformedMap.getLength().y(), 1e-6); + EXPECT_EQ(map.get(heightLayerName).size(), transformedMap.get(heightLayerName).size()); + + // Check if position was updated. + EXPECT_NEAR(transformedMap.getPosition().x(), transform.translation().x(), 1e-6); + 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); + + // Check that other layers were kept as before. + EXPECT_NEAR(map.get(otherLayerName)(0, 0), transformedMap.get(otherLayerName)(0, 0), 1e-6); +} + +TEST(GridMap, TransformUnaligned) { + // Initial map. + grid_map::GridMap map; + const auto heightLayerName = "height"; + 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, grid_map::Vector3::UnitY()); + + // Check that this unaligned transformation is not accepted. + ASSERT_ANY_THROW(const grid_map::GridMap transformedMap = + grid_map::GridMapCvProcessing::getTransformedMap(grid_map::GridMap(map), + transform, + heightLayerName, + map.getFrameId())); +} + +/** + * Make sure to run this flag with optimization flags enabled. This test always succeeds. + * + * Local output on 01.10.2020: + * --------------------------- + * + * 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 + */ +TEST(GridMap, TransformComputationTime) { + // Initial map. + grid_map::GridMap map; + const auto heightLayerName = "height"; + 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, grid_map::Vector3::UnitZ()); + + // Setup timing metrics. + 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 grid_map::GridMap transformedMap = + grid_map::GridMapCvProcessing::getTransformedMap(grid_map::GridMap(map), + transform, + heightLayerName, + map.getFrameId()); + auto timestampAfter = std::chrono::high_resolution_clock::now(); + + int64_t time = std::chrono::duration_cast(timestampAfter - + timestampBefore).count(); + sumTimes += time; + if (time < minTime) { + minTime = time; + } + if (time > maxTime) { + maxTime = time; + } + } + 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; +}