Add map centering

* Needs more transform utilities
* Switch to std::array instead of raw double array
* Add ROS params for setting map origin
* Set RasterIo origin in pixel space based on user supplied geo origin
* Switch to GdalDatasetUniquePtr
Signed-off-by: Ryan Friedman <[email protected]>

Author: Ryanf55

include/grid_map_geo/grid_map_geo.hpp

@@ -44,10 +44,6 @@ static const std::string COLOR_MAP_DEFAULT_PATH{""};
 
 #include "grid_map_geo/transform.hpp"
 #include "tf2_ros/transform_broadcaster.h"
-struct Location {
-  ESPG espg{ESPG::WGS84};
-  Eigen::Vector3d position{Eigen::Vector3d::Zero()};
-};
 
 class GridMapGeo {
  public:

include/grid_map_geo/transform.hpp

@@ -47,9 +47,18 @@
 #endif
 
 #include <Eigen/Dense>
+#include <array>
+#include <cassert>
 
 enum class ESPG { ECEF = 4978, WGS84 = 4326, WGS84_32N = 32632, CH1903_LV03 = 21781 };
 
+struct Location {
+  ESPG espg{ESPG::WGS84};
+  // <east (lat), north (lng),  up (alt)>
+  //! @todo Switch to geographic_msgs/GeoPoint to make x-y not confusing?
+  Eigen::Vector3d position{Eigen::Vector3d::Zero()};
+};
+
 /**
  * @brief Helper function for transforming using gdal
  *
@@ -100,4 +109,81 @@ inline Eigen::Vector3d transformCoordinates(ESPG src_coord, const std::string wk
   return target_coordinates;
 }
 
+struct Corners {
+  ESPG espg{ESPG::WGS84};
+  Eigen::Vector2d top_left{Eigen::Vector2d::Zero()};
+  Eigen::Vector2d top_right{Eigen::Vector2d::Zero()};
+  Eigen::Vector2d bottom_left{Eigen::Vector2d::Zero()};
+  Eigen::Vector2d bottom_right{Eigen::Vector2d::Zero()};
+};
+
+/**
+ * @brief Helper function converting from image to geo coordinates
+ *
+ * @ref
+ https://gdal.org/tutorials/geotransforms_tut.html#transformation-from-image-coordinate-space-to-georeferenced-coordinate-space
+ * @see GDALApplyGeoTransform
+ *
+ * @param geoTransform The 6-element Geo transform
+ * @param imageCoords The image-coordinates <row, column>, also called <pixel, line>
+
+ * @return The geo-coordinates in <x, y>
+ */
+inline Eigen::Vector2d imageToGeo(const std::array<double, 6> geoTransform, const Eigen::Vector2i imageCoords) {
+  const auto x_pixel = imageCoords.x();
+  const auto y_line = imageCoords.y();
+
+  return {geoTransform.at(0) + x_pixel * geoTransform.at(1) + y_line * geoTransform.at(2),
+          geoTransform.at(3) + x_pixel * geoTransform.at(4) + y_line * geoTransform.at(5)};
+}
+
+/**
+ * @brief Helper function converting from geo to image coordinates. Assumes no rotation.
+ *        Uses the assumption that GT2 and GT4 are zero
+ *
+ * @ref
+ * https://gis.stackexchange.com/questions/384221/calculating-inverse-polynomial-transforms-for-pixel-sampling-when-map-georeferen
+ * @see GDALApplyGeoTransform
+ *
+ * @param geoTransform The 6-element forward Geo transform
+ * @param geoCoords The geo-coordinates in <x, y>
+ *
+ * @return The image-coordinates in <row, column>, also called <pixel, line>
+ */
+inline Eigen::Vector2d geoToImageNoRot(const std::array<double, 6>& geoTransform, const Eigen::Vector2d geoCoords) {
+  assert(geoTransform.at(2) == 0);  // assume no rotation
+  assert(geoTransform.at(4) == 0);  // assume no rotation
+
+  return {(geoCoords.x() - geoTransform.at(0)) / geoTransform.at(1),
+          (geoCoords.y() - geoTransform.at(3)) / geoTransform.at(5)};
+}
+
+/**
+ * @brief Helper function for getting dataset corners
+ * Inspired by gdalinfo_lib.cpp::GDALInfoReportCorner()
+ *
+ * @param datasetPtr The pointer to the dataset
+ * @param corners The returned corners in the geographic coordinates
+ * @return
+ */
+inline bool getGeoCorners(const GDALDatasetUniquePtr& datasetPtr, Corners& corners) {
+  double originX, originY, pixelSizeX, pixelSizeY;
+  std::array<double, 6> geoTransform;
+
+  // https://gdal.org/tutorials/geotransforms_tut.html#introduction-to-geotransforms
+  if (!datasetPtr->GetGeoTransform(geoTransform.data()) == CE_None) {
+    return false;
+  }
+
+  const auto raster_width = datasetPtr->GetRasterXSize();
+  const auto raster_height = datasetPtr->GetRasterYSize();
+
+  corners.top_left = imageToGeo(geoTransform, {0, 0});
+  corners.top_right = imageToGeo(geoTransform, {raster_width, 0});
+  corners.bottom_left = imageToGeo(geoTransform, {0, raster_height});
+  corners.bottom_right = imageToGeo(geoTransform, {raster_width, raster_height});
+
+  return true;
+}
+
 #endif

launch/load_vrt_launch.xml

@@ -1,12 +1,19 @@
 <launch>
     <arg name="rviz" default="false"/>
+
+    <!-- Default CMAC https://maps.app.goo.gl/NXJ9JC23oskMDaEx8-->
+    <arg name="map_origin_latitude" default="-35.363266"/>
+    <arg name="map_origin_longitude" default="149.165199"/>
+
     <node pkg="tf2_ros" exec="static_transform_publisher" name="world_map" args="--frame-id world --child-frame-id map"/>
 
     <node pkg="grid_map_geo" exec="map_publisher" name="map_publisher" output="screen"> 
 	    <param name="gdal_dataset_path" value="/vsizip/$(find-pkg-share grid_map_geo)/resources/ap_srtm1.vrt.zip"/>
+        <param name="map_origin_latitude" value="$(var map_origin_latitude)"/>
+        <param name="map_origin_longitude" value="$(var map_origin_longitude)"/>
     </node>
 
     <group if="$(var rviz)">
-        <node exec="rviz2" name="rviz2" pkg="rviz2" args="-d $(find-pkg-share grid_map_geo)/launch/config.rviz" />
+        <node exec="rviz2" name="rviz2" pkg="rviz2" args="-d $(find-pkg-share grid_map_geo)/launch/config.rviz"/>
     </group>
 </launch>

src/grid_map_geo.cpp

@@ -40,6 +40,7 @@
 #include "grid_map_geo/grid_map_geo.hpp"
 
 #include <array>
+#include <cassert>
 #include <grid_map_core/GridMapMath.hpp>
 #include <grid_map_core/iterators/CircleIterator.hpp>
 #include <grid_map_core/iterators/GridMapIterator.hpp>
@@ -137,10 +138,28 @@ bool GridMapGeo::initializeFromGdalDataset(const std::string &path) {
   grid_map::Length length(lengthX, lengthY);
   std::cout << "GMLX: " << lengthX << " GMLY: " << lengthY << std::endl;
 
-  double mapcenter_e = originX + pixelSizeX * grid_width * 0.5;
-  double mapcenter_n = originY + pixelSizeY * grid_height * 0.5;
-  maporigin_.espg = ESPG::CH1903_LV03;
-  maporigin_.position = Eigen::Vector3d(mapcenter_e, mapcenter_n, 0.0);
+  //! @todo use WGS-84 as map origin if specified
+  //! @todo
+  //! @todo check the origin point is non-void (not in the middle of the ocean)
+  Corners corners;
+  if (!getGeoCorners(dataset, corners)) {
+    std::cerr << "Failed to get geographic corners of dataset!" << std::endl;
+    return false;
+  }
+
+  if (std::isnan(maporigin_.position.x()) || std::isnan(maporigin_.position.y())) {
+    //! @todo Figure out how to not hard code the espg, perhaps using the dataset GEOGCRS attribute.
+    // maporigin_.espg = ESPG::CH1903_LV03;
+    const double mapcenter_e = (corners.top_left.x() + corners.bottom_right.x()) / 2.0;
+    const double mapcenter_n = (corners.top_left.y() + corners.bottom_right.y()) / 2.0;
+    maporigin_.position = Eigen::Vector3d(mapcenter_e, mapcenter_n, 0.0);
+  } else {
+    if (maporigin_.position.x() < corners.top_left.x() || maporigin_.position.x() > corners.bottom_right.x() ||
+        maporigin_.position.y() < corners.top_left.y() || maporigin_.position.y() > corners.bottom_left.y()) {
+      std::cerr << "Requested map origin is outside of raster dataset" << std::endl;
+      return false;
+    }
+  }
 
   Eigen::Vector2d position{Eigen::Vector2d::Zero()};
 
@@ -152,8 +171,13 @@ bool GridMapGeo::initializeFromGdalDataset(const std::string &path) {
   assert(raster_count == 1);  // expect only elevation data, otherwise it's the wrong dataset.
   GDALRasterBand *elevationBand = dataset->GetRasterBand(1);
 
+  // Compute the center in pixel space, then get the raster bounds nXOff and nYOff to extract with RasterIO
+  const auto center_px = geoToImageNoRot(geoTransform, {maporigin_.position.x(), maporigin_.position.y()});
+  const auto nxOff = center_px.x() - grid_width / 2;
+  const auto nYOff = center_px.y() - grid_height / 2;
+
   std::vector<float> data(grid_width * grid_height, 0.0f);
-  const auto raster_err = elevationBand->RasterIO(GF_Read, 0, 0, grid_width, grid_height, &data[0], grid_width,
+  const auto raster_err = elevationBand->RasterIO(GF_Read, nxOff, nYOff, grid_width, grid_height, &data[0], grid_width,
                                                   grid_height, GDT_Float32, 0, 0);
   if (raster_err != CPLE_None) {
     return false;