[ROS 2][grid_map_cv] Add getTransformedMap 007163c

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

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 <string>
+
 #include <grid_map_core/grid_map_core.hpp>
 
 // 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

grid_map_cv/package.xml

@@ -10,6 +10,7 @@
   <url type="website">http://github.com/anybotics/grid_map</url>
   <url type="bugtracker">http://github.com/anybotics/grid_map/issues</url>
   <author email="[email protected]">Péter Fankhauser</author>
+  <author email="[email protected]">Magnus Gärtner</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>grid_map_cmake_helpers</build_depend>

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 <Eigen/Geometry>
+#include <opencv2/core/eigen.hpp>
+
 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<double>(0, 2) += boundingBox.width / 2.0 - imageSource.cols / 2.0;
+      imageRotationMatrix.at<double>(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<double>::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