GridMapCvProcessing.cpp
Go to the documentation of this file.
1 /*
2  * GridMapCvProcessing.cpp
3  *
4  * Created on: Apr 15, 2016
5  * Author: Péter Fankhauser, Magnus Gärtner
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
11 
12 #include <Eigen/Geometry>
13 #include <opencv2/core/eigen.hpp>
14 
15 namespace grid_map {
16 
18 
20 
21 bool GridMapCvProcessing::changeResolution(const GridMap& gridMapSource, GridMap& gridMapResult, const double resolution,
22  const int interpolationAlgorithm) {
23  GridMap gridMapSourceCopy(gridMapSource);
24  gridMapSourceCopy.convertToDefaultStartIndex();
25  const double sizeFactor = gridMapSourceCopy.getResolution() / resolution;
26  bool firstLayer = true;
27  for (const auto& layer : gridMapSourceCopy.getLayers()) {
28  cv::Mat imageSource, imageResult;
29  const float minValue = gridMapSourceCopy.get(layer).minCoeffOfFinites();
30  const float maxValue = gridMapSourceCopy.get(layer).maxCoeffOfFinites();
31  const bool hasNaN = gridMapSourceCopy.get(layer).hasNaN();
32  bool result;
33  if (hasNaN) {
34  result = GridMapCvConverter::toImage<unsigned short, 4>(gridMapSourceCopy, layer, CV_16UC4, minValue, maxValue, imageSource);
35  } else {
36  result = GridMapCvConverter::toImage<unsigned short, 1>(gridMapSourceCopy, layer, CV_16UC1, minValue, maxValue, imageSource);
37  }
38  if (!result) return false;
39  cv::resize(imageSource, imageResult, cv::Size(0.0, 0.0), sizeFactor, sizeFactor, interpolationAlgorithm);
40  if (firstLayer) {
41  if (!GridMapCvConverter::initializeFromImage(imageResult, resolution, gridMapResult, gridMapSourceCopy.getPosition())) return false;
42  firstLayer = false;
43  }
44  if (hasNaN) {
45  result = GridMapCvConverter::addLayerFromImage<unsigned short, 4>(imageResult, layer, gridMapResult, minValue, maxValue);
46  } else {
47  result = GridMapCvConverter::addLayerFromImage<unsigned short, 1>(imageResult, layer, gridMapResult, minValue, maxValue);
48  }
49  if (!result) return false;
50  }
51  gridMapResult.setFrameId(gridMapSourceCopy.getFrameId());
52  gridMapResult.setTimestamp(gridMapSourceCopy.getTimestamp());
53  gridMapResult.setBasicLayers(gridMapSourceCopy.getBasicLayers());
54  return true;
55 }
56 
57 GridMap GridMapCvProcessing::getTransformedMap(GridMap&& gridMapSource, const Eigen::Isometry3d& transform,
58  const std::string& heightLayerName, const std::string& newFrameId) {
59  // Check if height layer is valid.
60  if (!gridMapSource.exists(heightLayerName)) {
61  throw std::out_of_range("GridMap::getTransformedMap(...) : No map layer '" + heightLayerName + "' available.");
62  }
63 
64  // Check if transformation is z aligned.
65  if (std::abs(transform(2, 2) - 1) >= 0.05) {
66  throw std::invalid_argument("The given transform is not Z aligned!");
67  }
68 
69  auto yawPitchRoll = transform.rotation().eulerAngles(2, 1, 0); // Double check convention!
70  double rotationAngle = yawPitchRoll.x() * 180 / CV_PI;
71  if (std::abs(yawPitchRoll.y()) >= 3 && std::abs(yawPitchRoll.z()) >= 3) { // Resolve yaw ambiguity in euler angles.
72  rotationAngle += 180;
73  }
74 
75  gridMapSource.convertToDefaultStartIndex();
76 
77  // Create the rotated gridMap.
78  GridMap transformedMap(gridMapSource.getLayers());
79  transformedMap.setBasicLayers(gridMapSource.getBasicLayers());
80  transformedMap.setTimestamp(gridMapSource.getTimestamp());
81  transformedMap.setFrameId(newFrameId);
82 
83  // openCV rotation parameters, initalized on first layer.
84  cv::Mat imageRotationMatrix;
85  cv::Rect2f boundingBox;
86 
87  bool firstLayer = true;
88  for (const auto& layer : gridMapSource.getLayers()) {
89  cv::Mat imageSource, imageResult;
90 
91  // From gridMap to openCV image. Assumes defaultStartIndex.
92  cv::eigen2cv(gridMapSource[layer], imageSource);
93 
94  // Calculate transformation matrix and update geometry of the resulting grid map.
95  if (firstLayer) {
96  // Get rotation matrix for rotating the image around its center in pixel coordinates. See
97  // https://answers.opencv.org/question/82708/rotation-center-confusion/
98  cv::Point2f imageCenter =
99  cv::Point2f((imageSource.cols - 1) / 2.0, (imageSource.rows - 1) / 2.0);
100 
101  imageRotationMatrix = cv::getRotationMatrix2D(imageCenter, rotationAngle, 1.0);
102  boundingBox = cv::RotatedRect(cv::Point2f(0, 0), imageSource.size(), rotationAngle).boundingRect2f();
103 
104  // Adjust transformation matrix. See
105  // 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
106  imageRotationMatrix.at<double>(0, 2) += boundingBox.width / 2.0 - imageSource.cols / 2.0;
107  imageRotationMatrix.at<double>(1, 2) += boundingBox.height / 2.0 - imageSource.rows / 2.0;
108 
109  // Calculate the new center of the gridMap.
110  Position3 newCenter = transform * Position3{(gridMapSource.getPosition().x()), (gridMapSource.getPosition().y()), 0.0};
111 
112  // Set the size of the rotated gridMap.
113  transformedMap.setGeometry({boundingBox.height * gridMapSource.getResolution(), boundingBox.width * gridMapSource.getResolution()},
114  gridMapSource.getResolution(), Position(newCenter.x(), newCenter.y()));
115  firstLayer = false;
116  }
117 
118  // Rotate the layer.
119  imageResult = cv::Mat(boundingBox.size(), CV_32F, std::numeric_limits<double>::quiet_NaN());
120  cv::warpAffine(imageSource, imageResult, imageRotationMatrix, boundingBox.size(), cv::INTER_NEAREST, cv::BORDER_TRANSPARENT);
121 
122  // Copy result into gridMapLayer. Assumes default start index.
123  Matrix resultLayer;
124  cv::cv2eigen(imageResult, resultLayer);
125  transformedMap.add(layer, resultLayer);
126  }
127 
128  // Add height translation.
129  grid_map::Matrix heightLayer = transformedMap[heightLayerName];
130  transformedMap[heightLayerName] = heightLayer.array() + transform.translation().z();
131 
132  return transformedMap;
133 }
134 } // namespace grid_map
Time getTimestamp() const
bool getPosition(const Index &index, Position &position) const
void setBasicLayers(const std::vector< std::string > &basicLayers)
const std::string & getFrameId() const
Eigen::MatrixXf Matrix
static bool initializeFromImage(const cv::Mat &image, const double resolution, grid_map::GridMap &gridMap, const grid_map::Position &position)
void convertToDefaultStartIndex()
Eigen::Vector3d Position3
double getResolution() const
const std::vector< std::string > & getLayers() const
Eigen::Vector2d Position
const Matrix & get(const std::string &layer) const
static bool changeResolution(const GridMap &gridMapSource, GridMap &gridMapResult, const double resolution, const int interpolationAlgorithm=cv::INTER_CUBIC)
static GridMap getTransformedMap(GridMap &&gridMapSource, const Eigen::Isometry3d &transform, const std::string &heightLayerName, const std::string &newFrameId)
void setFrameId(const std::string &frameId)
void setTimestamp(const Time timestamp)
const std::vector< std::string > & getBasicLayers() const


grid_map_cv
Author(s): Péter Fankhauser , Magnus Gärtner
autogenerated on Tue Jun 1 2021 02:13:32