Eclipse SUMO - Simulation of Urban MObility
NBHeightMapper.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2011-2022 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials are made available under the
5 // terms of the Eclipse Public License 2.0 which is available at
6 // https://www.eclipse.org/legal/epl-2.0/
7 // This Source Code may also be made available under the following Secondary
8 // Licenses when the conditions for such availability set forth in the Eclipse
9 // Public License 2.0 are satisfied: GNU General Public License, version 2
10 // or later which is available at
11 // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 /****************************************************************************/
20 // Set z-values for all network positions based on data from a height map
21 /****************************************************************************/
22 #include <config.h>
23 
24 #include <string>
26 #include <utils/common/ToString.h>
29 #include <utils/geom/GeomHelper.h>
30 #include "NBHeightMapper.h"
32 #include <utils/common/RGBColor.h>
33 
34 #ifdef HAVE_GDAL
35 #if __GNUC__ > 3
36 #pragma GCC diagnostic push
37 #pragma GCC diagnostic ignored "-Wpedantic"
38 #endif
39 #include <ogrsf_frmts.h>
40 #include <ogr_api.h>
41 #include <gdal_priv.h>
42 #if __GNUC__ > 3
43 #pragma GCC diagnostic pop
44 #endif
45 #endif
46 
47 // ===========================================================================
48 // static members
49 // ===========================================================================
51 
52 
53 // ===========================================================================
54 // method definitions
55 // ===========================================================================
57  myRTree(&Triangle::addSelf) {
58 }
59 
60 
62  clearData();
63 }
64 
65 
66 const NBHeightMapper&
68  return myInstance;
69 }
70 
71 
72 bool
74  return myRasters.size() > 0 || myTriangles.size() > 0;
75 }
76 
77 
78 double
79 NBHeightMapper::getZ(const Position& geo) const {
80  if (!ready()) {
81  WRITE_WARNING("Cannot supply height since no height data was loaded");
82  return 0;
83  }
84  for (auto& item : myRasters) {
85  const Boundary& boundary = item.boundary;
86  int16_t* raster = item.raster;
87  double result = -1e6;
88  if (boundary.around(geo)) {
89  const int xSize = item.xSize;
90  const double normX = (geo.x() - boundary.xmin()) / mySizeOfPixel.x();
91  const double normY = (geo.y() - boundary.ymax()) / mySizeOfPixel.y();
92  PositionVector corners;
93  corners.push_back(Position(floor(normX) + 0.5, floor(normY) + 0.5, raster[(int)normY * xSize + (int)normX]));
94  if (normX - floor(normX) > 0.5) {
95  corners.push_back(Position(floor(normX) + 1.5, floor(normY) + 0.5, raster[(int)normY * xSize + (int)normX + 1]));
96  } else {
97  corners.push_back(Position(floor(normX) - 0.5, floor(normY) + 0.5, raster[(int)normY * xSize + (int)normX - 1]));
98  }
99  if (normY - floor(normY) > 0.5 && ((int)normY + 1) < item.ySize) {
100  corners.push_back(Position(floor(normX) + 0.5, floor(normY) + 1.5, raster[((int)normY + 1) * xSize + (int)normX]));
101  } else {
102  corners.push_back(Position(floor(normX) + 0.5, floor(normY) - 0.5, raster[((int)normY - 1) * xSize + (int)normX]));
103  }
104  result = Triangle(corners).getZ(Position(normX, normY));
105  }
106  if (result > -1e5 && result < 1e5) {
107  return result;
108  }
109  }
110  // coordinates in degrees hence a small search window
111  float minB[2];
112  float maxB[2];
113  minB[0] = (float)geo.x() - 0.00001f;
114  minB[1] = (float)geo.y() - 0.00001f;
115  maxB[0] = (float)geo.x() + 0.00001f;
116  maxB[1] = (float)geo.y() + 0.00001f;
117  QueryResult queryResult;
118  int hits = myRTree.Search(minB, maxB, queryResult);
119  Triangles result = queryResult.triangles;
120  assert(hits == (int)result.size());
121  UNUSED_PARAMETER(hits); // only used for assertion
122 
123  for (Triangles::iterator it = result.begin(); it != result.end(); it++) {
124  const Triangle* triangle = *it;
125  if (triangle->contains(geo)) {
126  return triangle->getZ(geo);
127  }
128  }
129  WRITE_WARNINGF("Could not get height data for coordinate %", toString(geo));
130  return 0;
131 }
132 
133 
134 void
136  Triangle* triangle = new Triangle(corners);
137  myTriangles.push_back(triangle);
138  Boundary b = corners.getBoxBoundary();
139  const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
140  const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
141  myRTree.Insert(cmin, cmax, triangle);
142 }
143 
144 
145 void
147  if (oc.isSet("heightmap.geotiff")) {
148  // parse file(s)
149  std::vector<std::string> files = oc.getStringVector("heightmap.geotiff");
150  for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
151  PROGRESS_BEGIN_MESSAGE("Parsing from GeoTIFF '" + *file + "'");
152  int numFeatures = myInstance.loadTiff(*file);
154  " done (parsed " + toString(numFeatures) +
155  " features, Boundary: " + toString(myInstance.getBoundary()) + ").");
156  }
157  }
158  if (oc.isSet("heightmap.shapefiles")) {
159  // parse file(s)
160  std::vector<std::string> files = oc.getStringVector("heightmap.shapefiles");
161  for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
162  PROGRESS_BEGIN_MESSAGE("Parsing from shape-file '" + *file + "'");
163  int numFeatures = myInstance.loadShapeFile(*file);
165  " done (parsed " + toString(numFeatures) +
166  " features, Boundary: " + toString(myInstance.getBoundary()) + ").");
167  }
168  }
169 }
170 
171 
172 int
173 NBHeightMapper::loadShapeFile(const std::string& file) {
174 #ifdef HAVE_GDAL
175 #if GDAL_VERSION_MAJOR < 2
176  OGRRegisterAll();
177  OGRDataSource* ds = OGRSFDriverRegistrar::Open(file.c_str(), FALSE);
178 #else
179  GDALAllRegister();
180  GDALDataset* ds = (GDALDataset*)GDALOpenEx(file.c_str(), GDAL_OF_VECTOR | GA_ReadOnly, nullptr, nullptr, nullptr);
181 #endif
182  if (ds == nullptr) {
183  throw ProcessError("Could not open shape file '" + file + "'.");
184  }
185 
186  // begin file parsing
187  OGRLayer* layer = ds->GetLayer(0);
188  layer->ResetReading();
189 
190  // triangle coordinates are stored in WGS84 and later matched with network coordinates in WGS84
191  // build coordinate transformation
192  OGRSpatialReference* sr_src = layer->GetSpatialRef();
193  OGRSpatialReference sr_dest;
194  sr_dest.SetWellKnownGeogCS("WGS84");
195  OGRCoordinateTransformation* toWGS84 = OGRCreateCoordinateTransformation(sr_src, &sr_dest);
196  if (toWGS84 == nullptr) {
197  WRITE_WARNING("Could not create geocoordinates converter; check whether proj.4 is installed.");
198  }
199 
200  int numFeatures = 0;
201  OGRFeature* feature;
202  layer->ResetReading();
203  while ((feature = layer->GetNextFeature()) != nullptr) {
204  OGRGeometry* geom = feature->GetGeometryRef();
205  assert(geom != 0);
206 
207  OGRwkbGeometryType gtype = geom->getGeometryType();
208  if (gtype == wkbPolygon) {
209  assert(std::string(geom->getGeometryName()) == std::string("POLYGON"));
210  // try transform to wgs84
211  geom->transform(toWGS84);
212  OGRLinearRing* cgeom = ((OGRPolygon*) geom)->getExteriorRing();
213  // assume TIN with with 4 points and point0 == point3
214  assert(cgeom->getNumPoints() == 4);
215  PositionVector corners;
216  for (int j = 0; j < 3; j++) {
217  Position pos((double) cgeom->getX(j), (double) cgeom->getY(j), (double) cgeom->getZ(j));
218  corners.push_back(pos);
219  myBoundary.add(pos);
220  }
221  addTriangle(corners);
222  numFeatures++;
223  } else {
224  WRITE_WARNINGF("Ignored heightmap feature type %", geom->getGeometryName());
225  }
226 
227  /*
228  switch (gtype) {
229  case wkbPolygon: {
230  break;
231  }
232  case wkbPoint: {
233  WRITE_WARNING("got wkbPoint");
234  break;
235  }
236  case wkbLineString: {
237  WRITE_WARNING("got wkbLineString");
238  break;
239  }
240  case wkbMultiPoint: {
241  WRITE_WARNING("got wkbMultiPoint");
242  break;
243  }
244  case wkbMultiLineString: {
245  WRITE_WARNING("got wkbMultiLineString");
246  break;
247  }
248  case wkbMultiPolygon: {
249  WRITE_WARNING("got wkbMultiPolygon");
250  break;
251  }
252  default:
253  WRITE_WARNING("Unsupported shape type occurred");
254  break;
255  }
256  */
257  OGRFeature::DestroyFeature(feature);
258  }
259 #if GDAL_VERSION_MAJOR < 2
260  OGRDataSource::DestroyDataSource(ds);
261 #else
262  GDALClose(ds);
263 #endif
264  OCTDestroyCoordinateTransformation(reinterpret_cast<OGRCoordinateTransformationH>(toWGS84));
265  OGRCleanupAll();
266  return numFeatures;
267 #else
268  UNUSED_PARAMETER(file);
269  WRITE_ERROR("Cannot load shape file since SUMO was compiled without GDAL support.");
270  return 0;
271 #endif
272 }
273 
274 
275 int
276 NBHeightMapper::loadTiff(const std::string& file) {
277 #ifdef HAVE_GDAL
278  GDALAllRegister();
279  GDALDataset* poDataset = (GDALDataset*)GDALOpen(file.c_str(), GA_ReadOnly);
280  if (poDataset == 0) {
281  WRITE_ERROR("Cannot load GeoTIFF file.");
282  return 0;
283  }
284  Boundary boundary;
285  const int xSize = poDataset->GetRasterXSize();
286  const int ySize = poDataset->GetRasterYSize();
287  double adfGeoTransform[6];
288  if (poDataset->GetGeoTransform(adfGeoTransform) == CE_None) {
289  Position topLeft(adfGeoTransform[0], adfGeoTransform[3]);
290  mySizeOfPixel.set(adfGeoTransform[1], adfGeoTransform[5]);
291  const double horizontalSize = xSize * mySizeOfPixel.x();
292  const double verticalSize = ySize * mySizeOfPixel.y();
293  boundary.add(topLeft);
294  boundary.add(topLeft.x() + horizontalSize, topLeft.y() + verticalSize);
295  } else {
296  WRITE_ERROR("Could not parse geo information from " + file + ".");
297  return 0;
298  }
299  const int picSize = xSize * ySize;
300  int16_t* raster = (int16_t*)CPLMalloc(sizeof(int16_t) * picSize);
301  bool ok = true;
302  for (int i = 1; i <= poDataset->GetRasterCount(); i++) {
303  GDALRasterBand* poBand = poDataset->GetRasterBand(i);
304  if (poBand->GetColorInterpretation() != GCI_GrayIndex) {
305  WRITE_ERROR("Unknown color band in " + file + ".");
306  clearData();
307  ok = false;
308  break;
309  }
310  assert(xSize == poBand->GetXSize() && ySize == poBand->GetYSize());
311  if (poBand->RasterIO(GF_Read, 0, 0, xSize, ySize, raster, xSize, ySize, GDT_Int16, 0, 0) == CE_Failure) {
312  WRITE_ERROR("Failure in reading " + file + ".");
313  clearData();
314  ok = false;
315  break;
316  }
317  }
318  double min = std::numeric_limits<double>::max();
319  double max = -std::numeric_limits<double>::max();
320  for (int i = 0; i < picSize; i++) {
321  min = MIN2(min, (double)raster[i]);
322  max = MAX2(max, (double)raster[i]);
323  }
324  GDALClose(poDataset);
325  if (ok) {
326  WRITE_MESSAGE("Read geotiff heightmap with size " + toString(xSize) + "," + toString(ySize)
327  + " for geo boundary [" + toString(boundary)
328  + "] with elevation range [" + toString(min) + "," + toString(max) + "].");
329  RasterData rasterData;
330  rasterData.raster = raster;
331  rasterData.boundary = boundary;
332  rasterData.xSize = xSize;
333  rasterData.ySize = ySize;
334  myRasters.push_back(rasterData);
335  return picSize;
336  } else {
337  return 0;
338  }
339 #else
340  UNUSED_PARAMETER(file);
341  WRITE_ERROR("Cannot load GeoTIFF file since SUMO was compiled without GDAL support.");
342  return 0;
343 #endif
344 }
345 
346 
347 void
349  for (Triangles::iterator it = myTriangles.begin(); it != myTriangles.end(); it++) {
350  delete *it;
351  }
352  myTriangles.clear();
353 #ifdef HAVE_GDAL
354  for (auto& item : myRasters) {
355  CPLFree(item.raster);
356  }
357  myRasters.clear();
358 #endif
359  myBoundary.reset();
360 }
361 
362 
363 // ===========================================================================
364 // Triangle member methods
365 // ===========================================================================
367  myCorners(corners) {
368  assert(myCorners.size() == 3);
369  // @todo assert non-colinearity
370 }
371 
372 
373 void
375  queryResult.triangles.push_back(this);
376 }
377 
378 
379 bool
381  return myCorners.around(pos);
382 }
383 
384 
385 double
387  // en.wikipedia.org/wiki/Line-plane_intersection
388  Position p0 = myCorners.front();
389  Position line(0, 0, 1);
390  p0.sub(geo); // p0 - l0
391  Position normal = normalVector();
392  return p0.dotProduct(normal) / line.dotProduct(normal);
393 }
394 
395 
396 Position
398  // @todo maybe cache result to avoid multiple computations?
399  Position side1 = myCorners[1] - myCorners[0];
400  Position side2 = myCorners[2] - myCorners[0];
401  return side1.crossProduct(side2);
402 }
403 
404 
405 /****************************************************************************/
#define WRITE_WARNINGF(...)
Definition: MsgHandler.h:281
#define WRITE_MESSAGE(msg)
Definition: MsgHandler.h:282
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:288
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:280
#define PROGRESS_BEGIN_MESSAGE(msg)
Definition: MsgHandler.h:283
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:30
T MIN2(T a, T b)
Definition: StdDefs.h:74
T MAX2(T a, T b)
Definition: StdDefs.h:80
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:46
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:39
void add(double x, double y, double z=0)
Makes the boundary include the given coordinate.
Definition: Boundary.cpp:77
double ymin() const
Returns minimum y-coordinate.
Definition: Boundary.cpp:129
void reset()
Resets the boundary.
Definition: Boundary.cpp:65
double xmin() const
Returns minimum x-coordinate.
Definition: Boundary.cpp:117
bool around(const Position &p, double offset=0) const
Returns whether the AbstractPoly the given coordinate.
Definition: Boundary.cpp:171
double ymax() const
Returns maximum y-coordinate.
Definition: Boundary.cpp:135
double xmax() const
Returns maximum x-coordinate.
Definition: Boundary.cpp:123
virtual void endProcessMsg(std::string msg)
Ends a process information.
Definition: MsgHandler.cpp:150
static MsgHandler * getMessageInstance()
Returns the instance to add normal messages to.
Definition: MsgHandler.cpp:54
class for cirumventing the const-restriction of RTree::Search-context
Position normalVector() const
returns the normal vector for this triangles plane
double getZ(const Position &geo) const
returns the projection of the give geoCoordinate (WGS84) onto triangle plane
PositionVector myCorners
the corners of the triangle
Triangle(const PositionVector &corners)
void addSelf(const QueryResult &queryResult) const
callback for RTree search
bool contains(const Position &pos) const
checks whether pos lies within triangle (only checks x,y)
Set z-values for all network positions based on data from a height map.
std::vector< const Triangle * > Triangles
Triangles myTriangles
double getZ(const Position &geo) const
returns height for the given geo coordinate (WGS84)
static const NBHeightMapper & get()
return the singleton instance (maybe 0)
int loadShapeFile(const std::string &file)
load height data from Arcgis-shape file and returns the number of parsed features
static NBHeightMapper myInstance
the singleton instance
bool ready() const
returns whether the NBHeightMapper has data
std::vector< RasterData > myRasters
raster height information in m for all loaded files
NBHeightMapper()
private constructor and destructor (Singleton)
static void loadIfSet(OptionsCont &oc)
loads height map data if any loading options are set
void clearData()
clears loaded data
Position mySizeOfPixel
dimensions of one pixel in raster data
void addTriangle(PositionVector corners)
adds one triangles worth of height data
Boundary myBoundary
convex boundary of all known triangles;
TRIANGLE_RTREE_QUAL myRTree
The RTree for spatial queries.
const Boundary & getBoundary()
returns the convex boundary of all known triangles
int loadTiff(const std::string &file)
load height data from GeoTIFF file and returns the number of non void pixels
A storage for options typed value containers)
Definition: OptionsCont.h:89
bool isSet(const std::string &name, bool failOnNonExistant=true) const
Returns the information whether the named option is set.
const StringVector & getStringVector(const std::string &name) const
Returns the list of string-value of the named option (only for Option_StringVector)
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:37
void set(double x, double y)
set positions x and y
Definition: Position.h:85
double dotProduct(const Position &pos)
returns the dot product (scalar product) between this point and the second one
Definition: Position.h:275
void sub(double dx, double dy)
Substracts the given position from this one.
Definition: Position.h:145
double x() const
Returns the x-position.
Definition: Position.h:55
Position crossProduct(const Position &pos)
returns the cross product between this point and the second one
Definition: Position.h:267
double y() const
Returns the y-position.
Definition: Position.h:60
A list of positions.
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.