@@ -141,6 +141,102 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) {
141141 return true ;
142142}
143143
144+ bool GridMapGeo::initializeFromVrt (const std::string &path, const Eigen::Vector2d &map_center,
145+ Eigen::Vector2d &extent) {
146+ GDALAllRegister ();
147+ GDALDataset *dataset = (GDALDataset *)GDALOpen (path.c_str (), GA_ReadOnly);
148+ if (!dataset) {
149+ std::cout << " Failed to open"
150+ return false ;
151+ }
152+ std::cout << std::endl << " Loading GeoTIFF file for gridmap"
153+
154+ double originX, originY, pixelSizeX, pixelSizeY;
155+ double geoTransform[6 ];
156+ if (dataset->GetGeoTransform (geoTransform) == CE_None) {
157+ originX = geoTransform[0 ];
158+ originY = geoTransform[3 ];
159+ pixelSizeX = geoTransform[1 ];
160+ pixelSizeY = geoTransform[5 ];
161+ } else {
162+ std::cout << " Failed read geotransform"
163+ return false ;
164+ }
165+
166+ const char *pszProjection = dataset->GetProjectionRef ();
167+ std::cout << std::endl << " Wkt ProjectionRef: "
168+
169+ const OGRSpatialReference *spatial_ref = dataset->GetSpatialRef ();
170+ std::string name_coordinate = spatial_ref->GetAttrValue (" geogcs"
171+ std::string epsg_code = spatial_ref->GetAttrValue (" AUTHORITY" 1 );
172+ // Get image metadata
173+ unsigned width = dataset->GetRasterXSize ();
174+ unsigned height = dataset->GetRasterYSize ();
175+ double resolution = pixelSizeX;
176+ std::cout << " Width: " " Height: " " Resolution: "
177+
178+ // pixelSizeY is negative because the origin of the image is the north-east corner and positive
179+ // Y pixel coordinates go towards the south
180+ int grid_width = extent (0 ) / std::abs (resolution);
181+ int grid_height = extent (1 ) / std::abs (resolution);
182+ const double lengthX = resolution * grid_width;
183+ const double lengthY = resolution * grid_height;
184+ grid_map::Length length (lengthX, lengthY);
185+ std::cout << " length: " transpose () << std::endl;
186+
187+ maporigin_.espg = static_cast <ESPG>(std::stoi (epsg_code));
188+ maporigin_.position = map_center.head (2 );
189+
190+ Eigen::Vector2d position{Eigen::Vector2d::Zero ()};
191+
192+ grid_map_.setGeometry (length, resolution, position);
193+ std::cout << " position: " transpose () << std::endl;
194+ // / TODO: Use TF for geocoordinates
195+ grid_map_.setFrameId (frame_id_);
196+ grid_map_.add (" elevation"
197+ GDALRasterBand *elevationBand = dataset->GetRasterBand (1 );
198+
199+ Eigen::Vector2d center_px ((map_center (1 ) - geoTransform[0 ]) / geoTransform[1 ],
200+ (map_center (0 ) - geoTransform[3 ]) / geoTransform[5 ]);
201+
202+ const auto raster_io_x_offset = center_px.x () - grid_width / 2 ;
203+ const auto raster_io_y_offset = center_px.y () - grid_height / 2 ;
204+ std::cout << " center_px: " transpose () << std::endl;
205+
206+ std::vector<float > data (grid_width * grid_height, 0 .0f );
207+ const auto raster_err = elevationBand->RasterIO (GF_Read, raster_io_x_offset, raster_io_y_offset, grid_width,
208+ grid_height, &data[0 ], grid_width, grid_height, GDT_Float32, 0 , 0 );
209+ if (raster_err != CPLE_None) {
210+ std::cout << " Error loading raster"
211+ return false ;
212+ }
213+ grid_map::Matrix &layer_elevation = grid_map_[" elevation"
214+ for (grid_map::GridMapIterator iterator (grid_map_); !iterator.isPastEnd (); ++iterator) {
215+ const grid_map::Index gridMapIndex = *iterator;
216+ // TODO: This may be wrong if the pixelSizeY > 0
217+ int x = grid_width - 1 - gridMapIndex (0 );
218+ int y = gridMapIndex (1 );
219+
220+ layer_elevation (x, y) = data[gridMapIndex (0 ) + grid_width * gridMapIndex (1 )];
221+ }
222+
223+ static tf2_ros::StaticTransformBroadcaster static_broadcaster;
224+ geometry_msgs::TransformStamped static_transformStamped;
225+
226+ static_transformStamped.header .stamp = ros::Time::now ();
227+ static_transformStamped.header .frame_id = name_coordinate;
228+ static_transformStamped.child_frame_id = frame_id_;
229+ static_transformStamped.transform .translation .x = map_center (0 );
230+ static_transformStamped.transform .translation .y = map_center (1 );
231+ static_transformStamped.transform .translation .z = 0.0 ;
232+ static_transformStamped.transform .rotation .x = 0.0 ;
233+ static_transformStamped.transform .rotation .y = 0.0 ;
234+ static_transformStamped.transform .rotation .z = 0.0 ;
235+ static_transformStamped.transform .rotation .w = 1.0 ;
236+ static_broadcaster.sendTransform (static_transformStamped);
237+ return true ;
238+ }
239+
144240bool GridMapGeo::addColorFromGeotiff (const std::string &path) {
145241 GDALAllRegister ();
146242 GDALDataset *dataset = (GDALDataset *)GDALOpen (path.c_str (), GA_ReadOnly);
0 commit comments