12 #include <Eigen/Geometry> 13 #include <opencv2/core/eigen.hpp> 22 const int interpolationAlgorithm) {
23 GridMap gridMapSourceCopy(gridMapSource);
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();
34 result = GridMapCvConverter::toImage<unsigned short, 4>(gridMapSourceCopy, layer, CV_16UC4, minValue, maxValue, imageSource);
36 result = GridMapCvConverter::toImage<unsigned short, 1>(gridMapSourceCopy, layer, CV_16UC1, minValue, maxValue, imageSource);
38 if (!result)
return false;
39 cv::resize(imageSource, imageResult, cv::Size(0.0, 0.0), sizeFactor, sizeFactor, interpolationAlgorithm);
45 result = GridMapCvConverter::addLayerFromImage<unsigned short, 4>(imageResult, layer, gridMapResult, minValue, maxValue);
47 result = GridMapCvConverter::addLayerFromImage<unsigned short, 1>(imageResult, layer, gridMapResult, minValue, maxValue);
49 if (!result)
return false;
58 const std::string& heightLayerName,
const std::string& newFrameId) {
60 if (!gridMapSource.exists(heightLayerName)) {
61 throw std::out_of_range(
"GridMap::getTransformedMap(...) : No map layer '" + heightLayerName +
"' available.");
65 if (std::abs(transform(2, 2) - 1) >= 0.05) {
66 throw std::invalid_argument(
"The given transform is not Z aligned!");
69 auto yawPitchRoll = transform.rotation().eulerAngles(2, 1, 0);
70 double rotationAngle = yawPitchRoll.x() * 180 / CV_PI;
71 if (std::abs(yawPitchRoll.y()) >= 3 && std::abs(yawPitchRoll.z()) >= 3) {
75 gridMapSource.convertToDefaultStartIndex();
78 GridMap transformedMap(gridMapSource.getLayers());
80 transformedMap.setTimestamp(gridMapSource.getTimestamp());
81 transformedMap.setFrameId(newFrameId);
84 cv::Mat imageRotationMatrix;
85 cv::Rect2f boundingBox;
87 bool firstLayer =
true;
88 for (
const auto& layer : gridMapSource.getLayers()) {
89 cv::Mat imageSource, imageResult;
92 cv::eigen2cv(gridMapSource[layer], imageSource);
98 cv::Point2f imageCenter =
99 cv::Point2f((imageSource.cols - 1) / 2.0, (imageSource.rows - 1) / 2.0);
101 imageRotationMatrix = cv::getRotationMatrix2D(imageCenter, rotationAngle, 1.0);
102 boundingBox = cv::RotatedRect(cv::Point2f(0, 0), imageSource.size(), rotationAngle).boundingRect2f();
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;
110 Position3 newCenter = transform *
Position3{(gridMapSource.getPosition().x()), (gridMapSource.getPosition().y()), 0.0};
113 transformedMap.setGeometry({boundingBox.height * gridMapSource.getResolution(), boundingBox.width * gridMapSource.getResolution()},
114 gridMapSource.getResolution(),
Position(newCenter.x(), newCenter.y()));
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);
124 cv::cv2eigen(imageResult, resultLayer);
125 transformedMap.add(layer, resultLayer);
130 transformedMap[heightLayerName] = heightLayer.array() + transform.translation().z();
132 return transformedMap;
virtual ~GridMapCvProcessing()
Time getTimestamp() const
bool getPosition(const Index &index, Position &position) const
void setBasicLayers(const std::vector< std::string > &basicLayers)
const std::string & getFrameId() const
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
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