From 5f0da739f6aeb9378f06043e3b2eafbafd899cb7 Mon Sep 17 00:00:00 2001
From: Jaeyoung-Lim <jalim@ethz.ch>
Date: Mon, 8 Apr 2024 18:26:36 +0200
Subject: [PATCH] Load position and extent from VRT server

F
---
 .../include/grid_map_geo/grid_map_geo.h       |  2 +-
 grid_map_geo/src/grid_map_geo.cpp             | 36 +++++++++++++------
 2 files changed, 26 insertions(+), 12 deletions(-)

diff --git a/grid_map_geo/include/grid_map_geo/grid_map_geo.h b/grid_map_geo/include/grid_map_geo/grid_map_geo.h
index 248a703..9d2ee62 100644
--- a/grid_map_geo/include/grid_map_geo/grid_map_geo.h
+++ b/grid_map_geo/include/grid_map_geo/grid_map_geo.h
@@ -107,7 +107,7 @@ class GridMapGeo {
    */
   bool initializeFromGeotiff(const std::string& path);
 
-  bool initializeFromVrt(const std::string& path);
+  bool initializeFromVrt(const std::string& path, const Eigen::Vector2d &map_center, Eigen::Vector2d &extent);
 
   /**
    * @brief Load a color layer from a geotiff file (orthomosaic)
diff --git a/grid_map_geo/src/grid_map_geo.cpp b/grid_map_geo/src/grid_map_geo.cpp
index 5cec16f..82b8d21 100644
--- a/grid_map_geo/src/grid_map_geo.cpp
+++ b/grid_map_geo/src/grid_map_geo.cpp
@@ -141,7 +141,7 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) {
   return true;
 }
 
-bool GridMapGeo::initializeFromVrt(const std::string &path) {
+bool GridMapGeo::initializeFromVrt(const std::string &path, const Eigen::Vector2d &map_center, Eigen::Vector2d &extent) {
   GDALAllRegister();
   GDALDataset *dataset = (GDALDataset *)GDALOpen(path.c_str(), GA_ReadOnly);
   if (!dataset) {
@@ -175,35 +175,50 @@ bool GridMapGeo::initializeFromVrt(const std::string &path) {
   std::cout << "Width: " << width << " Height: " << height << " Resolution: " << resolution << std::endl;
 
   // pixelSizeY is negative because the origin of the image is the north-east corner and positive
-  // Y pixel coordinates go towards the south
-  const double lengthX = resolution * width;
-  const double lengthY = resolution * height;
+  // Y pixel coordinates go towards the south  
+  int grid_width = extent(0)/std::abs(resolution);
+  int grid_height = extent(1)/std::abs(resolution);
+  const double lengthX = resolution * grid_width;
+  const double lengthY = resolution * grid_height;
   grid_map::Length length(lengthX, lengthY);
+  std::cout << "length: " << length.transpose() << std::endl;
 
-  double mapcenter_e = originX + pixelSizeX * width * 0.5;
-  double mapcenter_n = originY + pixelSizeY * height * 0.5;
+  double mapcenter_e = map_center(0);
+  double mapcenter_n = map_center(1);
   maporigin_.espg = static_cast<ESPG>(std::stoi(epsg_code));
   maporigin_.position = Eigen::Vector3d(mapcenter_e, mapcenter_n, 0.0);
 
   Eigen::Vector2d position{Eigen::Vector2d::Zero()};
 
   grid_map_.setGeometry(length, resolution, position);
+  std::cout << "position: " << position.transpose() << std::endl;
   /// TODO: Use TF for geocoordinates
   grid_map_.setFrameId(frame_id_);
   grid_map_.add("elevation");
   GDALRasterBand *elevationBand = dataset->GetRasterBand(1);
 
-  std::vector<float> data(width * height, 0.0f);
-  elevationBand->RasterIO(GF_Read, 0, 0, width, height, &data[0], width, height, GDT_Float32, 0, 0);
+  Eigen::Vector2d center_px((mapcenter_e - geoTransform[0])/geoTransform[1], (mapcenter_n - geoTransform[3])/geoTransform[5]);
 
+  const auto raster_io_x_offset = center_px.x() - grid_width / 2;
+  const auto raster_io_y_offset = center_px.y() - grid_height / 2;
+  std::cout << "center_px: " << center_px.transpose() << std::endl;
+
+
+  std::vector<float> data(grid_width * grid_height, 0.0f);
+  const auto raster_err = elevationBand->RasterIO(GF_Read, raster_io_x_offset, raster_io_y_offset, grid_width,
+                                                  grid_height, &data[0], grid_width, grid_height, GDT_Float32, 0, 0);
+  if (raster_err != CPLE_None) {
+    std::cout << "Error loading raster" << std::endl;
+    return false;
+  }
   grid_map::Matrix &layer_elevation = grid_map_["elevation"];
   for (grid_map::GridMapIterator iterator(grid_map_); !iterator.isPastEnd(); ++iterator) {
     const grid_map::Index gridMapIndex = *iterator;
     // TODO: This may be wrong if the pixelSizeY > 0
-    int x = width - 1 - gridMapIndex(0);
+    int x = grid_width - 1 - gridMapIndex(0);
     int y = gridMapIndex(1);
 
-    layer_elevation(x, y) = data[gridMapIndex(0) + width * gridMapIndex(1)];
+    layer_elevation(x, y) = data[gridMapIndex(0) + grid_width * gridMapIndex(1)];
   }
 
   static tf2_ros::StaticTransformBroadcaster static_broadcaster;
@@ -220,7 +235,6 @@ bool GridMapGeo::initializeFromVrt(const std::string &path) {
   static_transformStamped.transform.rotation.z = 0.0;
   static_transformStamped.transform.rotation.w = 1.0;
   static_broadcaster.sendTransform(static_transformStamped);
-
   return true;
 }