@@ -44,6 +44,24 @@ class gz::common::Dem::Implementation
4444 // / \brief Terrain's side (after the padding).
4545 public: unsigned int side;
4646
47+ // / \brief The maximum length of data to load in the X direction.
48+ // / By default, load the entire raster.
49+ public: unsigned int maxXSize {std::numeric_limits<unsigned int >::max ()};
50+
51+ // / \brief The maximum length of data to load in the Y direction.
52+ // / By default, load the entire raster.
53+ public: unsigned int maxYSize {std::numeric_limits<unsigned int >::max ()};
54+
55+ // / \brief The desired length of data to load in the X direction.
56+ // / Internally, the implementation may use a
57+ // higher value for performance.
58+ public: unsigned int configuredXSize;
59+
60+ // / \brief The desired length of data to load in the Y direction.
61+ // / Internally, the implementation may use a
62+ // higher value for performance.
63+ public: unsigned int configuredYSize;
64+
4765 // / \brief Minimum elevation in meters.
4866 public: double minElevation;
4967
@@ -67,6 +85,12 @@ class gz::common::Dem::Implementation
6785 // / \brief Holds the spherical coordinates object from the world.
6886 public: math::SphericalCoordinates sphericalCoordinates =
6987 math::SphericalCoordinates ();
88+
89+ // / \brief Once the user configures size limits, apply that
90+ // / to the internal configured size, which is limited
91+ // / based on the dataset size.
92+ // / \return True if configured size was valid.
93+ public: [[nodiscard]] bool ConfigureLoadedSize ();
7094};
7195
7296// ////////////////////////////////////////////////
@@ -81,9 +105,8 @@ Dem::Dem()
81105Dem::~Dem ()
82106{
83107 this ->dataPtr ->demData .clear ();
84-
85108 if (this ->dataPtr ->dataSet )
86- GDALClose (reinterpret_cast < GDALDataset *> (this ->dataPtr ->dataSet ));
109+ GDALClose (GDALDataset::ToHandle (this ->dataPtr ->dataSet ));
87110}
88111
89112// ////////////////////////////////////////////////
@@ -93,12 +116,33 @@ void Dem::SetSphericalCoordinates(
93116 this ->dataPtr ->sphericalCoordinates = _worldSphericalCoordinates;
94117}
95118
119+ // ////////////////////////////////////////////////
120+ unsigned int Dem::RasterXSizeLimit ()
121+ {
122+ return this ->dataPtr ->maxXSize ;
123+ }
124+
125+ // ////////////////////////////////////////////////
126+ void Dem::SetRasterXSizeLimit (const unsigned int &_xLimit)
127+ {
128+ this ->dataPtr ->maxXSize = _xLimit;
129+ }
130+
131+ // ////////////////////////////////////////////////
132+ unsigned int Dem::RasterYSizeLimit ()
133+ {
134+ return this ->dataPtr ->maxYSize ;
135+ }
136+
137+ // ////////////////////////////////////////////////
138+ void Dem::SetRasterYSizeLimit (const unsigned int &_yLimit)
139+ {
140+ this ->dataPtr ->maxYSize = _yLimit;
141+ }
142+
96143// ////////////////////////////////////////////////
97144int Dem::Load (const std::string &_filename)
98145{
99- unsigned int width;
100- unsigned int height;
101- int xSize, ySize;
102146 double upLeftX, upLeftY, upRightX, upRightY, lowLeftX, lowLeftY;
103147 gz::math::Angle upLeftLat, upLeftLong, upRightLat, upRightLong;
104148 gz::math::Angle lowLeftLat, lowLeftLong;
@@ -110,21 +154,34 @@ int Dem::Load(const std::string &_filename)
110154
111155 this ->dataPtr ->filename = fullName;
112156
113- if (!exists (findFilePath (fullName)))
157+ // Create a re-usable lambda for opening a dataset.
158+ auto openInGdal = [this ](const std::string& name) -> bool
114159 {
115- gzerr << " Unable to find DEM file[" << _filename << " ]." << std::endl;
116- return -1 ;
117- }
160+ GDALDatasetH handle = GDALOpen (name.c_str (), GA_ReadOnly);
161+ if (handle)
162+ {
163+ this ->dataPtr ->dataSet = GDALDataset::FromHandle (handle);
164+ }
118165
119- this ->dataPtr ->dataSet = reinterpret_cast <GDALDataset *>( GDALOpen (
120- fullName. c_str (), GA_ReadOnly)) ;
166+ return this ->dataPtr ->dataSet != nullptr ;
167+ } ;
121168
122- if (this -> dataPtr -> dataSet == nullptr )
169+ if (! exists ( findFilePath (fullName)) )
123170 {
171+ // https://github.com/gazebosim/gz-common/issues/596
172+ // Attempt loading anyways to support /vsicurl, /vsizip, and other
173+ // GDAL Virtual File Formats.
174+ // The "exists()" function does not handle GDAL's special formats.
175+ if (!openInGdal (_filename)) {
176+ gzerr << " Unable to read DEM file[" << _filename << " ]." << std::endl;
177+ return -1 ;
178+ }
179+ } else if (!openInGdal (fullName)){
124180 gzerr << " Unable to open DEM file[" << fullName
125181 << " ]. Format not recognized as a supported dataset." << std::endl;
126182 return -1 ;
127183 }
184+ assert (this ->dataPtr ->dataSet != nullptr );
128185
129186 int nBands = this ->dataPtr ->dataSet ->GetRasterCount ();
130187 if (nBands != 1 )
@@ -137,9 +194,16 @@ int Dem::Load(const std::string &_filename)
137194 // Set the pointer to the band
138195 this ->dataPtr ->band = this ->dataPtr ->dataSet ->GetRasterBand (1 );
139196
197+ // Validate the raster size and apply the user-configured size limits
198+ // on loaded data.
199+ if (!this ->dataPtr ->ConfigureLoadedSize ())
200+ {
201+ return -1 ;
202+ }
203+
140204 // Raster width and height
141- xSize = this ->dataPtr ->dataSet -> GetRasterXSize () ;
142- ySize = this ->dataPtr ->dataSet -> GetRasterYSize () ;
205+ const int xSize = this ->dataPtr ->configuredXSize ;
206+ const int ySize = this ->dataPtr ->configuredYSize ;
143207
144208 // Corner coordinates
145209 upLeftX = 0.0 ;
@@ -173,16 +237,20 @@ int Dem::Load(const std::string &_filename)
173237 }
174238
175239 // Set the terrain's side (the terrain will be squared after the padding)
240+
241+ unsigned int height;
176242 if (gz::math::isPowerOfTwo (ySize - 1 ))
177243 height = ySize;
178244 else
179245 height = gz::math::roundUpPowerOfTwo (ySize) + 1 ;
180246
247+ unsigned int width;
181248 if (gz::math::isPowerOfTwo (xSize - 1 ))
182249 width = xSize;
183250 else
184251 width = gz::math::roundUpPowerOfTwo (xSize) + 1 ;
185252
253+ // ! @todo use by limits.
186254 this ->dataPtr ->side = std::max (width, height);
187255
188256 // Preload the DEM's data
@@ -226,7 +294,7 @@ int Dem::Load(const std::string &_filename)
226294 if (gz::math::equal (min, gz::math::MAX_D) ||
227295 gz::math::equal (max, -gz::math::MAX_D))
228296 {
229- gzwarn << " DEM is composed of 'nodata' values!" << std::endl;
297+ gzwarn << " The DEM contains 'nodata' values!" << std::endl;
230298 }
231299
232300 this ->dataPtr ->minElevation = min;
@@ -281,6 +349,7 @@ bool Dem::GeoReference(double _x, double _y,
281349 }
282350
283351 double geoTransf[6 ];
352+ assert (this ->dataPtr ->dataSet != nullptr );
284353 if (this ->dataPtr ->dataSet ->GetGeoTransform (geoTransf) == CE_None)
285354 {
286355 OGRCoordinateTransformation *cT = nullptr ;
@@ -448,44 +517,64 @@ void Dem::FillHeightMap(int _subSampling, unsigned int _vertSize,
448517 }
449518}
450519
451- // ////////////////////////////////////////////////
452- int Dem::LoadData ()
520+ bool gz::common::Dem::Implementation::ConfigureLoadedSize ()
453521{
454- unsigned int nXSize = this ->dataPtr ->dataSet ->GetRasterXSize ();
455- unsigned int nYSize = this ->dataPtr ->dataSet ->GetRasterYSize ();
456- if (nXSize == 0 || nYSize == 0 )
522+ assert (this ->dataSet != nullptr );
523+ const unsigned int nRasterXSize = this ->dataSet ->GetRasterXSize ();
524+ const unsigned int nRasterYSize = this ->dataSet ->GetRasterYSize ();
525+ if (nRasterXSize == 0 || nRasterYSize == 0 )
457526 {
458- gzerr << " Illegal size loading a DEM file (" << nXSize << " ,"
459- << nYSize << " )\n " ;
460- return - 1 ;
527+ gzerr << " Illegal raster size loading a DEM file (" << nRasterXSize << " ,"
528+ << nRasterYSize << " )\n " ;
529+ return false ;
461530 }
462531
532+ this ->configuredXSize = std::min (nRasterXSize, this ->maxXSize );
533+ this ->configuredYSize = std::min (nRasterYSize, this ->maxYSize );
534+ return true ;
535+ }
536+
537+ // ////////////////////////////////////////////////
538+ int Dem::LoadData ()
539+ {
540+ // Capture a local for re-use.
541+ const auto desiredXSize = this ->dataPtr ->configuredXSize ;
542+ const auto desiredYSize = this ->dataPtr ->configuredYSize ;
543+
463544 // Scale the terrain keeping the same ratio between width and height
464545 unsigned int destWidth;
465546 unsigned int destHeight;
466547 float ratio;
467- if (nXSize > nYSize )
548+ if (desiredXSize > desiredYSize )
468549 {
469- ratio = static_cast <float >(nXSize ) / static_cast <float >(nYSize );
550+ ratio = static_cast <float >(desiredXSize ) / static_cast <float >(desiredYSize );
470551 destWidth = this ->dataPtr ->side ;
471552 // The decimal part is discarted for interpret the result as pixels
472553 float h = static_cast <float >(destWidth) / static_cast <float >(ratio);
473554 destHeight = static_cast <unsigned int >(h);
474555 }
475556 else
476557 {
477- ratio = static_cast <float >(nYSize ) / static_cast <float >(nXSize );
558+ ratio = static_cast <float >(desiredYSize ) / static_cast <float >(desiredXSize );
478559 destHeight = this ->dataPtr ->side ;
479560 // The decimal part is discarted for interpret the result as pixels
480561 float w = static_cast <float >(destHeight) / static_cast <float >(ratio);
481562 destWidth = static_cast <unsigned int >(w);
482563 }
483564
484565 // Read the whole raster data and convert it to a GDT_Float32 array.
485- // In this step the DEM is scaled to destWidth x destHeight
566+ // In this step the DEM is scaled to destWidth x destHeight.
567+
486568 std::vector<float > buffer;
487- buffer.resize (destWidth * destHeight);
488- if (this ->dataPtr ->band ->RasterIO (GF_Read, 0 , 0 , nXSize, nYSize, &buffer[0 ],
569+ // Convert to uint64_t for multiplication to avoid overflow.
570+ // https://github.com/OSGeo/gdal/issues/9713
571+ buffer.resize (static_cast <uint64_t >(destWidth) * \
572+ static_cast <uint64_t >(destHeight));
573+ // ! @todo Do not assume users only want to load
574+ // ! from the origin of the dataset.
575+ // Instead, add a configuration to change where in the dataset to read from.
576+ if (this ->dataPtr ->band ->RasterIO (GF_Read, 0 , 0 ,
577+ desiredXSize, desiredYSize, &buffer[0 ],
489578 destWidth, destHeight, GDT_Float32, 0 , 0 ) != CE_None)
490579 {
491580 gzerr << " Failure calling RasterIO while loading a DEM file\n " ;
@@ -494,8 +583,9 @@ int Dem::LoadData()
494583
495584 // Copy and align 'buffer' into the target vector. The destination vector is
496585 // initialized to max() and later converted to the minimum elevation, so all
497- // the points not contained in 'buffer' will be extra padding
498- this ->dataPtr ->demData .resize (this ->Width () * this ->Height (),
586+ // the points not contained in 'buffer' will be extra padding.
587+ this ->dataPtr ->demData .resize (static_cast <uint64_t >(this ->Width ()) * \
588+ static_cast <uint64_t >(this ->Height ()),
499589 this ->dataPtr ->bufferVal );
500590 for (unsigned int y = 0 ; y < destHeight; ++y)
501591 {
0 commit comments