Use gdal transforms

Author: Jaeyoung-Lim

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, const Eigen::Vector2d &map_center, Eigen::Vector2d &extent);
+  bool initializeFromVrt(const std::string& path, const Eigen::Vector2d& map_center, Eigen::Vector2d& extent);
 
   /**
    * @brief Load a color layer from a geotiff file (orthomosaic)

grid_map_geo/src/grid_map_geo.cpp

@@ -141,7 +141,8 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) {
   return true;
 }
 
-bool GridMapGeo::initializeFromVrt(const std::string &path, const Eigen::Vector2d &map_center, Eigen::Vector2d &extent) {
+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,9 +176,9 @@ bool GridMapGeo::initializeFromVrt(const std::string &path, const Eigen::Vector2
   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  
-  int grid_width = extent(0)/std::abs(resolution);
-  int grid_height = extent(1)/std::abs(resolution);
+  // 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);
@@ -195,13 +196,13 @@ bool GridMapGeo::initializeFromVrt(const std::string &path, const Eigen::Vector2
   grid_map_.add("elevation");
   GDALRasterBand *elevationBand = dataset->GetRasterBand(1);
 
-  Eigen::Vector2d center_px((map_center(1) - geoTransform[0])/geoTransform[1], (map_center(0) - geoTransform[3])/geoTransform[5]);
+  Eigen::Vector2d center_px((map_center(1) - geoTransform[0]) / geoTransform[1],
+                            (map_center(0) - 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);

grid_map_geo/src/terrain_loader.cpp

@@ -43,19 +43,42 @@
 #include <grid_map_ros/GridMapRosConverter.hpp>
 
 #include "grid_map_geo/geo_conversions.h"
+#include <gdal/ogr_spatialref.h>
+
+void transformCoordinates(int src_coord, const double &x, const double &y, const double &z, int tgt_coord,
+                          double &x_t, double &y_t, double &z_t) {
+  OGRSpatialReference source, target;
+  source.importFromEPSG(src_coord);
+  target.importFromEPSG(tgt_coord);
+
+  OGRPoint p;
+  p.setX(x);
+  p.setY(y);
+  p.setZ(z);
+  p.assignSpatialReference(&source);
+
+  p.transformTo(&target);
+  x_t = p.getX();
+  y_t = p.getY();
+  z_t = p.getZ();
+  return;
+}
 
-void LoadTerrainFromVrt(std::string path, const Eigen::Vector3d &query_position, const Eigen::Vector2d &offset, grid_map::GridMap &map, grid_map::GridMap &vrt_map_object) {
+void LoadTerrainFromVrt(std::string path, const Eigen::Vector3d &query_position, const Eigen::Vector2d &offset,
+                        grid_map::GridMap &map, grid_map::GridMap &vrt_map_object) {
   std::shared_ptr<GridMapGeo> vrt_map = std::make_shared<GridMapGeo>();
 
   Eigen::Vector3d query_position_lv03 = query_position;
   /// Convert LV03 to WGS84
   Eigen::Vector3d map_center_wgs84;  // Map center in WGS84
-  GeoConversions::reverse(query_position_lv03(0), query_position_lv03(1), query_position_lv03(2), map_center_wgs84.x(),
+  // GeoConversions::reverse(query_position_lv03(0), query_position_lv03(1), query_position_lv03(2), map_center_wgs84.x(),
+  //                         map_center_wgs84.y(), map_center_wgs84.z());
+  transformCoordinates(21781, query_position_lv03(0), query_position_lv03(1), query_position_lv03(2), 4326, map_center_wgs84.x(),
                           map_center_wgs84.y(), map_center_wgs84.z());
 
   std::cout << "Loading VRT Map:" << std::endl;
   std::cout << "  - map_center_wgs84:" << map_center_wgs84.transpose() << std::endl;
-  ///TODO: Discover extent from corners
+  /// TODO: Discover extent from corners
   Eigen::Vector2d extent_wgs84(0.5, 0.5);
   vrt_map->initializeFromVrt(path, map_center_wgs84.head(2), extent_wgs84);
   std::cout << "  - Success!" << std::endl;
@@ -70,9 +93,9 @@ void LoadTerrainFromVrt(std::string path, const Eigen::Vector3d &query_position,
   map.setFrameId("map");
   for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
     const grid_map::Index index = *iterator;
-    Eigen::Vector2d cell_position; // Position of cell relative to map coordinates
+    Eigen::Vector2d cell_position;  // Position of cell relative to map coordinates
     map.getPosition(index, cell_position);
-    auto cell_position_lv03 = cell_position + query_position_lv03.head(2);// Position of cell in CH1903/LV03
+    auto cell_position_lv03 = cell_position + query_position_lv03.head(2);  // Position of cell in CH1903/LV03
     double dummy;
     Eigen::Vector2d cell_position_wgs84;
     GeoConversions::reverse(cell_position_lv03(0), cell_position_lv03(1), 0.0, cell_position_wgs84.x(),
@@ -106,7 +129,7 @@ int main(int argc, char **argv) {
 
   for (int i = 0; i < 4; i++) {
     Eigen::Vector3d offset = static_cast<double>(i) * Eigen::Vector3d(2500.0, 2500.0, 0.0);
-    Eigen::Vector3d query_position = test_position +  offset;
+    Eigen::Vector3d query_position = test_position + offset;
 
     grid_map::GridMap map;
     grid_map::GridMap vrt_map;
@@ -117,13 +140,11 @@ int main(int argc, char **argv) {
     grid_map::GridMapRosConverter::toMessage(map, msg);
     map_pub.publish(msg);
 
-
     grid_map_msgs::GridMap msg2;
     grid_map::GridMapRosConverter::toMessage(vrt_map, msg2);
     map_pub2.publish(msg2);
   }
 
-
   ros::spin();
   return 0;
 }