Geospatial component for heightmap & DEMs

.github/ci/packages.apt

@@ -3,6 +3,7 @@ libavdevice-dev
 libavformat-dev
 libavutil-dev
 libfreeimage-dev
+libgdal-dev
 libgts-dev
 libignition-cmake2-dev
 libignition-math6-dev

CMakeLists.txt

@@ -48,12 +48,12 @@ message(STATUS "\n\n-- ====== Finding Dependencies ======")
 
 #--------------------------------------
 # Find ignition-math
-ign_find_package(ignition-math6 REQUIRED_BY graphics events VERSION 6.6)
+ign_find_package(ignition-math6 REQUIRED_BY geospatial graphics events VERSION 6.6)
 set(IGN_MATH_VER ${ignition-math6_VERSION_MAJOR})
 
 #--------------------------------------
 # Find ignition-utils
-ign_find_package(ignition-utils1 REQUIRED_BY graphics VERSION 1.0)
+ign_find_package(ignition-utils1 REQUIRED_BY geospatial graphics VERSION 1.0)
 set(IGN_UTILS_VER ${ignition-utils1_VERSION_MAJOR})
 
 #--------------------------------------
@@ -94,6 +94,12 @@ ign_find_package(
   REQUIRED_BY graphics
   PRIVATE_FOR graphics)
 
+#------------------------------------
+# Find GDAL
+ign_find_package(GDAL VERSION 3.0
+  REQUIRED_BY geospatial
+  PRIVATE_FOR geospatial)
+
 #------------------------------------
 # Find libswscale
 ign_find_package(SWSCALE REQUIRED_BY av PRETTY libswscale)
@@ -125,7 +131,7 @@ configure_file("${PROJECT_SOURCE_DIR}/cppcheck.suppress.in"
                ${PROJECT_BINARY_DIR}/cppcheck.suppress)
 
 ign_configure_build(QUIT_IF_BUILD_ERRORS
-  COMPONENTS av events graphics profiler)
+  COMPONENTS av events geospatial graphics profiler)
 
 #============================================================================
 # Create package information

geospatial/include/ignition/common/CMakeLists.txt

@@ -0,0 +1,2 @@
+
+ign_install_all_headers(COMPONENT geospatial)

graphics/include/ignition/common/Dem.hh → geospatial/include/ignition/common/Dem.hh

@@ -18,18 +18,17 @@
 #define IGNITION_COMMON_DEM_HH_
 
 #include <memory>
+#include <string>
+#include <vector>
+
 #include <ignition/math/Vector3.hh>
 #include <ignition/math/Angle.hh>
 
+#include <ignition/common/HeightmapData.hh>
 #include <ignition/common/graphics/Export.hh>
 
 #include <ignition/utils/ImplPtr.hh>
 
-#ifdef HAVE_GDAL
-# include <string>
-# include <vector>
-
-# include <ignition/common/HeightmapData.hh>
 
 namespace ignition
 {
@@ -137,4 +136,3 @@ namespace ignition
   }
 }
 #endif
-#endif

geospatial/src/CMakeLists.txt

@@ -0,0 +1,22 @@
+
+ign_get_libsources_and_unittests(sources gtest_sources)
+
+ign_add_component(geospatial
+  SOURCES ${sources}
+  DEPENDS_ON_COMPONENTS graphics
+  GET_TARGET_NAME geospatial_target)
+
+target_link_libraries(${geospatial_target}
+  PUBLIC
+    ${PROJECT_LIBRARY_TARGET_NAME}-graphics
+    ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}
+    ignition-utils${IGN_UTILS_VER}::ignition-utils${IGN_UTILS_VER}
+  PRIVATE
+    ${GDAL_LIBRARY})
+
+target_include_directories(${geospatial_target}
+  PRIVATE
+    ${GDAL_INCLUDE_DIR})
+
+ign_build_tests(TYPE UNIT SOURCES ${gtest_sources}
+  LIB_DEPS ${geospatial_target})

graphics/src/Dem.cc → geospatial/src/Dem.cc

@@ -16,12 +16,8 @@
 */
 #include <algorithm>
 
-#ifdef HAVE_GDAL
-# pragma GCC diagnostic push
-# pragma GCC diagnostic ignored "-Wfloat-equal"
-# include <ogr_spatialref.h>
-# pragma GCC diagnostic pop
-#endif
+#include <gdal_priv.h>
+#include <ogr_spatialref.h>
 
 #include "ignition/common/Console.hh"
 #include "ignition/common/Dem.hh"
@@ -30,8 +26,6 @@
 using namespace ignition;
 using namespace common;
 
-#ifdef HAVE_GDAL
-
 class ignition::common::Dem::Implementation
 {
   /// \brief A set of associated raster bands.
@@ -89,11 +83,11 @@ int Dem::Load(const std::string &_filename)
   // Sanity check
   std::string fullName = _filename;
   if (!exists(findFilePath(fullName)))
-    fullName = common::find_file(_filename);
+    fullName = common::findFile(_filename);
 
   if (!exists(findFilePath(fullName)))
   {
-    gzerr << "Unable to open DEM file[" << _filename
+    ignerr << "Unable to open DEM file[" << _filename
           << "], check your GAZEBO_RESOURCE_PATH settings." << std::endl;
     return -1;
   }
@@ -103,46 +97,55 @@ int Dem::Load(const std::string &_filename)
 
   if (this->dataPtr->dataSet == nullptr)
   {
-    gzerr << "Unable to open DEM file[" << fullName
+    ignerr << "Unable to open DEM file[" << fullName
           << "]. Format not recognised as a supported dataset." << std::endl;
     return -1;
   }
 
-  int nBands = this->dataPtr->dataSet->RasterCount();
+  int nBands = this->dataPtr->dataSet->GetRasterCount();
   if (nBands != 1)
   {
-    gzerr << "Unsupported number of bands in file [" << fullName + "]. Found "
+    ignerr << "Unsupported number of bands in file [" << fullName + "]. Found "
           << nBands << " but only 1 is a valid value." << std::endl;
     return -1;
   }
 
   // Set the pointer to the band
-  this->dataPtr->band = this->dataPtr->dataSet->RasterBand(1);
+  this->dataPtr->band = this->dataPtr->dataSet->GetRasterBand(1);
 
   // Raster width and height
-  xSize = this->dataPtr->dataSet->RasterXSize();
-  ySize = this->dataPtr->dataSet->RasterYSize();
+  xSize = this->dataPtr->dataSet->GetRasterXSize();
+  ySize = this->dataPtr->dataSet->GetRasterYSize();
 
   // Corner coordinates
-  upLeftX = 0.0;
-  upLeftY = 0.0;
-  upRightX = xSize;
-  upRightY = 0.0;
-  lowLeftX = 0.0;
-  lowLeftY = ySize;
-
-  // Calculate the georeferenced coordinates of the terrain corners
-  this->GeoReference(upLeftX, upLeftY, upLeftLat, upLeftLong);
-  this->GeoReference(upRightX, upRightY, upRightLat, upRightLong);
-  this->GeoReference(lowLeftX, lowLeftY, lowLeftLat, lowLeftLong);
-
-  // Set the world width and height
-  this->dataPtr->worldWidth =
-     math::SphericalCoordinates::Distance(upLeftLat, upLeftLong,
-                                            upRightLat, upRightLong);
-  this->dataPtr->worldHeight =
-     math::SphericalCoordinates::Distance(upLeftLat, upLeftLong,
-                                            lowLeftLat, lowLeftLong);
+  try
+  {
+    upLeftX = 0.0;
+    upLeftY = 0.0;
+    upRightX = xSize;
+    upRightY = 0.0;
+    lowLeftX = 0.0;
+    lowLeftY = ySize;
+
+    // Calculate the georeferenced coordinates of the terrain corners
+    this->GeoReference(upLeftX, upLeftY, upLeftLat, upLeftLong);
+    this->GeoReference(upRightX, upRightY, upRightLat, upRightLong);
+    this->GeoReference(lowLeftX, lowLeftY, lowLeftLat, lowLeftLong);
+
+    // Set the world width and height
+    this->dataPtr->worldWidth =
+       math::SphericalCoordinates::Distance(upLeftLat, upLeftLong,
+                                              upRightLat, upRightLong);
+    this->dataPtr->worldHeight =
+       math::SphericalCoordinates::Distance(upLeftLat, upLeftLong,
+                                              lowLeftLat, lowLeftLong);
+  }
+  catch (const std::invalid_argument &)
+  {
+    ignwarn << "Failed to automatically compute DEM size. "
+            << "Please use the <size> element to manually set DEM size."
+            << std::endl;
+  }
 
   // Set the terrain's side (the terrain will be squared after the padding)
   if (ignition::math::isPowerOfTwo(ySize - 1))
@@ -161,11 +164,34 @@ int Dem::Load(const std::string &_filename)
   if (this->LoadData() != 0)
     return -1;
 
-  // Set the min/max heights
-  this->dataPtr->minElevation = *std::min_element(&this->dataPtr->demData[0],
-      &this->dataPtr->demData[0] + this->dataPtr->side * this->dataPtr->side);
-  this->dataPtr->maxElevation = *std::max_element(&this->dataPtr->demData[0],
-      &this->dataPtr->demData[0] + this->dataPtr->side * this->dataPtr->side);
+  // Check for nodata value in dem data. This is used when computing the
+  // min elevation. If nodata value is not defined, we assume it will be one
+  // of the commonly used values such as -9999, -32768, etc.
+  // For simplicity, we will treat values <= -9999 as nodata values and
+  // ignore them when computing the min elevation.
+  int validNoData = 0;
+  const double defaultNoDataValue = -9999;
+  double noDataValue = this->dataPtr->band->GetNoDataValue(&validNoData);
+  if (validNoData <= 0)
+    noDataValue = defaultNoDataValue;
+
+  double min = ignition::math::MAX_D;
+  double max = -ignition::math::MAX_D;
+  for (auto d : this->dataPtr->demData)
+  {
+    if (d < min && d > noDataValue)
+      min = d;
+    if (d > max && d > noDataValue)
+      max = d;
+  }
+  if (ignition::math::equal(min, ignition::math::MAX_D) ||
+      ignition::math::equal(max, -ignition::math::MAX_D))
+  {
+    ignwarn << "Dem is composed of 'nodata' values!" << std::endl;
+  }
+
+  this->dataPtr->minElevation = min;
+  this->dataPtr->maxElevation = max;
 
   return 0;
 }
@@ -175,9 +201,11 @@ double Dem::Elevation(double _x, double _y)
 {
   if (_x >= this->Width() || _y >= this->Height())
   {
-    gzthrow("Illegal coordinates. You are asking for the elevation in (" <<
-          _x << "," << _y << ") but the terrain is [" << this->Width() <<
-           " x " << this->Height() << "]\n");
+    throw std::invalid_argument(
+      "Illegal coordinates. You are asking for the elevation in ("
+      + std::to_string(_x) + "," + std::to_string(_y) + ") but the terrain is ["
+      + std::to_string(this->Width()) + " x " + std::to_string(this->Height())
+      + "]\n");
   }
 
   return this->dataPtr->demData.at(_y * this->Width() + _x);
@@ -200,18 +228,26 @@ void Dem::GeoReference(double _x, double _y,
     ignition::math::Angle &_latitude, ignition::math::Angle &_longitude) const
 {
   double geoTransf[6];
-  if (this->dataPtr->dataSet->GeoTransform(geoTransf) == CE_None)
+  if (this->dataPtr->dataSet->GetGeoTransform(geoTransf) == CE_None)
   {
     OGRSpatialReference sourceCs;
     OGRSpatialReference targetCs;
     OGRCoordinateTransformation *cT;
     double xGeoDeg, yGeoDeg;
 
     // Transform the terrain's coordinate system to WGS84
-    char *importString = strdup(this->dataPtr->dataSet->ProjectionRef());
+    const char *importString
+        = strdup(this->dataPtr->dataSet->GetProjectionRef());
     sourceCs.importFromWkt(&importString);
     targetCs.SetWellKnownGeogCS("WGS84");
     cT = OGRCreateCoordinateTransformation(&sourceCs, &targetCs);
+    if (nullptr == cT)
+    {
+      throw std::invalid_argument(
+        "Unable to transform terrain coordinate system to WGS84 for "
+        "coordinates (" + std::to_string(_x) + ","
+        + std::to_string(_y) + ")");
+    }
 
     xGeoDeg = geoTransf[0] + _x * geoTransf[1] + _y * geoTransf[2];
     yGeoDeg = geoTransf[3] + _x * geoTransf[4] + _y * geoTransf[5];
@@ -220,10 +256,15 @@ void Dem::GeoReference(double _x, double _y,
 
     _latitude.Degree(yGeoDeg);
     _longitude.Degree(xGeoDeg);
+
+    OCTDestroyCoordinateTransformation(cT);
   }
   else
-    gzthrow("Unable to obtain the georeferenced values for coordinates ("
-            << _x << "," << _y << ")\n");
+  {
+    throw std::invalid_argument(
+      "Unable to obtain the georeferenced values for coordinates ("
+      + std::to_string(_x) + "," + std::to_string(_y) + ")\n");
+  }
 }
 
 //////////////////////////////////////////////////
@@ -265,7 +306,7 @@ void Dem::FillHeightMap(int _subSampling, unsigned int _vertSize,
 {
   if (_subSampling <= 0)
   {
-    gzerr << "Illegal subsampling value (" << _subSampling << ")\n";
+    ignerr << "Illegal subsampling value (" << _subSampling << ")\n";
     return;
   }
 
@@ -299,18 +340,18 @@ void Dem::FillHeightMap(int _subSampling, unsigned int _vertSize,
       double px4 = this->dataPtr->demData[y2 * this->dataPtr->side + x2];
       float h2 = (px3 - ((px3 - px4) * dx));
 
-      float h = (h1 - ((h1 - h2) * dy) - std::max(0.0f,
-          this->MinElevation())) * _scale.Z();
+      float h = this->dataPtr->minElevation +
+          (h1 - ((h1 - h2) * dy) - this->dataPtr->minElevation) * _scale.Z();
 
       // Invert pixel definition so 1=ground, 0=full height,
       // if the terrain size has a negative z component
       // this is mainly for backward compatibility
       if (_size.Z() < 0)
         h *= -1;
 
-      // Convert to 0 if a NODATA value is found
-      if (_size.Z() >= 0 && h < 0)
-        h = 0;
+      // Convert to minElevation if a NODATA value is found
+      if (_size.Z() >= 0 && h < this->dataPtr->minElevation)
+        h = this->dataPtr->minElevation;
 
       // Store the height for future use
       if (!_flipY)
@@ -326,14 +367,14 @@ int Dem::LoadData()
 {
     unsigned int destWidth;
     unsigned int destHeight;
-    unsigned int nXSize = this->dataPtr->dataSet->RasterXSize();
-    unsigned int nYSize = this->dataPtr->dataSet->RasterYSize();
+    unsigned int nXSize = this->dataPtr->dataSet->GetRasterXSize();
+    unsigned int nYSize = this->dataPtr->dataSet->GetRasterYSize();
     float ratio;
     std::vector<float> buffer;
 
     if (nXSize == 0 || nYSize == 0)
     {
-      gzerr << "Illegal size loading a DEM file (" << nXSize << ","
+      ignerr << "Illegal size loading a DEM file (" << nXSize << ","
             << nYSize << ")\n";
       return -1;
     }
@@ -360,7 +401,7 @@ int Dem::LoadData()
     if (this->dataPtr->band->RasterIO(GF_Read, 0, 0, nXSize, nYSize, &buffer[0],
                          destWidth, destHeight, GDT_Float32, 0, 0) != CE_None)
     {
-      gzerr << "Failure calling RasterIO while loading a DEM file\n";
+      ignerr << "Failure calling RasterIO while loading a DEM file\n";
       return -1;
     }
 
@@ -377,5 +418,3 @@ int Dem::LoadData()
 
     return 0;
 }
-
-#endif