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