Gazebo Common

API Reference

4.7.0
gz/common/ImageHeightmap.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef IGNITION_COMMON_IMAGEHEIGHTMAPDATA_HH_
18 #define IGNITION_COMMON_IMAGEHEIGHTMAPDATA_HH_
19 
20 #include <limits>
21 #include <string>
22 #include <vector>
23 #include <gz/math/Vector3.hh>
24 
25 #include <gz/common/config.hh>
26 #include <gz/common/graphics/Export.hh>
28 #include <gz/common/Image.hh>
29 
30 namespace ignition
31 {
32  namespace common
33  {
35  class IGNITION_COMMON_GRAPHICS_VISIBLE ImageHeightmap
37  {
40  public: ImageHeightmap();
41 
45  public: int Load(const std::string &_filename = "");
46 
47  // Documentation inherited.
48  public: void FillHeightMap(int _subSampling, unsigned int _vertSize,
49  const ignition::math::Vector3d &_size,
50  const ignition::math::Vector3d &_scale, bool _flipY,
51  std::vector<float> &_heights);
52 
55  public: std::string Filename() const;
56 
57  // Documentation inherited.
58  public: unsigned int Height() const;
59 
60  // Documentation inherited.
61  public: unsigned int Width() const;
62 
63  // Documentation inherited.
64  public: float MaxElevation() const;
65 
67  private: ignition::common::Image img;
68 
79  private: template <typename T>
80  void FillHeights(T *_data, int _imgHeight, int _imgWidth,
81  unsigned int _pitch, int _subSampling, unsigned int _vertSize,
82  const ignition::math::Vector3d &_size,
83  const ignition::math::Vector3d &_scale,
84  bool _flipY, std::vector<float> &_heights)
85  {
86  // bytes per pixel
87  const unsigned int bpp = _pitch / _imgWidth;
88  // number of channels in a pixel
89  const unsigned int channels = bpp / sizeof(T);
90  // number of pixels in a row of image
91  const unsigned int pitchInPixels = _pitch / bpp;
92 
93  const double maxPixelValue =
94  static_cast<double>(std::numeric_limits<T>::max());
95 
96  // Iterate over all the vertices
97  for (unsigned int y = 0; y < _vertSize; ++y)
98  {
99  // yf ranges between 0 and 4
100  const double yf = y / static_cast<double>(_subSampling);
101  const int y1 = static_cast<int>(std::floor(yf));
102  int y2 = static_cast<int>(std::ceil(yf));
103  if (y2 >= _imgHeight)
104  y2 = _imgHeight - 1;
105  const double dy = yf - y1;
106 
107  for (unsigned int x = 0; x < _vertSize; ++x)
108  {
109  const double xf = x / static_cast<double>(_subSampling);
110  const int x1 = static_cast<int>(std::floor(xf));
111  int x2 = static_cast<int>(std::ceil(xf));
112  if (x2 >= _imgWidth)
113  x2 = _imgWidth - 1;
114  const double dx = xf - x1;
115 
116  const double px1 = static_cast<int>(
117  _data[(y1 * pitchInPixels + x1) * channels]) / maxPixelValue;
118  const double px2 = static_cast<int>(
119  _data[(y1 * pitchInPixels + x2) * channels]) / maxPixelValue;
120  const float h1 = (px1 - ((px1 - px2) * dx));
121 
122  const double px3 = static_cast<int>(
123  _data[(y2 * pitchInPixels + x1) * channels]) / maxPixelValue;
124  const double px4 = static_cast<int>(
125  _data[(y2 * pitchInPixels + x2) * channels]) / maxPixelValue;
126  const float h2 = (px3 - ((px3 - px4) * dx));
127  float h = (h1 - ((h1 - h2) * dy)) * _scale.Z();
128 
129  // invert pixel definition so 1=ground, 0=full height,
130  // if the terrain size has a negative z component
131  // this is mainly for backward compatibility
132  if (_size.Z() < 0)
133  h = 1.0 - h;
134 
135  // Store the height for future use
136  if (!_flipY)
137  _heights[y * _vertSize + x] = h;
138  else
139  _heights[(_vertSize - y - 1) * _vertSize + x] = h;
140  }
141  }
142  }
143  };
144  }
145 }
146 #endif
T floor(T... args)
Forward declarations for the common classes.
STL class.
STL class.
T ceil(T... args)
Encapsulates an image that will be interpreted as a heightmap.
Definition: gz/common/ImageHeightmap.hh:35
Encapsulates a generic heightmap data file.
Definition: gz/common/HeightmapData.hh:31
T max(T... args)
Encapsulates an image.
Definition: gz/common/Image.hh:63