From 5c42d0bace7f4943cc492a11b8fc36b307f45819 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco-Claraco <joseluisblancoc@gmail.com>
Date: Fri, 31 Jan 2025 09:34:15 +0100
Subject: [PATCH] mola_kernel: Add Georeferencing structure and add it to map
 updates

---
 mola_kernel/CMakeLists.txt                    |  1 +
 .../include/mola_kernel/Georeferencing.h      | 41 +++++++++++++++++++
 .../mola_kernel/interfaces/MapSourceBase.h    | 11 ++++-
 .../mola_kernel/interfaces/NavStateFilter.h   | 30 --------------
 4 files changed, 52 insertions(+), 31 deletions(-)
 create mode 100644 mola_kernel/include/mola_kernel/Georeferencing.h

diff --git a/mola_kernel/CMakeLists.txt b/mola_kernel/CMakeLists.txt
index b88e0ce..be60d45 100644
--- a/mola_kernel/CMakeLists.txt
+++ b/mola_kernel/CMakeLists.txt
@@ -64,6 +64,7 @@ set(LIB_PUBLIC_HDRS
   include/mola_kernel/entities/RelPose3KF.h
   include/mola_kernel/Entity.h
   include/mola_kernel/Factor.h
+  include/mola_kernel/Georeferencing.h
   include/mola_kernel/factors/FactorBase.h
   include/mola_kernel/factors/FactorConstVelKinematics.h
   include/mola_kernel/factors/FactorRelativePose3.h
diff --git a/mola_kernel/include/mola_kernel/Georeferencing.h b/mola_kernel/include/mola_kernel/Georeferencing.h
new file mode 100644
index 0000000..8337997
--- /dev/null
+++ b/mola_kernel/include/mola_kernel/Georeferencing.h
@@ -0,0 +1,41 @@
+/* -------------------------------------------------------------------------
+ *   A Modular Optimization framework for Localization and mApping  (MOLA)
+ * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
+ * See LICENSE for license information.
+ * ------------------------------------------------------------------------- */
+/**
+ * @file   Georeferencing.h
+ * @brief  Defines a common georeferencing data structure, similar to mp2p_icp
+ * but independent
+ * @author Jose Luis Blanco Claraco
+ * @date   Jan 31, 2025
+ */
+#pragma once
+
+#include <mrpt/poses/CPose3DPDFGaussian.h>
+#include <mrpt/topography/data_types.h>
+
+namespace mola
+{
+/** Defines a common georeferencing data structure, similar to mp2p_icp but
+ * independent of that library.
+ *
+ * \ingroup mola_kernel_grp */
+struct Georeferencing
+{
+    Georeferencing() = default;
+
+    /** The geodetic coordinates (on WGS-84) of the metric map ENU frame of
+     * reference. */
+    mrpt::topography::TGeodeticCoords geo_coord;
+
+    /** The SE(3) transformation from the ENU (earth-north-up) frame
+     * to the metric map local frame of reference.
+     * If this is the identity (default) it means the map is already in
+     * ENU coordinates (i.e. +X is East, +Y is North, +Z is up) and
+     * the point (0,0,0) is the one having the geodetic coordinates
+     * geo_coord
+     */
+    mrpt::poses::CPose3DPDFGaussian T_enu_to_map;
+};
+}  // namespace mola
diff --git a/mola_kernel/include/mola_kernel/interfaces/MapSourceBase.h b/mola_kernel/include/mola_kernel/interfaces/MapSourceBase.h
index 739d35c..8a5d4fc 100644
--- a/mola_kernel/include/mola_kernel/interfaces/MapSourceBase.h
+++ b/mola_kernel/include/mola_kernel/interfaces/MapSourceBase.h
@@ -11,6 +11,7 @@
  */
 #pragma once
 
+#include <mola_kernel/Georeferencing.h>
 #include <mrpt/maps/CMetricMap.h>
 
 #include <functional>
@@ -21,7 +22,8 @@
 
 namespace mola
 {
-/** Virtual interface for SLAM/odometry methods publishing a map
+/** Virtual interface for SLAM/odometry methods publishing a map and/or
+ * georeferencing information.
  *
  * \ingroup mola_kernel_interfaces_grp */
 class MapSourceBase
@@ -47,7 +49,14 @@ class MapSourceBase
         /** Map layer/submap name */
         std::string map_name = "local_map";
 
+        /** Might be null if georeferencing is provided, but both can be also
+         * populated. */
         mrpt::maps::CMetricMap::Ptr map;
+
+        /** If the map is georeferenced, its metadata
+         *  \note Added in MOLA 1.7.0
+         */
+        std::optional<Georeferencing> georeferencing;
     };
 
     using map_updates_callback_t = std::function<void(const MapUpdate&)>;
diff --git a/mola_kernel/include/mola_kernel/interfaces/NavStateFilter.h b/mola_kernel/include/mola_kernel/interfaces/NavStateFilter.h
index b8fdaee..77295be 100644
--- a/mola_kernel/include/mola_kernel/interfaces/NavStateFilter.h
+++ b/mola_kernel/include/mola_kernel/interfaces/NavStateFilter.h
@@ -121,36 +121,6 @@ class NavStateFilter : public mola::ExecutableBase
     {
         return {};  // Default: none
     }
-
-    /** Must be invoked with the mp2p_icp metric map geo-referencing information
-     *  of the map in order to have GNSS observations correctly fused.
-     */
-    void set_georeferencing_params(
-        /** The geodetic coordinates (on WGS-84) of the metric map ENU frame of
-         * reference. */
-        mrpt::topography::TGeodeticCoords geo_coord,
-        /** The SE(3) transformation from the ENU (earth-north-up) frame
-         * to the metric map local frame of reference.
-         * If this is the identity (default) it means the map is already in
-         * ENU coordinates (i.e. +X is East, +Y is North, +Z is up) and
-         * the point (0,0,0) is the one having the geodetic coordinates
-         * geo_coord
-         */
-        mrpt::poses::CPose3DPDFGaussian T_enu_to_map)
-    {
-        auto& g = geoRefParams_.emplace();
-
-        g.geo_coord    = geo_coord;
-        g.T_enu_to_map = T_enu_to_map;
-    }
-
-   protected:
-    struct GeoReferenceParams
-    {
-        mrpt::topography::TGeodeticCoords geo_coord;
-        mrpt::poses::CPose3DPDFGaussian   T_enu_to_map;
-    };
-    std::optional<GeoReferenceParams> geoRefParams_;
 };
 
 }  // namespace mola