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-2020 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.first;
86  int16_t* raster = item.second;
87  double result = -1e6;
88  if (boundary.around(geo)) {
89  const int xSize = int((boundary.xmax() - boundary.xmin()) / mySizeOfPixel.x() + .5);
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) {
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_WARNING("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  // @todo gracefull handling of shapefiles with unexpected contents or any error handling for that matter
208  assert(std::string(geom->getGeometryName()) == std::string("POLYGON"));
209  // try transform to wgs84
210  geom->transform(toWGS84);
211  OGRLinearRing* cgeom = ((OGRPolygon*) geom)->getExteriorRing();
212  // assume TIN with with 4 points and point0 == point3
213  assert(cgeom->getNumPoints() == 4);
214  PositionVector corners;
215  for (int j = 0; j < 3; j++) {
216  Position pos((double) cgeom->getX(j), (double) cgeom->getY(j), (double) cgeom->getZ(j));
217  corners.push_back(pos);
218  myBoundary.add(pos);
219  }
220  addTriangle(corners);
221  numFeatures++;
222 
223  /*
224  OGRwkbGeometryType gtype = geom->getGeometryType();
225  switch (gtype) {
226  case wkbPolygon: {
227  break;
228  }
229  case wkbPoint: {
230  WRITE_WARNING("got wkbPoint");
231  break;
232  }
233  case wkbLineString: {
234  WRITE_WARNING("got wkbLineString");
235  break;
236  }
237  case wkbMultiPoint: {
238  WRITE_WARNING("got wkbMultiPoint");
239  break;
240  }
241  case wkbMultiLineString: {
242  WRITE_WARNING("got wkbMultiLineString");
243  break;
244  }
245  case wkbMultiPolygon: {
246  WRITE_WARNING("got wkbMultiPolygon");
247  break;
248  }
249  default:
250  WRITE_WARNING("Unsupported shape type occurred");
251  break;
252  }
253  */
254  OGRFeature::DestroyFeature(feature);
255  }
256 #if GDAL_VERSION_MAJOR < 2
257  OGRDataSource::DestroyDataSource(ds);
258 #else
259  GDALClose(ds);
260 #endif
261  OCTDestroyCoordinateTransformation(reinterpret_cast<OGRCoordinateTransformationH>(toWGS84));
262  OGRCleanupAll();
263  return numFeatures;
264 #else
265  UNUSED_PARAMETER(file);
266  WRITE_ERROR("Cannot load shape file since SUMO was compiled without GDAL support.");
267  return 0;
268 #endif
269 }
270 
271 
272 int
273 NBHeightMapper::loadTiff(const std::string& file) {
274 #ifdef HAVE_GDAL
275  GDALAllRegister();
276  GDALDataset* poDataset = (GDALDataset*)GDALOpen(file.c_str(), GA_ReadOnly);
277  if (poDataset == 0) {
278  WRITE_ERROR("Cannot load GeoTIFF file.");
279  return 0;
280  }
281  Boundary boundary;
282  const int xSize = poDataset->GetRasterXSize();
283  const int ySize = poDataset->GetRasterYSize();
284  double adfGeoTransform[6];
285  if (poDataset->GetGeoTransform(adfGeoTransform) == CE_None) {
286  Position topLeft(adfGeoTransform[0], adfGeoTransform[3]);
287  mySizeOfPixel.set(adfGeoTransform[1], adfGeoTransform[5]);
288  const double horizontalSize = xSize * mySizeOfPixel.x();
289  const double verticalSize = ySize * mySizeOfPixel.y();
290  boundary.add(topLeft);
291  boundary.add(topLeft.x() + horizontalSize, topLeft.y() + verticalSize);
292  } else {
293  WRITE_ERROR("Could not parse geo information from " + file + ".");
294  return 0;
295  }
296  const int picSize = xSize * ySize;
297  int16_t* raster = (int16_t*)CPLMalloc(sizeof(int16_t) * picSize);
298  for (int i = 1; i <= poDataset->GetRasterCount(); i++) {
299  GDALRasterBand* poBand = poDataset->GetRasterBand(i);
300  if (poBand->GetColorInterpretation() != GCI_GrayIndex) {
301  WRITE_ERROR("Unknown color band in " + file + ".");
302  clearData();
303  break;
304  }
305  if (poBand->GetRasterDataType() != GDT_Int16) {
306  WRITE_ERROR("Unknown data type in " + file + ".");
307  clearData();
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  break;
315  }
316  }
317  GDALClose(poDataset);
318  myRasters.push_back(std::make_pair(boundary, raster));
319  return picSize;
320 #else
321  UNUSED_PARAMETER(file);
322  WRITE_ERROR("Cannot load GeoTIFF file since SUMO was compiled without GDAL support.");
323  return 0;
324 #endif
325 }
326 
327 
328 void
330  for (Triangles::iterator it = myTriangles.begin(); it != myTriangles.end(); it++) {
331  delete *it;
332  }
333  myTriangles.clear();
334 #ifdef HAVE_GDAL
335  for (auto& item : myRasters) {
336  CPLFree(item.second);
337  }
338  myRasters.clear();
339 #endif
340  myBoundary.reset();
341 }
342 
343 
344 // ===========================================================================
345 // Triangle member methods
346 // ===========================================================================
348  myCorners(corners) {
349  assert(myCorners.size() == 3);
350  // @todo assert non-colinearity
351 }
352 
353 
354 void
356  queryResult.triangles.push_back(this);
357 }
358 
359 
360 bool
362  return myCorners.around(pos);
363 }
364 
365 
366 double
368  // en.wikipedia.org/wiki/Line-plane_intersection
369  Position p0 = myCorners.front();
370  Position line(0, 0, 1);
371  p0.sub(geo); // p0 - l0
372  Position normal = normalVector();
373  return p0.dotProduct(normal) / line.dotProduct(normal);
374 }
375 
376 
377 Position
379  // @todo maybe cache result to avoid multiple computations?
380  Position side1 = myCorners[1] - myCorners[0];
381  Position side2 = myCorners[2] - myCorners[0];
382  return side1.crossProduct(side2);
383 }
384 
385 
386 /****************************************************************************/
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:284
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:276
#define PROGRESS_BEGIN_MESSAGE(msg)
Definition: MsgHandler.h:279
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:29
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:44
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:147
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< std::pair< Boundary, int16_t * > > 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:36
void set(double x, double y)
set positions x and y
Definition: Position.h:84
double dotProduct(const Position &pos)
returns the dot product (scalar product) between this point and the second one
Definition: Position.h:264
void sub(double dx, double dy)
Substracts the given position from this one.
Definition: Position.h:144
double x() const
Returns the x-position.
Definition: Position.h:54
Position crossProduct(const Position &pos)
returns the cross product between this point and the second one
Definition: Position.h:256
double y() const
Returns the y-position.
Definition: Position.h:59
A list of positions.
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.