Skip to content

Commit

Permalink
Fix build and lint errors
Browse files Browse the repository at this point in the history
  • Loading branch information
kjalloul-anybotics committed Nov 22, 2024
1 parent fa009a7 commit 99cd204
Show file tree
Hide file tree
Showing 5 changed files with 143 additions and 86 deletions.
10 changes: 6 additions & 4 deletions grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -119,13 +119,15 @@ class GridMapCvConverter

// Check for alpha layer.
if (hasAlpha) {
const Type_ alpha = image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[NChannels_ - 1];
if (alpha < alphaTreshold) continue;
const Type_ alpha = image.at<cv::Vec<Type_,
NChannels_>>(imageIndex(0), imageIndex(1))[NChannels_ - 1];
if (alpha < alphaTreshold) {continue;}
}

// Compute value.
const Type_ imageValue = imageMono.at<Type_>(imageIndex(0), imageIndex(1));
const float mapValue = lowerValue + mapValueDifference * ((float) imageValue / maxImageValue);
const float mapValue =
lowerValue + mapValueDifference * (static_cast<float>(imageValue) / maxImageValue);
data(gridMapIndex(0), gridMapIndex(1)) = mapValue;
}

Expand Down Expand Up @@ -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)) *
Expand Down
9 changes: 7 additions & 2 deletions grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,8 @@

#pragma once

#include <string>

#include <grid_map_core/grid_map_core.hpp>

// OpenCV
Expand Down Expand Up @@ -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
33 changes: 22 additions & 11 deletions grid_map_cv/src/GridMapCvProcessing.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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;
}

Expand All @@ -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.
Expand All @@ -124,28 +131,32 @@ 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
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};
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<double>::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;
Expand Down
Loading

0 comments on commit 99cd204

Please sign in to comment.