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-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
17 // Set z-values for all network positions based on data from a height map
18 /****************************************************************************/
19 
20 
21 // ===========================================================================
22 // included modules
23 // ===========================================================================
24 #include <config.h>
25 
26 #include <string>
28 #include <utils/common/ToString.h>
31 #include <utils/geom/GeomHelper.h>
32 #include "NBHeightMapper.h"
34 #include <utils/common/RGBColor.h>
35 
36 #ifdef HAVE_GDAL
37 #if __GNUC__ > 3
38 #pragma GCC diagnostic push
39 #pragma GCC diagnostic ignored "-Wpedantic"
40 #endif
41 #include <ogrsf_frmts.h>
42 #include <ogr_api.h>
43 #include <gdal_priv.h>
44 #if __GNUC__ > 3
45 #pragma GCC diagnostic pop
46 #endif
47 #endif
48 
49 // ===========================================================================
50 // static members
51 // ===========================================================================
53 
54 // ===========================================================================
55 // method definitions
56 // ===========================================================================
57 
58 
60  myRTree(&Triangle::addSelf) {
61 }
62 
63 
65  clearData();
66 }
67 
68 
69 const NBHeightMapper&
71  return Singleton;
72 }
73 
74 
75 bool
77  return myRasters.size() > 0 || myTriangles.size() > 0;
78 }
79 
80 
81 double
82 NBHeightMapper::getZ(const Position& geo) const {
83  if (!ready()) {
84  WRITE_WARNING("Cannot supply height since no height data was loaded");
85  return 0;
86  }
87  for (auto& item : myRasters) {
88  const Boundary& boundary = item.first;
89  int16_t* raster = item.second;
90  double result = -1e6;
91  if (boundary.around(geo)) {
92  const int xSize = int((boundary.xmax() - boundary.xmin()) / mySizeOfPixel.x() + .5);
93  const double normX = (geo.x() - boundary.xmin()) / mySizeOfPixel.x();
94  const double normY = (geo.y() - boundary.ymax()) / mySizeOfPixel.y();
95  PositionVector corners;
96  corners.push_back(Position(floor(normX) + 0.5, floor(normY) + 0.5, raster[(int)normY * xSize + (int)normX]));
97  if (normX - floor(normX) > 0.5) {
98  corners.push_back(Position(floor(normX) + 1.5, floor(normY) + 0.5, raster[(int)normY * xSize + (int)normX + 1]));
99  } else {
100  corners.push_back(Position(floor(normX) - 0.5, floor(normY) + 0.5, raster[(int)normY * xSize + (int)normX - 1]));
101  }
102  if (normY - floor(normY) > 0.5) {
103  corners.push_back(Position(floor(normX) + 0.5, floor(normY) + 1.5, raster[((int)normY + 1) * xSize + (int)normX]));
104  } else {
105  corners.push_back(Position(floor(normX) + 0.5, floor(normY) - 0.5, raster[((int)normY - 1) * xSize + (int)normX]));
106  }
107  result = Triangle(corners).getZ(Position(normX, normY));
108  }
109  if (result > -1e5 && result < 1e5) {
110  return result;
111  }
112  }
113  // coordinates in degrees hence a small search window
114  float minB[2];
115  float maxB[2];
116  minB[0] = (float)geo.x() - 0.00001f;
117  minB[1] = (float)geo.y() - 0.00001f;
118  maxB[0] = (float)geo.x() + 0.00001f;
119  maxB[1] = (float)geo.y() + 0.00001f;
120  QueryResult queryResult;
121  int hits = myRTree.Search(minB, maxB, queryResult);
122  Triangles result = queryResult.triangles;
123  assert(hits == (int)result.size());
124  UNUSED_PARAMETER(hits); // only used for assertion
125 
126  for (Triangles::iterator it = result.begin(); it != result.end(); it++) {
127  const Triangle* triangle = *it;
128  if (triangle->contains(geo)) {
129  return triangle->getZ(geo);
130  }
131  }
132  WRITE_WARNING("Could not get height data for coordinate " + toString(geo));
133  return 0;
134 }
135 
136 
137 void
139  Triangle* triangle = new Triangle(corners);
140  myTriangles.push_back(triangle);
141  Boundary b = corners.getBoxBoundary();
142  const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
143  const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
144  myRTree.Insert(cmin, cmax, triangle);
145 }
146 
147 
148 void
150  if (oc.isSet("heightmap.geotiff")) {
151  // parse file(s)
152  std::vector<std::string> files = oc.getStringVector("heightmap.geotiff");
153  for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
154  PROGRESS_BEGIN_MESSAGE("Parsing from GeoTIFF '" + *file + "'");
155  int numFeatures = Singleton.loadTiff(*file);
157  " done (parsed " + toString(numFeatures) +
158  " features, Boundary: " + toString(Singleton.getBoundary()) + ").");
159  }
160  }
161  if (oc.isSet("heightmap.shapefiles")) {
162  // parse file(s)
163  std::vector<std::string> files = oc.getStringVector("heightmap.shapefiles");
164  for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
165  PROGRESS_BEGIN_MESSAGE("Parsing from shape-file '" + *file + "'");
166  int numFeatures = Singleton.loadShapeFile(*file);
168  " done (parsed " + toString(numFeatures) +
169  " features, Boundary: " + toString(Singleton.getBoundary()) + ").");
170  }
171  }
172 }
173 
174 
175 int
176 NBHeightMapper::loadShapeFile(const std::string& file) {
177 #ifdef HAVE_GDAL
178 #if GDAL_VERSION_MAJOR < 2
179  OGRRegisterAll();
180  OGRDataSource* ds = OGRSFDriverRegistrar::Open(file.c_str(), FALSE);
181 #else
182  GDALAllRegister();
183  GDALDataset* ds = (GDALDataset*)GDALOpenEx(file.c_str(), GDAL_OF_VECTOR | GA_ReadOnly, NULL, NULL, NULL);
184 #endif
185  if (ds == NULL) {
186  throw ProcessError("Could not open shape file '" + file + "'.");
187  }
188 
189  // begin file parsing
190  OGRLayer* layer = ds->GetLayer(0);
191  layer->ResetReading();
192 
193  // triangle coordinates are stored in WGS84 and later matched with network coordinates in WGS84
194  // build coordinate transformation
195  OGRSpatialReference* sr_src = layer->GetSpatialRef();
196  OGRSpatialReference sr_dest;
197  sr_dest.SetWellKnownGeogCS("WGS84");
198  OGRCoordinateTransformation* toWGS84 = OGRCreateCoordinateTransformation(sr_src, &sr_dest);
199  if (toWGS84 == 0) {
200  WRITE_WARNING("Could not create geocoordinates converter; check whether proj.4 is installed.");
201  }
202 
203  int numFeatures = 0;
204  OGRFeature* feature;
205  layer->ResetReading();
206  while ((feature = layer->GetNextFeature()) != NULL) {
207  OGRGeometry* geom = feature->GetGeometryRef();
208  assert(geom != 0);
209 
210  // @todo gracefull handling of shapefiles with unexpected contents or any error handling for that matter
211  assert(std::string(geom->getGeometryName()) == std::string("POLYGON"));
212  // try transform to wgs84
213  geom->transform(toWGS84);
214  OGRLinearRing* cgeom = ((OGRPolygon*) geom)->getExteriorRing();
215  // assume TIN with with 4 points and point0 == point3
216  assert(cgeom->getNumPoints() == 4);
217  PositionVector corners;
218  for (int j = 0; j < 3; j++) {
219  Position pos((double) cgeom->getX(j), (double) cgeom->getY(j), (double) cgeom->getZ(j));
220  corners.push_back(pos);
221  myBoundary.add(pos);
222  }
223  addTriangle(corners);
224  numFeatures++;
225 
226  /*
227  OGRwkbGeometryType gtype = geom->getGeometryType();
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  for (int i = 1; i <= poDataset->GetRasterCount(); i++) {
302  GDALRasterBand* poBand = poDataset->GetRasterBand(i);
303  if (poBand->GetColorInterpretation() != GCI_GrayIndex) {
304  WRITE_ERROR("Unknown color band in " + file + ".");
305  clearData();
306  break;
307  }
308  if (poBand->GetRasterDataType() != GDT_Int16) {
309  WRITE_ERROR("Unknown data type in " + file + ".");
310  clearData();
311  break;
312  }
313  assert(xSize == poBand->GetXSize() && ySize == poBand->GetYSize());
314  if (poBand->RasterIO(GF_Read, 0, 0, xSize, ySize, raster, xSize, ySize, GDT_Int16, 0, 0) == CE_Failure) {
315  WRITE_ERROR("Failure in reading " + file + ".");
316  clearData();
317  break;
318  }
319  }
320  GDALClose(poDataset);
321  myRasters.push_back(std::make_pair(boundary, raster));
322  return picSize;
323 #else
324  UNUSED_PARAMETER(file);
325  WRITE_ERROR("Cannot load GeoTIFF file since SUMO was compiled without GDAL support.");
326  return 0;
327 #endif
328 }
329 
330 
331 void
333  for (Triangles::iterator it = myTriangles.begin(); it != myTriangles.end(); it++) {
334  delete *it;
335  }
336  myTriangles.clear();
337 #ifdef HAVE_GDAL
338  for (auto& item : myRasters) {
339  CPLFree(item.second);
340  }
341  myRasters.clear();
342 #endif
343  myBoundary.reset();
344 }
345 
346 
347 // ===========================================================================
348 // Triangle member methods
349 // ===========================================================================
351  myCorners(corners) {
352  assert(myCorners.size() == 3);
353  // @todo assert non-colinearity
354 }
355 
356 
357 void
359  queryResult.triangles.push_back(this);
360 }
361 
362 
363 bool
365  return myCorners.around(pos);
366 }
367 
368 
369 double
371  // en.wikipedia.org/wiki/Line-plane_intersection
372  Position p0 = myCorners.front();
373  Position line(0, 0, 1);
374  p0.sub(geo); // p0 - l0
375  Position normal = normalVector();
376  return p0.dotProduct(normal) / line.dotProduct(normal);
377 }
378 
379 
380 Position
382  // @todo maybe cache result to avoid multiple computations?
383  Position side1 = myCorners[1] - myCorners[0];
384  Position side2 = myCorners[2] - myCorners[0];
385  return side1.crossProduct(side2);
386 }
387 
388 
389 /****************************************************************************/
390 
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:131
double xmax() const
Returns maximum x-coordinate.
Definition: Boundary.cpp:125
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:62
double x() const
Returns the x-position.
Definition: Position.h:57
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:87
NBHeightMapper()
private constructor and destructor (Singleton)
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:32
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:42
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:239
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:259
void addSelf(const QueryResult &queryResult) const
callback for RTree search
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:48
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:39
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:119
#define PROGRESS_BEGIN_MESSAGE(msg)
Definition: MsgHandler.h:241
static MsgHandler * getMessageInstance()
Returns the instance to add normal messages to.
Definition: MsgHandler.cpp:59
Position mySizeOfPixel
dimensions of one pixel in raster data
Boundary myBoundary
convex boundary of all known triangles;
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:245
void reset()
Resets the boundary.
Definition: Boundary.cpp:67
double dotProduct(const Position &pos)
returns the dot product (scalar product) between this point and the second one
Definition: Position.h:267
bool around(const Position &p, double offset=0) const
Returns whether the AbstractPoly the given coordinate.
Definition: Boundary.cpp:173
PositionVector myCorners
the corners of the triangle
A storage for options typed value containers)
Definition: OptionsCont.h:90
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)
std::vector< std::pair< Boundary, int16_t * > > myRasters
raster height information in m for all loaded files
void add(double x, double y, double z=0)
Makes the boundary include the given coordinate.
Definition: Boundary.cpp:79
const Boundary & getBoundary()
returns the convex boundary of all known triangles
double ymax() const
Returns maximum y-coordinate.
Definition: Boundary.cpp:137
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:147
virtual void endProcessMsg(std::string msg)
Ends a process information.
Definition: MsgHandler.cpp:148