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 #include <algorithm>
18
19 #ifdef HAVE_GDAL
20 # pragma GCC diagnostic push
21 # pragma GCC diagnostic ignored "-Wfloat-equal"
22 # include <ogr_spatialref.h>
23 # pragma GCC diagnostic pop
24 #endif
25
26 #include "ignition/common/Console.hh"
27 #include "ignition/common/Dem.hh"
28 #include "ignition/math/SphericalCoordinates.hh"
29
30 using namespace ignition;
31 using namespace common;
32
33 #ifdef HAVE_GDAL
34
35 class ignition::common::DemPrivate
36 {
37 /// \brief A set of associated raster bands.
38 public: GDALDataset *dataSet;
39
40 /// \brief A pointer to the band.
41 public: GDALRasterBand *band;
42
43 /// \brief Real width of the world in meters.
44 public: double worldWidth;
45
46 /// \brief Real height of the world in meters.
47 public: double worldHeight;
48
49 /// \brief Terrain's side (after the padding).
50 public: unsigned int side;
51
52 /// \brief Minimum elevation in meters.
53 public: double minElevation;
54
55 /// \brief Maximum elevation in meters.
56 public: double maxElevation;
57
58 /// \brief DEM data converted to be OGRE-compatible.
59 public: std::vector<float> demData;
60 };
61
62 //////////////////////////////////////////////////
Dem()63 Dem::Dem()
64 : dataPtr(new DemPrivate)
65 {
66 this->dataPtr->dataSet = nullptr;
67 GDALAllRegister();
68 }
69
70 //////////////////////////////////////////////////
~Dem()71 Dem::~Dem()
72 {
73 this->dataPtr->demData.clear();
74
75 if (this->dataPtr->dataSet)
76 GDALClose(reinterpret_cast<GDALDataset *>(this->dataPtr->dataSet));
77 }
78
79 //////////////////////////////////////////////////
Load(const std::string & _filename)80 int Dem::Load(const std::string &_filename)
81 {
82 unsigned int width;
83 unsigned int height;
84 int xSize, ySize;
85 double upLeftX, upLeftY, upRightX, upRightY, lowLeftX, lowLeftY;
86 ignition::math::Angle upLeftLat, upLeftLong, upRightLat, upRightLong;
87 ignition::math::Angle lowLeftLat, lowLeftLong;
88
89 // Sanity check
90 std::string fullName = _filename;
91 if (!exists(findFilePath(fullName)))
92 fullName = common::find_file(_filename);
93
94 if (!exists(findFilePath(fullName)))
95 {
96 gzerr << "Unable to open DEM file[" << _filename
97 << "], check your GAZEBO_RESOURCE_PATH settings." << std::endl;
98 return -1;
99 }
100
101 this->dataPtr->dataSet = reinterpret_cast<GDALDataset *>(GDALOpen(
102 fullName.c_str(), GA_ReadOnly));
103
104 if (this->dataPtr->dataSet == nullptr)
105 {
106 gzerr << "Unable to open DEM file[" << fullName
107 << "]. Format not recognised as a supported dataset." << std::endl;
108 return -1;
109 }
110
111 int nBands = this->dataPtr->dataSet->RasterCount();
112 if (nBands != 1)
113 {
114 gzerr << "Unsupported number of bands in file [" << fullName + "]. Found "
115 << nBands << " but only 1 is a valid value." << std::endl;
116 return -1;
117 }
118
119 // Set the pointer to the band
120 this->dataPtr->band = this->dataPtr->dataSet->RasterBand(1);
121
122 // Raster width and height
123 xSize = this->dataPtr->dataSet->RasterXSize();
124 ySize = this->dataPtr->dataSet->RasterYSize();
125
126 // Corner coordinates
127 upLeftX = 0.0;
128 upLeftY = 0.0;
129 upRightX = xSize;
130 upRightY = 0.0;
131 lowLeftX = 0.0;
132 lowLeftY = ySize;
133
134 // Calculate the georeferenced coordinates of the terrain corners
135 this->GeoReference(upLeftX, upLeftY, upLeftLat, upLeftLong);
136 this->GeoReference(upRightX, upRightY, upRightLat, upRightLong);
137 this->GeoReference(lowLeftX, lowLeftY, lowLeftLat, lowLeftLong);
138
139 // Set the world width and height
140 this->dataPtr->worldWidth =
141 math::SphericalCoordinates::Distance(upLeftLat, upLeftLong,
142 upRightLat, upRightLong);
143 this->dataPtr->worldHeight =
144 math::SphericalCoordinates::Distance(upLeftLat, upLeftLong,
145 lowLeftLat, lowLeftLong);
146
147 // Set the terrain's side (the terrain will be squared after the padding)
148 if (ignition::math::isPowerOfTwo(ySize - 1))
149 height = ySize;
150 else
151 height = ignition::math::roundUpPowerOfTwo(ySize) + 1;
152
153 if (ignition::math::isPowerOfTwo(xSize - 1))
154 width = xSize;
155 else
156 width = ignition::math::roundUpPowerOfTwo(xSize) + 1;
157
158 this->dataPtr->side = std::max(width, height);
159
160 // Preload the DEM's data
161 if (this->LoadData() != 0)
162 return -1;
163
164 // Set the min/max heights
165 this->dataPtr->minElevation = *std::min_element(&this->dataPtr->demData[0],
166 &this->dataPtr->demData[0] + this->dataPtr->side * this->dataPtr->side);
167 this->dataPtr->maxElevation = *std::max_element(&this->dataPtr->demData[0],
168 &this->dataPtr->demData[0] + this->dataPtr->side * this->dataPtr->side);
169
170 return 0;
171 }
172
173 //////////////////////////////////////////////////
Elevation(double _x,double _y)174 double Dem::Elevation(double _x, double _y)
175 {
176 if (_x >= this->Width() || _y >= this->Height())
177 {
178 gzthrow("Illegal coordinates. You are asking for the elevation in (" <<
179 _x << "," << _y << ") but the terrain is [" << this->Width() <<
180 " x " << this->Height() << "]\n");
181 }
182
183 return this->dataPtr->demData.at(_y * this->Width() + _x);
184 }
185
186 //////////////////////////////////////////////////
MinElevation() const187 float Dem::MinElevation() const
188 {
189 return this->dataPtr->minElevation;
190 }
191
192 //////////////////////////////////////////////////
MaxElevation() const193 float Dem::MaxElevation() const
194 {
195 return this->dataPtr->maxElevation;
196 }
197
198 //////////////////////////////////////////////////
GeoReference(double _x,double _y,ignition::math::Angle & _latitude,ignition::math::Angle & _longitude) const199 void Dem::GeoReference(double _x, double _y,
200 ignition::math::Angle &_latitude, ignition::math::Angle &_longitude) const
201 {
202 double geoTransf[6];
203 if (this->dataPtr->dataSet->GeoTransform(geoTransf) == CE_None)
204 {
205 OGRSpatialReference sourceCs;
206 OGRSpatialReference targetCs;
207 OGRCoordinateTransformation *cT;
208 double xGeoDeg, yGeoDeg;
209
210 // Transform the terrain's coordinate system to WGS84
211 char *importString = strdup(this->dataPtr->dataSet->ProjectionRef());
212 sourceCs.importFromWkt(&importString);
213 targetCs.SetWellKnownGeogCS("WGS84");
214 cT = OGRCreateCoordinateTransformation(&sourceCs, &targetCs);
215
216 xGeoDeg = geoTransf[0] + _x * geoTransf[1] + _y * geoTransf[2];
217 yGeoDeg = geoTransf[3] + _x * geoTransf[4] + _y * geoTransf[5];
218
219 cT->Transform(1, &xGeoDeg, &yGeoDeg);
220
221 _latitude.Degree(yGeoDeg);
222 _longitude.Degree(xGeoDeg);
223 }
224 else
225 gzthrow("Unable to obtain the georeferenced values for coordinates ("
226 << _x << "," << _y << ")\n");
227 }
228
229 //////////////////////////////////////////////////
GeoReferenceOrigin(ignition::math::Angle & _latitude,ignition::math::Angle & _longitude) const230 void Dem::GeoReferenceOrigin(ignition::math::Angle &_latitude,
231 ignition::math::Angle &_longitude) const
232 {
233 return this->GeoReference(0, 0, _latitude, _longitude);
234 }
235
236 //////////////////////////////////////////////////
Height() const237 unsigned int Dem::Height() const
238 {
239 return this->dataPtr->side;
240 }
241
242 //////////////////////////////////////////////////
Width() const243 unsigned int Dem::Width() const
244 {
245 return this->dataPtr->side;
246 }
247
248 //////////////////////////////////////////////////
WorldWidth() const249 double Dem::WorldWidth() const
250 {
251 return this->dataPtr->worldWidth;
252 }
253
254 //////////////////////////////////////////////////
WorldHeight() const255 double Dem::WorldHeight() const
256 {
257 return this->dataPtr->worldHeight;
258 }
259
260 //////////////////////////////////////////////////
FillHeightMap(int _subSampling,unsigned int _vertSize,const ignition::math::Vector3d & _size,const ignition::math::Vector3d & _scale,bool _flipY,std::vector<float> & _heights)261 void Dem::FillHeightMap(int _subSampling, unsigned int _vertSize,
262 const ignition::math::Vector3d &_size,
263 const ignition::math::Vector3d &_scale,
264 bool _flipY, std::vector<float> &_heights)
265 {
266 if (_subSampling <= 0)
267 {
268 gzerr << "Illegal subsampling value (" << _subSampling << ")\n";
269 return;
270 }
271
272 // Resize the vector to match the size of the vertices.
273 _heights.resize(_vertSize * _vertSize);
274
275 // Iterate over all the vertices
276 for (unsigned int y = 0; y < _vertSize; ++y)
277 {
278 double yf = y / static_cast<double>(_subSampling);
279 unsigned int y1 = floor(yf);
280 unsigned int y2 = ceil(yf);
281 if (y2 >= this->dataPtr->side)
282 y2 = this->dataPtr->side - 1;
283 double dy = yf - y1;
284
285 for (unsigned int x = 0; x < _vertSize; ++x)
286 {
287 double xf = x / static_cast<double>(_subSampling);
288 unsigned int x1 = floor(xf);
289 unsigned int x2 = ceil(xf);
290 if (x2 >= this->dataPtr->side)
291 x2 = this->dataPtr->side - 1;
292 double dx = xf - x1;
293
294 double px1 = this->dataPtr->demData[y1 * this->dataPtr->side + x1];
295 double px2 = this->dataPtr->demData[y1 * this->dataPtr->side + x2];
296 float h1 = (px1 - ((px1 - px2) * dx));
297
298 double px3 = this->dataPtr->demData[y2 * this->dataPtr->side + x1];
299 double px4 = this->dataPtr->demData[y2 * this->dataPtr->side + x2];
300 float h2 = (px3 - ((px3 - px4) * dx));
301
302 float h = (h1 - ((h1 - h2) * dy) - std::max(0.0f,
303 this->MinElevation())) * _scale.Z();
304
305 // Invert pixel definition so 1=ground, 0=full height,
306 // if the terrain size has a negative z component
307 // this is mainly for backward compatibility
308 if (_size.Z() < 0)
309 h *= -1;
310
311 // Convert to 0 if a NODATA value is found
312 if (_size.Z() >= 0 && h < 0)
313 h = 0;
314
315 // Store the height for future use
316 if (!_flipY)
317 _heights[y * _vertSize + x] = h;
318 else
319 _heights[(_vertSize - y - 1) * _vertSize + x] = h;
320 }
321 }
322 }
323
324 //////////////////////////////////////////////////
LoadData()325 int Dem::LoadData()
326 {
327 unsigned int destWidth;
328 unsigned int destHeight;
329 unsigned int nXSize = this->dataPtr->dataSet->RasterXSize();
330 unsigned int nYSize = this->dataPtr->dataSet->RasterYSize();
331 float ratio;
332 std::vector<float> buffer;
333
334 if (nXSize == 0 || nYSize == 0)
335 {
336 gzerr << "Illegal size loading a DEM file (" << nXSize << ","
337 << nYSize << ")\n";
338 return -1;
339 }
340
341 // Scale the terrain keeping the same ratio between width and height
342 if (nXSize > nYSize)
343 {
344 ratio = static_cast<float>(nXSize) / static_cast<float>(nYSize);
345 destWidth = this->dataPtr->side;
346 // The decimal part is discarted for interpret the result as pixels
347 destHeight = static_cast<float>(destWidth) / static_cast<float>(ratio);
348 }
349 else
350 {
351 ratio = static_cast<float>(nYSize) / static_cast<float>(nXSize);
352 destHeight = this->dataPtr->side;
353 // The decimal part is discarted for interpret the result as pixels
354 destWidth = static_cast<float>(destHeight) / static_cast<float>(ratio);
355 }
356
357 // Read the whole raster data and convert it to a GDT_Float32 array.
358 // In this step the DEM is scaled to destWidth x destHeight
359 buffer.resize(destWidth * destHeight);
360 if (this->dataPtr->band->RasterIO(GF_Read, 0, 0, nXSize, nYSize, &buffer[0],
361 destWidth, destHeight, GDT_Float32, 0, 0) != CE_None)
362 {
363 gzerr << "Failure calling RasterIO while loading a DEM file\n";
364 return -1;
365 }
366
367 // Copy and align 'buffer' into the target vector. The destination vector is
368 // initialized to 0, so all the points not contained in 'buffer' will be
369 // extra padding
370 this->dataPtr->demData.resize(this->Width() * this->Height());
371 for (unsigned int y = 0; y < destHeight; ++y)
372 {
373 std::copy(&buffer[destWidth * y], &buffer[destWidth * y] + destWidth,
374 this->dataPtr->demData.begin() + this->Width() * y);
375 }
376 buffer.clear();
377
378 return 0;
379 }
380
381 #endif
382