Skip to content

Commit abcafb1

Browse files
author
Peter Fankhauser
committed
Added new convenience function to change the resolution of grid maps with help of OpenCV interpolation methods.
1 parent 7850a0a commit abcafb1

14 files changed

+244
-25
lines changed

‎README.md

+4
Original file line numberDiff line numberDiff line change
@@ -139,6 +139,10 @@ The *grid_map_demos* package contains several demonstration nodes. Use this code
139139

140140
![OpenCV demo result](grid_map_demos/doc/opencv_demo_result.gif)
141141

142+
* *[resolution_change_demo](grid_map_demos/src/resolution_change_demo_node.cpp)* shows how the resolution of a grid map can be changed with help of the [OpenCV] image scaling methods. The see the results, use
143+
144+
roslaunch grid_map_demos resolution_change_demo.launch
145+
142146

143147
### Conventions & Definitions
144148

‎grid_map_cv/CMakeLists.txt

+24-1
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ catkin_package(
2323
INCLUDE_DIRS
2424
include
2525
LIBRARIES
26+
${PROJECT_NAME}
2627
CATKIN_DEPENDS
2728
grid_map_core
2829
DEPENDS
@@ -41,10 +42,32 @@ include_directories(
4142
${OpenCV_INCLUDE_DIRS}
4243
)
4344

45+
## Declare a cpp library
46+
add_library(${PROJECT_NAME}
47+
src/GridMapCvProcessing.cpp
48+
)
49+
50+
target_link_libraries(${PROJECT_NAME}
51+
${catkin_LIBRARIES}
52+
${OpenCV_LIBRARIES}
53+
)
54+
55+
add_dependencies(${PROJECT_NAME}
56+
${catkin_EXPORTED_TARGETS}
57+
)
58+
4459
#############
4560
## Install ##
4661
#############
4762

63+
# Mark executables and/or libraries for installation
64+
install(
65+
TARGETS ${PROJECT_NAME}
66+
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
67+
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
68+
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
69+
)
70+
4871
# Mark cpp header files for installation
4972
install(
5073
DIRECTORY include/${PROJECT_NAME}/
@@ -60,5 +83,5 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
6083
## Add gtest based cpp test target and link libraries
6184
catkin_add_gtest(${PROJECT_NAME}-test test/test_grid_map_cv.cpp test/GridMapCvTest.cpp)
6285
if(TARGET ${PROJECT_NAME}-test)
63-
target_link_libraries(${PROJECT_NAME}-test ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
86+
target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
6487
endif()

‎grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp

+4-9
Original file line numberDiff line numberDiff line change
@@ -32,26 +32,21 @@ class GridMapCvConverter
3232
virtual ~GridMapCvConverter();
3333

3434
/*!
35-
* Initializes a the geometry of a grid map from a cv image. This changes
35+
* Initializes the geometry of a grid map from an image. This changes
3636
* the geometry of the map and deletes all contents of the layers!
37-
* @param[in] image the cv image.
37+
* @param[in] image the image.
3838
* @param[in] resolution the desired resolution of the grid map [m/cell].
3939
* @param[out] gridMap the grid map to be initialized.
40-
* @param[in] (optional) position of the grid map, default to [0,0], [m].
41-
* @param[in] (optional) frame id of the grid map, default is "map".
42-
* @param[in] (optional) timestamp.
40+
* @param[in](optional) position the position of the grid map.
4341
* @return true if successful, false otherwise.
4442
*/
4543
static bool initializeFromImage(const cv::Mat& image, const double resolution,
46-
grid_map::GridMap& gridMap, const grid_map::Position& position = grid_map::Position::Zero(),
47-
const std::string frameId = "map", const Time timestamp = ros::Time::now().toNSec())
44+
grid_map::GridMap& gridMap, const grid_map::Position& position)
4845
{
4946
const double lengthX = resolution * image.rows;
5047
const double lengthY = resolution * image.cols;
5148
Length length(lengthX, lengthY);
5249
gridMap.setGeometry(length, resolution, position);
53-
gridMap.setFrameId(frameId);
54-
gridMap.setTimestamp(timestamp);
5550
return true;
5651
}
5752

‎grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp

+41
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
/*
2+
* GridMapCvProcessing.hpp
3+
*
4+
* Created on: Apr 15, 2016
5+
* Author: Péter Fankhauser
6+
* Institute: ETH Zurich, Autonomous Systems Lab
7+
*/
8+
9+
#pragma once
10+
11+
#include <grid_map_core/grid_map_core.hpp>
12+
13+
// OpenCV
14+
#include <opencv/cv.h>
15+
16+
namespace grid_map {
17+
18+
/*!
19+
* Processing of grid maps with OpenCV methods.
20+
*/
21+
class GridMapCvProcessing
22+
{
23+
public:
24+
/*!
25+
* Default constructor.
26+
*/
27+
GridMapCvProcessing();
28+
29+
/*!
30+
* Destructor.
31+
*/
32+
virtual ~GridMapCvProcessing();
33+
34+
static bool changeResolution(const grid_map::GridMap& gridMapSource,
35+
grid_map::GridMap& gridMapResult,
36+
const double resolution,
37+
const int interpolationAlgrithm = cv::INTER_CUBIC);
38+
39+
};
40+
41+
} /* namespace */

‎grid_map_cv/include/grid_map_cv/grid_map_cv.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -10,3 +10,4 @@
1010

1111
#include <grid_map_core/grid_map_core.hpp>
1212
#include <grid_map_cv/GridMapCvConverter.hpp>
13+
#include <grid_map_cv/GridMapCvProcessing.hpp>

‎grid_map_cv/src/GridMapCvProcessing.cpp

+61
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
/*
2+
* GridMapCvProcessing.cpp
3+
*
4+
* Created on: Apr 15, 2016
5+
* Author: Péter Fankhauser
6+
* Institute: ETH Zurich, Autonomous Systems Lab
7+
*/
8+
9+
#include "grid_map_cv/GridMapCvProcessing.hpp"
10+
#include "grid_map_cv/GridMapCvConverter.hpp"
11+
12+
namespace grid_map {
13+
14+
GridMapCvProcessing::GridMapCvProcessing()
15+
{
16+
}
17+
18+
GridMapCvProcessing::~GridMapCvProcessing()
19+
{
20+
}
21+
22+
bool GridMapCvProcessing::changeResolution(const grid_map::GridMap& gridMapSource,
23+
grid_map::GridMap& gridMapResult,
24+
const double resolution,
25+
const int interpolationAlgrithm)
26+
{
27+
const double sizeFactor = gridMapSource.getResolution() / resolution;
28+
bool firstLayer = true;
29+
for (const auto& layer : gridMapSource.getLayers()) {
30+
cv::Mat imageSource, imageResult;
31+
const float minValue = gridMapSource.get(layer).minCoeffOfFinites();
32+
const float maxValue = gridMapSource.get(layer).maxCoeffOfFinites();
33+
const bool hasNaN = gridMapSource.get(layer).hasNaN();
34+
bool result;
35+
if (hasNaN) {
36+
result = GridMapCvConverter::toImage<unsigned short, 4>(gridMapSource, layer, CV_16UC4, minValue, maxValue, imageSource);
37+
} else {
38+
result = GridMapCvConverter::toImage<unsigned short, 1>(gridMapSource, layer, CV_16UC1, minValue, maxValue, imageSource);
39+
}
40+
if (!result) return false;
41+
cv::resize(imageSource, imageResult, cv::Size(0.0, 0.0), sizeFactor, sizeFactor, interpolationAlgrithm);
42+
if (firstLayer) {
43+
if (!GridMapCvConverter::initializeFromImage(imageResult, resolution, gridMapResult, gridMapSource.getPosition()))
44+
return false;
45+
firstLayer = false;
46+
}
47+
if (hasNaN) {
48+
result = GridMapCvConverter::addLayerFromImage<unsigned short, 4>(imageResult, layer, gridMapResult, minValue, maxValue);
49+
} else {
50+
result = GridMapCvConverter::addLayerFromImage<unsigned short, 1>(imageResult, layer, gridMapResult, minValue, maxValue);
51+
}
52+
if (!result) return false;
53+
}
54+
gridMapResult.setFrameId(gridMapSource.getFrameId());
55+
gridMapResult.setTimestamp(gridMapSource.getTimestamp());
56+
gridMapResult.setBasicLayers(gridMapSource.getBasicLayers());
57+
gridMapResult.setStartIndex(gridMapSource.getStartIndex());
58+
return true;
59+
}
60+
61+
} /* namespace */

‎grid_map_demos/CMakeLists.txt

+11-1
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,11 @@ add_executable(
8383
src/opencv_demo_node.cpp
8484
)
8585

86+
add_executable(
87+
resolution_change_demo
88+
src/resolution_change_demo_node.cpp
89+
)
90+
8691
## Specify libraries to link a library or executable target against
8792
target_link_libraries(
8893
simple_demo
@@ -120,6 +125,11 @@ target_link_libraries(
120125
${OpenCV_LIBRARIES}
121126
)
122127

128+
target_link_libraries(
129+
resolution_change_demo
130+
${catkin_LIBRARIES}
131+
)
132+
123133
#############
124134
## Install ##
125135
#############
@@ -131,7 +141,7 @@ catkin_install_python(
131141

132142
# Mark executables and/or libraries for installation
133143
install(
134-
TARGETS simple_demo tutorial_demo iterators_demo image_to_gridmap_demo move_demo iterator_benchmark opencv_demo
144+
TARGETS simple_demo tutorial_demo iterators_demo image_to_gridmap_demo move_demo iterator_benchmark opencv_demo resolution_change_demo
135145
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
136146
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
137147
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

‎grid_map_demos/config/resolution_change_demo.yaml

+8
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
grid_map_topic: /grid_map_resolution_change_demo/grid_map
2+
grid_map_visualizations:
3+
- name: occupancy_grid
4+
type: occupancy_grid
5+
params:
6+
layer: elevation
7+
data_min: 0.0
8+
data_max: 0.3

‎grid_map_demos/launch/resolution_change_demo.launch

+10
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
<launch>
2+
<!-- Launch the grid map resolution change demo node -->
3+
<node pkg="grid_map_demos" type="resolution_change_demo" name="grid_map_resolution_change_demo" output="screen" />
4+
<!-- Launch the grid map visualizer -->
5+
<node pkg="grid_map_visualization" type="grid_map_visualization" name="grid_map_visualization" output="screen">
6+
<rosparam command="load" file="$(find grid_map_demos)/config/resolution_change_demo.yaml" />
7+
</node>
8+
<!-- Launch RViz with the demo configuration -->
9+
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find grid_map_demos)/rviz/grid_map_iterators_demo.rviz" />
10+
</launch>

‎grid_map_demos/src/opencv_demo_node.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ int main(int argc, char** argv)
1414
init(argc, argv, "opencv_demo");
1515
NodeHandle nodeHandle("~");
1616
Publisher publisher = nodeHandle.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
17-
const bool useTransparency = true;
17+
const bool useTransparency = false;
1818

1919
// Create grid map.
2020
GridMap map({"original", "elevation"});

‎grid_map_demos/src/resolution_change_demo_node.cpp

+66
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
#include <ros/ros.h>
2+
#include <grid_map_ros/grid_map_ros.hpp>
3+
#include <grid_map_cv/grid_map_cv.hpp>
4+
5+
using namespace grid_map;
6+
using namespace ros;
7+
8+
int main(int argc, char** argv)
9+
{
10+
// Initialize node and publisher.
11+
init(argc, argv, "resolution_change_demo");
12+
NodeHandle nodeHandle("~");
13+
Publisher publisher = nodeHandle.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
14+
15+
// Create grid map.
16+
GridMap map({"elevation"});
17+
map.setFrameId("map");
18+
map.setGeometry(Length(1.2, 2.0), 0.03);
19+
ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
20+
map.getLength().x(), map.getLength().y(),
21+
map.getSize()(0), map.getSize()(1));
22+
23+
// Add data.
24+
map["elevation"].setZero(); // Un/comment this line to try with and without transparency.
25+
grid_map::Polygon polygon;
26+
polygon.setFrameId(map.getFrameId());
27+
polygon.addVertex(Position( 0.480, 0.000));
28+
polygon.addVertex(Position( 0.164, 0.155));
29+
polygon.addVertex(Position( 0.116, 0.500));
30+
polygon.addVertex(Position(-0.133, 0.250));
31+
polygon.addVertex(Position(-0.480, 0.399));
32+
polygon.addVertex(Position(-0.316, 0.000));
33+
polygon.addVertex(Position(-0.480, -0.399));
34+
polygon.addVertex(Position(-0.133, -0.250));
35+
polygon.addVertex(Position( 0.116, -0.500));
36+
polygon.addVertex(Position( 0.164, -0.155));
37+
polygon.addVertex(Position( 0.480, 0.000));
38+
39+
for (grid_map::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); ++iterator) {
40+
map.at("elevation", *iterator) = 0.3;
41+
}
42+
43+
Rate rate(10.0);
44+
45+
// Work in a loop.
46+
while (nodeHandle.ok()) {
47+
48+
// Initialize.
49+
ros::Time time = ros::Time::now();
50+
const double resolution = 0.05 + 0.04 * sin(time.toSec());
51+
52+
// Change resoltion of grid map.
53+
GridMap modifiedMap;
54+
GridMapCvProcessing::changeResolution(map, modifiedMap, resolution);
55+
56+
// Publish grid map.
57+
grid_map_msgs::GridMap message;
58+
GridMapRosConverter::toMessage(modifiedMap, message);
59+
publisher.publish(message);
60+
ROS_INFO_STREAM("Published grid map with " << resolution << " m/cell resolution.");
61+
62+
rate.sleep();
63+
}
64+
65+
return 0;
66+
}

‎grid_map_ros/CMakeLists.txt

+1-1
Original file line numberDiff line numberDiff line change
@@ -102,5 +102,5 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
102102
## Add gtest based cpp test target and link libraries
103103
catkin_add_gtest(${PROJECT_NAME}-test test/test_grid_map_ros.cpp test/GridMapRosTest.cpp)
104104
if(TARGET ${PROJECT_NAME}-test)
105-
target_link_libraries(${PROJECT_NAME}-test grid_map_ros)
105+
target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
106106
endif()

‎grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -132,15 +132,17 @@ class GridMapRosConverter
132132
nav_msgs::GridCells& gridCells);
133133

134134
/*!
135-
* Initializes a the geometry of a grid map from an image messages. This changes
135+
* Initializes the geometry of a grid map from an image messages. This changes
136136
* the geometry of the map and deletes all contents of the layers!
137137
* @param[in] image the image.
138138
* @param[in] resolution the desired resolution of the grid map [m/cell].
139139
* @param[out] gridMap the grid map to be initialized.
140+
* @param[in](optional) position the position of the grid map.
140141
* @return true if successful, false otherwise.
141142
*/
142143
static bool initializeFromImage(const sensor_msgs::Image& image, const double resolution,
143-
grid_map::GridMap& gridMap, const grid_map::Position& position = grid_map::Position::Zero());
144+
grid_map::GridMap& gridMap,
145+
const grid_map::Position& position = grid_map::Position::Zero());
144146

145147
/*!
146148
* Adds a layer with data from image.

‎grid_map_ros/src/GridMapRosConverter.cpp

+8-10
Original file line numberDiff line numberDiff line change
@@ -309,17 +309,15 @@ void GridMapRosConverter::toGridCells(const grid_map::GridMap& gridMap, const st
309309
}
310310

311311
bool GridMapRosConverter::initializeFromImage(const sensor_msgs::Image& image,
312-
const double resolution, grid_map::GridMap& gridMap, const grid_map::Position& position)
312+
const double resolution, grid_map::GridMap& gridMap,
313+
const grid_map::Position& position)
313314
{
314-
cv_bridge::CvImageConstPtr cvImage;
315-
try {
316-
// TODO Use `toCvShared()`?
317-
cvImage = cv_bridge::toCvCopy(image, image.encoding);
318-
} catch (cv_bridge::Exception& e) {
319-
ROS_ERROR("cv_bridge exception: %s", e.what());
320-
return false;
321-
}
322-
GridMapCvConverter::initializeFromImage(cvImage->image,resolution,gridMap,position,image.header.frame_id,image.header.stamp.toNSec());
315+
const double lengthX = resolution * image.height;
316+
const double lengthY = resolution * image.width;
317+
Length length(lengthX, lengthY);
318+
gridMap.setGeometry(length, resolution, position);
319+
gridMap.setFrameId(image.header.frame_id);
320+
gridMap.setTimestamp(image.header.stamp.toNSec());
323321
return true;
324322
}
325323

0 commit comments

Comments
 (0)