From fab160bbee4941b204b0eaecb39f4e3502580524 Mon Sep 17 00:00:00 2001
From: Michal Pelka <michalpelka@gmail.com>
Date: Mon, 4 May 2026 13:36:40 +0200
Subject: [PATCH] Fix undulation handling for NMEA messages

Signed-off-by: Michal Pelka <michalpelka@gmail.com>
---
 .../multi_view_tls_registration_gui.cpp              |  2 +-
 core/include/Core/gnss.h                             |  6 ++++--
 core/include/Core/nmea.h                             |  1 +
 core/src/gnss.cpp                                    | 12 ++++++++----
 core/src/nmea.cpp                                    |  1 +
 5 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp
index 7e9037cd..1e3fcda0 100644
--- a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp
+++ b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp
@@ -3232,7 +3232,7 @@ void display()
                             std::vector<double> timestamps;
                             for (const auto& gnss : tls_registration.gnss.gnss_poses)
                             {
-                                lla_points.emplace_back(gnss.lat, gnss.lon, gnss.alt);
+                                lla_points.emplace_back(gnss.lat, gnss.lon, gnss.h_wgs84);
                                 intensity.push_back(gnss.hdop);
                                 timestamps.push_back(gnss.timestamp);
                             }
diff --git a/core/include/Core/gnss.h b/core/include/Core/gnss.h
index 19227211..c2d9b8e7 100644
--- a/core/include/Core/gnss.h
+++ b/core/include/Core/gnss.h
@@ -21,8 +21,10 @@ class GNSS
     struct GlobalPose
     {
         double timestamp;
-        double lat;
-        double lon;
+        double lat; // WGS-84 Ellipsoid
+        double lon; // WGS-84 Ellipsoid
+        double h_wgs84; // height above WGS-84 ellipsoid
+        double undulation;
         double alt;
         double hdop;
         double satelites_tracked;
diff --git a/core/include/Core/nmea.h b/core/include/Core/nmea.h
index 461cc188..dbacfbdb 100644
--- a/core/include/Core/nmea.h
+++ b/core/include/Core/nmea.h
@@ -36,6 +36,7 @@ namespace hd_mapping::nmea
         int fix_quality;
         int satellites_tracked;
         double age_of_data;
+        double undulation;
     };
 
     //! Breaks a line from an NMEA file into its components.
diff --git a/core/src/gnss.cpp b/core/src/gnss.cpp
index 7b444125..98731127 100644
--- a/core/src/gnss.cpp
+++ b/core/src/gnss.cpp
@@ -239,12 +239,14 @@ bool GNSS::load_raw_data_from_gnss(const std::vector<std::string>& input_file_na
                 std::istringstream(strs[3]) >> gp.alt;
                 std::istringstream(strs[4]) >> gp.hdop;
                 std::istringstream(strs[5]) >> gp.satelites_tracked;
-                std::istringstream(strs[6]) >> gp.height;
+                std::istringstream(strs[6]) >> gp.undulation;
                 std::istringstream(strs[7]) >> gp.age;
                 std::istringstream(strs[8]) >> gp.time;
                 std::istringstream(strs[9]) >> gp.fix_quality;
-                if (std::isfinite(gp.lat) && std::isfinite(gp.lon) && std::isfinite(gp.alt) && gp.lat != 0.0 && gp.lon != 0.0)
+                if (std::isfinite(gp.lat) && std::isfinite(gp.lon) && std::isfinite(gp.alt) && gp.lat != 0.0 && gp.lon != 0.0 &&
+                    std::isfinite(gp.undulation))
                 {
+                    gp.h_wgs84 = gp.alt + gp.undulation;
                     gnss_poses.push_back(gp);
                 }
             }
@@ -295,7 +297,7 @@ bool GNSS::project_to_mercator_projection()
 
 double get_ellipsoid_height(const GNSS::GlobalPose& pose)
 {
-    return pose.alt + pose.height;
+    return pose.h_wgs84;
 }
 
 bool GNSS::project_using_proj()
@@ -492,10 +494,12 @@ bool GNSS::load_raw_data_from_nmea(const std::vector<std::string>& input_file_na
                 gp.age = gga->age_of_data;
                 gp.time = 0; // todo convert to seconds from string
                 gp.fix_quality = gga->fix_quality;
+                gp.undulation = gga->undulation;
 
                 // register if there  is not nans
-                if (gp.lat == gp.lat && gp.lon == gp.lon && gp.alt == gp.alt)
+                if (gp.lat == gp.lat && gp.lon == gp.lon && gp.alt == gp.alt && gp.undulation == gp.undulation)
                 {
+                    gp.h_wgs84 = gp.alt + gp.undulation;
                     gnss_poses.push_back(gp);
                 }
             }
diff --git a/core/src/nmea.cpp b/core/src/nmea.cpp
index 85b1df93..26501cbe 100644
--- a/core/src/nmea.cpp
+++ b/core/src/nmea.cpp
@@ -119,6 +119,7 @@ namespace hd_mapping::nmea
         data.hdop = fields[8].empty() ? 0.0 : std::stod(fields[8]);
         data.fix_quality = fields[6].empty() ? 0 : std::stoi(fields[6]);
         data.satellites_tracked = fields[7].empty() ? 0 : std::stoi(fields[7]);
+        data.undulation = fields[11].empty() ? 0.0 : std::stod(fields[11]);
         data.age_of_data = fields.size() > 13 && !fields[13].empty() ? std::stod(fields[13]) : -1.0;
 
         return data;