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