Publish georeferencing frames (utm, enu) when loading a metric map wi…

…th georef. info
MOLAorg · Jan 19, 2025 · bdf5fb7 · bdf5fb7
1 parent 2e4b710
commit bdf5fb7
Show file tree

Hide file tree

Showing 2 changed files with 72 additions and 0 deletions.
diff --git a/module/include/mola_lidar_odometry/LidarOdometry.h b/module/include/mola_lidar_odometry/LidarOdometry.h
@@ -408,6 +408,10 @@ class LidarOdometry : public mola::FrontEndBase,
 
     /** When publishing pose updates, the vehicle frame name.*/
     std::string publish_vehicle_frame = "base_link";
+
+    std::string georef_map_reference_frame = "map";
+    std::string georef_map_utm_frame = "utm";
+    std::string georef_map_enu_frame = "enu";
   };
 
   /** Algorithm parameters */
@@ -493,6 +497,8 @@ class LidarOdometry : public mola::FrontEndBase,
 #endif
   void onExposeParameters();  // called after initialization
 
+  void publishMetricMapGeoreferencingData();
+
 private:
   struct ICP_Input
   {

diff --git a/module/src/LidarOdometry.cpp b/module/src/LidarOdometry.cpp
@@ -63,6 +63,7 @@
 #include <mrpt/system/datetime.h>
 #include <mrpt/system/filesystem.h>
 #include <mrpt/system/string_utils.h>
+#include <mrpt/topography/conversions.h>
 
 // GUI:
 #include <mrpt/gui/CDisplayWindowGUI.h>
@@ -314,6 +315,10 @@ void LidarOdometry::initialize_frontend(const Yaml & c)
   YAML_LOAD_OPT(params_, publish_reference_frame, std::string);
   YAML_LOAD_OPT(params_, publish_vehicle_frame, std::string);
 
+  YAML_LOAD_OPT(params_, georef_map_reference_frame, std::string);
+  YAML_LOAD_OPT(params_, georef_map_utm_frame, std::string);
+  YAML_LOAD_OPT(params_, georef_map_enu_frame, std::string);
+
   if (cfg.has("adaptive_threshold"))
     params_.adaptive_threshold.initialize(cfg["adaptive_threshold"]);
 
@@ -473,6 +478,8 @@ void LidarOdometry::initialize_frontend(const Yaml & c)
     bool loadOk =
       state_.local_map->load_from_file(params_.local_map_updates.load_existing_local_map);
     ASSERT_(loadOk);
+    // Publish geo-referenced data for the map, if applicable.
+    publishMetricMapGeoreferencingData();
   }
 
   if (!params_.simplemap.load_existing_simple_map.empty()) {
@@ -2378,6 +2385,9 @@ MapServer::ReturnStatus LidarOdometry::map_load(const std::string & path)
     ret.error_message = "Warning: no simplemap (keyframes) file found (expected: '"s + smFile +
                         "'). Required for multisession mapping. ";
 
+  // Publish geo-referenced data for the map, if applicable.
+  if (mmLoadOk) publishMetricMapGeoreferencingData();
+
   if (ret.success) {
     state_.local_map_needs_viz_update = true;  // refresh map in GUI, if enabled
 
@@ -2506,3 +2516,59 @@ void LidarOdometry::onExposeParameters()
   this->exposeParameters(nv);
 #endif
 }
+
+void LidarOdometry::publishMetricMapGeoreferencingData()
+{
+#if MOLA_VERSION_CHECK(1, 6, 0)  // we need lu.child_frame
+
+  ASSERT_(state_.local_map);
+  //TODO: Define new mola_kernel API for sending out geo-ref data and publish as mrpt_nav_interfaces::msg::GeoreferencingMetadata.
+
+  if (!state_.local_map->georeferencing.has_value()) return;  // no geo-ref data
+
+  const auto & g = state_.local_map->georeferencing.value();
+
+  MRPT_LOG_INFO_STREAM(
+    "Publishing map georeferencing metadata: T_enu_to_map="
+    << g.T_enu_to_map.asString()                           //
+    << " geo_coord.lat=" << g.geo_coord.lat.getAsString()  //
+    << " geo_coord.lon=" << g.geo_coord.lon.getAsString()  //
+    << " geo_coord.height=" << g.geo_coord.height          //
+  );
+
+  // ENU -> MAP
+  {
+    LocalizationUpdate lu;
+
+    const auto & T_enu_to_map = g.T_enu_to_map.mean;
+
+    lu.method = "map_server";
+    lu.reference_frame = params_.georef_map_enu_frame;    // "enu"
+    lu.child_frame = params_.georef_map_reference_frame;  // "map"
+    lu.timestamp = mrpt::Clock::now();                    // Anything better?
+    lu.pose = T_enu_to_map.asTPose();
+
+    advertiseUpdatedLocalization(lu);
+  }
+
+  // ENU -> UTM
+  {
+    LocalizationUpdate lu;
+    mrpt::topography::TUTMCoords utmCoordsOfENU;
+    int utmZone = 0;
+    char utmBand = 0;
+    mrpt::topography::GeodeticToUTM(g.geo_coord, utmCoordsOfENU, utmZone, utmBand);
+
+    // T_enu_to_utm = - utmCoordsOfENU  (without rotation, both are "ENU")
+    const auto T_enu_to_utm = mrpt::poses::CPose3D::FromTranslation(-utmCoordsOfENU);
+
+    lu.method = "map_server";
+    lu.reference_frame = params_.georef_map_enu_frame;  // "enu"
+    lu.child_frame = params_.georef_map_utm_frame;      // "utm"
+    lu.timestamp = mrpt::Clock::now();                  // Anything better?
+    lu.pose = T_enu_to_utm.asTPose();
+
+    advertiseUpdatedLocalization(lu);
+  }
+#endif
+}