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

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

Author: Ryanf55

include/grid_map_geo/grid_map_geo.hpp

@@ -44,10 +44,6 @@ static const std::string COLOR_MAP_DEFAULT_PATH {""};
 
 
 #include <iostream>
-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
  *
@@ -74,6 +83,8 @@ inline Eigen::Vector3d transformCoordinates(ESPG src_coord, ESPG tgt_coord, cons
   return target_coordinates;
 }
 
+
+
 /**
  * @brief Helper function for transforming using gdal
  *
@@ -100,4 +111,83 @@ 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

@@ -43,6 +43,8 @@
 #include <grid_map_core/iterators/CircleIterator.hpp>
 #include <grid_map_core/iterators/GridMapIterator.hpp>
 
+#include <cassert>
+
 #if __APPLE__
 #include <cpl_string.h>
 #include <gdal.h>
@@ -83,16 +85,17 @@ bool GridMapGeo::Load(const std::string &map_path, bool algin_terrain, const std
 
 bool GridMapGeo::initializeFromGdalDataset(const std::string &path, bool align_terrain) {
   GDALAllRegister();
-  GDALDataset *dataset = (GDALDataset *)GDALOpen(path.c_str(), GA_ReadOnly);
+  GDALDatasetUniquePtr dataset;
+  dataset = GDALDatasetUniquePtr(GDALDataset::FromHandle(GDALOpen( path.c_str(), GA_ReadOnly )));
   if (!dataset) {
     std::cout << __func__ << "Failed to open '" << path << "'" << std::endl;
     return false;
   }
   std::cout << std::endl << "Loading GDAL Dataset for gridmap" << std::endl;
 
   double originX, originY, pixelSizeX, pixelSizeY;
-  double geoTransform[6];
-  if (dataset->GetGeoTransform(geoTransform) == CE_None) {
+  std::array<double, 6> geoTransform;
+  if (dataset->GetGeoTransform(geoTransform.data()) == CE_None) {
     originX = geoTransform[0];
     originY = geoTransform[3];
     pixelSizeX = geoTransform[1];
@@ -133,10 +136,30 @@ bool GridMapGeo::initializeFromGdalDataset(const std::string &path, bool align_t
   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()};
 
@@ -164,8 +187,13 @@ bool GridMapGeo::initializeFromGdalDataset(const std::string &path, bool align_t
   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, grid_height, GDT_Float32, 0, 0);
+  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;
   }
@@ -186,10 +214,10 @@ bool GridMapGeo::initializeFromGdalDataset(const std::string &path, bool align_t
     altitude = grid_map_.atPosition("elevation", Eigen::Vector2d(0.0, 0.0));
   }
 
-  Eigen::Translation3d meshlab_translation(0.0, 0.0, -altitude);
-  Eigen::AngleAxisd meshlab_rotation(Eigen::AngleAxisd::Identity());
-  Eigen::Isometry3d transform = meshlab_translation * meshlab_rotation;  // Apply affine transformation.
-  grid_map_ = grid_map_.getTransformedMap(transform, "elevation", grid_map_.getFrameId(), true);
+  // Eigen::Translation3d meshlab_translation(0.0, 0.0, -altitude);
+  // Eigen::AngleAxisd meshlab_rotation(Eigen::AngleAxisd::Identity());
+  // Eigen::Isometry3d transform = meshlab_translation * meshlab_rotation;  // Apply affine transformation.
+  // grid_map_ = grid_map_.getTransformedMap(transform, "elevation", grid_map_.getFrameId(), true);
   return true;
 }
 
@@ -203,8 +231,8 @@ bool GridMapGeo::addColorFromGeotiff(const std::string &path) {
   std::cout << std::endl << "Loading color layer from GeoTIFF file for gridmap" << std::endl;
 
   double originX, originY, pixelSizeX, pixelSizeY;
-  double geoTransform[6];
-  if (dataset->GetGeoTransform(geoTransform) == CE_None) {
+  std::array<double, 6> geoTransform;
+  if (dataset->GetGeoTransform(geoTransform.data()) == CE_None) {
     originX = geoTransform[0];
     originY = geoTransform[3];
     pixelSizeX = geoTransform[1];
@@ -264,8 +292,8 @@ bool GridMapGeo::addLayerFromGeotiff(const std::string &layer_name, const std::s
   std::cout << std::endl << "Loading color layer from GeoTIFF file for gridmap" << std::endl;
 
   double originX, originY, pixelSizeX, pixelSizeY;
-  double geoTransform[6];
-  if (dataset->GetGeoTransform(geoTransform) == CE_None) {
+  std::array<double, 6> geoTransform;
+  if (dataset->GetGeoTransform(geoTransform.data()) == CE_None) {
     originX = geoTransform[0];
     originY = geoTransform[3];
     pixelSizeX = geoTransform[1];

src/map_publisher.cpp

@@ -81,8 +81,34 @@ class MapPublisher : public rclcpp::Node {
       RCLCPP_INFO_STREAM(get_logger(), "gdal_dataset_path: '" << gdal_dataset_path << "'");
       RCLCPP_INFO_STREAM(get_logger(), "gdal_dataset_color_path: '" << color_path << "'");
 
+      rcl_interfaces::msg::ParameterDescriptor origin_descriptor;
+      origin_descriptor.read_only = true;
+      origin_descriptor.description = "Map origin latitude (WGS-84) in degrees.";
+      rcl_interfaces::msg::FloatingPointRange origin_range;
+
+      origin_range.from_value = -90.0;
+      origin_range.to_value = 90.0;
+      origin_descriptor.floating_point_range.push_back(origin_range);
+
+      static_assert(std::numeric_limits<double>::has_quiet_NaN == true, "Need quiet NaN for default value");
+      const auto map_origin_latitude = std::clamp(
+        this->declare_parameter("map_origin_latitude", std::numeric_limits<double>::quiet_NaN(), origin_descriptor),
+        origin_range.from_value,
+        origin_range.to_value);
+
+      origin_range.from_value = -180.0;
+      origin_range.to_value = 180.0;
+      origin_descriptor.floating_point_range.at(0) = origin_range;
+
+      origin_descriptor.description = "Map origin longitude (WGS-84) in degrees.";
+      const auto map_origin_longitude = std::clamp(
+        this->declare_parameter("map_origin_longitude", std::numeric_limits<double>::quiet_NaN(), origin_descriptor),
+        origin_range.from_value,
+        origin_range.to_value);
+
       map_ = std::make_shared<GridMapGeo>();
       map_->setMaxMapSizePixels(max_map_width, max_map_height);
+      map_->setGlobalOrigin(ESPG::WGS84, Eigen::Vector3d(map_origin_latitude, map_origin_longitude, 0.0));
       map_->Load(gdal_dataset_path, false, color_path);
 
       auto timer_callback = [this]() -> void {
@@ -95,9 +121,6 @@ class MapPublisher : public rclcpp::Node {
       timer_ = this->create_wall_timer(5s, timer_callback);
     }
   private:
-    void timer_callback() {
-
-    }
     rclcpp::TimerBase::SharedPtr timer_;
     rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr original_map_pub_;
     std::shared_ptr<GridMapGeo> map_;