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