From 1624a57600b49f22b22e9010fe3df5f1b3697acf Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Mon, 31 Aug 2026 17:02:19 +0200 Subject: [PATCH] step2: persist ENU origin + GNSS/TUM tracks in session file Session::save()/load() only handle point clouds, so the GNSS track, TUM track and ENU projection origin (previously always re-derived from the first GNSS pose) are now written into / read back from the *.mjs JSON as extra "enu_origin", "gnss_measurements" and "tum_trajectory" keys. All keys are optional; older sessions load unchanged. Also fix Session::load() path resolution: getNewPath() now keeps a referenced .laz/.csv path as-is when it still exists on disk, only relocating next to the session file as a fallback. This lets a session saved to a directory other than the one holding its scan files (e.g. one level up from a lio_result_* folder) reload its point clouds. Co-Authored-By: Claude Sonnet 5 New PoseGraphLoopClosure::use_gnss_correspondences flag (default true). When off, graph_slam() adds no GNSS <-> LiDAR-trajectory observations to the optimisation; the GNSS track is still drawn. Exposed as a checkbox in the Manual Pose Graph Loop Closure GUI when GNSS poses are loaded. Co-Authored-By: Claude Sonnet 5 --- .../multi_view_tls_registration_gui.cpp | 183 ++++++++++++++++++ core/include/Core/pose_graph_loop_closure.h | 6 + core/src/manual_pose_graph_loop_closure.cpp | 9 + core/src/pose_graph_loop_closure.cpp | 2 +- core/src/session.cpp | 9 + 5 files changed, 208 insertions(+), 1 deletion(-) 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 ddbf5046..1480afff 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 @@ -78,6 +78,8 @@ #include +#include + #include #include #include @@ -2333,6 +2335,181 @@ void lio_segments_gui() ImGui::End(); } +// ----------------------------------------------------------------------------- +// GNSS / TUM / ENU-origin session persistence +// +// core's Session::save()/Session::load() (core/src/session.cpp) only knows about +// point clouds, loop-closure edges and control points -- the GNSS track, the TUM +// track and the ENU projection origin live in the GUI's own `tls_registration` +// (see multi_view_tls_registration.h), so they're persisted here by re-opening +// the *.mjs JSON that Session::save() just wrote and adding three extra +// top-level keys: +// +// "enu_origin" - the WGS84 lat/lon (+ geoid separation) the GNSS +// topocentric projection is referenced to. Previously +// this was always re-derived from the first GNSS pose on +// every projection/load; saving it lets a reopened +// session keep the exact same origin even before GNSS +// data is reloaded. +// "gnss_measurements" - every GNSS::GlobalPose (raw WGS84 + projected ENU). +// "tum_trajectory" - every TUM::TumPose. +// +// All three keys are optional: an older session without them just loads with +// empty GNSS/TUM tracks and the from-first-pose origin behaviour, as before. +// ----------------------------------------------------------------------------- +static void writeGnssTumToSessionFile(const std::string& path, const GNSS& gnss, const TUM& tum) +{ + try + { + nlohmann::json data; + { + std::ifstream fin(path); + if (!fin.good()) + { + spdlog::error("Can't reopen session file to append GNSS/TUM data: '{}'", path); + return; + } + data = nlohmann::json::parse(fin); + } + + nlohmann::json jorigin; + jorigin["latitude"] = gnss.WGS84ReferenceLatitude; + jorigin["longitude"] = gnss.WGS84ReferenceLongitude; + jorigin["geoid_separation"] = gnss.geoidSeparation; + jorigin["set_from_first_pose"] = gnss.setWGS84ReferenceFromFirstPose; + data["enu_origin"] = jorigin; + + nlohmann::json jgnss = nlohmann::json::array(); + for (const auto& p : gnss.gnss_poses) + { + jgnss.push_back( + { { "timestamp", p.timestamp }, + { "lat", p.lat }, + { "lon", p.lon }, + { "h_wgs84", p.h_wgs84 }, + { "undulation", p.undulation }, + { "alt", p.alt }, + { "hdop", p.hdop }, + { "satelites_tracked", p.satelites_tracked }, + { "height", p.height }, + { "age", p.age }, + { "time", p.time }, + { "fix_quality", p.fix_quality }, + { "enu_x", p.enu_x }, + { "enu_y", p.enu_y }, + { "enu_z", p.enu_z }, + { "dist_xy_along", p.dist_xy_along } }); + } + data["gnss_measurements"] = jgnss; + + nlohmann::json jtum = nlohmann::json::array(); + for (const auto& p : tum.tum_poses) + { + jtum.push_back( + { { "timestamp", p.timestamp }, + { "x", p.x }, + { "y", p.y }, + { "z", p.z }, + { "qx", p.qx }, + { "qy", p.qy }, + { "qz", p.qz }, + { "qw", p.qw } }); + } + data["tum_trajectory"] = jtum; + + std::ofstream fout(path); + if (!fout.good()) + { + spdlog::error("Can't rewrite session file with GNSS/TUM data: '{}'", path); + return; + } + fout << data.dump(2); + spdlog::info( + "Wrote {} GNSS pose(s), {} TUM pose(s) and ENU origin to session '{}'", gnss.gnss_poses.size(), tum.tum_poses.size(), path); + } catch (const std::exception& e) + { + spdlog::error("Failed writing GNSS/TUM data to session '{}': {}", path, e.what()); + } +} + +static void readGnssTumFromSessionFile(const std::string& path, GNSS& gnss, TUM& tum) +{ + try + { + std::ifstream fin(path); + if (!fin.good()) + return; + nlohmann::json data = nlohmann::json::parse(fin); + + if (data.contains("enu_origin")) + { + const auto& jorigin = data["enu_origin"]; + gnss.WGS84ReferenceLatitude = jorigin.value("latitude", 0.0); + gnss.WGS84ReferenceLongitude = jorigin.value("longitude", 0.0); + gnss.geoidSeparation = jorigin.value("geoid_separation", 0.0); + // A stored origin takes precedence over re-deriving one from the + // first GNSS pose; honour an explicit flag if the session carries + // one, otherwise assume the stored origin should be used as-is. + gnss.setWGS84ReferenceFromFirstPose = jorigin.value("set_from_first_pose", false); + spdlog::info( + "Loaded ENU origin from session: lat={}, lon={}, geoid_separation={}", + gnss.WGS84ReferenceLatitude, + gnss.WGS84ReferenceLongitude, + gnss.geoidSeparation); + } + + if (data.contains("gnss_measurements")) + { + gnss.gnss_poses.clear(); + for (const auto& j : data["gnss_measurements"]) + { + GNSS::GlobalPose p{}; + p.timestamp = j.value("timestamp", 0.0); + p.lat = j.value("lat", 0.0); + p.lon = j.value("lon", 0.0); + p.h_wgs84 = j.value("h_wgs84", 0.0); + p.undulation = j.value("undulation", 0.0); + p.alt = j.value("alt", 0.0); + p.hdop = j.value("hdop", 0.0); + p.satelites_tracked = j.value("satelites_tracked", 0.0); + p.height = j.value("height", 0.0); + p.age = j.value("age", 0.0); + p.time = j.value("time", 0.0); + p.fix_quality = j.value("fix_quality", 0.0); + p.enu_x = j.value("enu_x", 0.0); + p.enu_y = j.value("enu_y", 0.0); + p.enu_z = j.value("enu_z", 0.0); + p.dist_xy_along = j.value("dist_xy_along", 0.0); + gnss.gnss_poses.push_back(p); + } + spdlog::info("Loaded {} GNSS measurement(s) from session '{}'", gnss.gnss_poses.size(), path); + } + + if (data.contains("tum_trajectory")) + { + tum.tum_poses.clear(); + for (const auto& j : data["tum_trajectory"]) + { + TUM::TumPose p{}; + p.timestamp = j.value("timestamp", 0.0); + p.x = j.value("x", 0.0); + p.y = j.value("y", 0.0); + p.z = j.value("z", 0.0); + p.qx = j.value("qx", 0.0); + p.qy = j.value("qy", 0.0); + p.qz = j.value("qz", 0.0); + p.qw = j.value("qw", 1.0); + tum.tum_poses.push_back(p); + } + tum.version++; + spdlog::info("Loaded {} TUM pose(s) from session '{}'", tum.tum_poses.size(), path); + } + } catch (const std::exception& e) + { + spdlog::error("Failed reading GNSS/TUM data from session '{}': {}", path, e.what()); + } +} + void loadSession(const std::string& session_file_name) { spdlog::info("Session file: '{}'", session_file_name); @@ -2358,6 +2535,10 @@ void loadSession(const std::string& session_file_name) session_dims = session.point_clouds_container.compute_point_cloud_dimension(); scan_renderer.rebuildAll(session.point_clouds_container.point_clouds); + + // Restore the ENU projection origin and the GNSS / TUM tracks that + // writeGnssTumToSessionFile() stored alongside the core session data. + readGnssTumFromSessionFile(fs::path(session_file_name).string(), tls_registration.gnss, tls_registration.tum); } } @@ -2462,6 +2643,7 @@ std::string saveSession() } session.save(output_file_name, poses_file_name, initial_poses_file_name, false); + writeGnssTumToSessionFile(output_file_name, tls_registration.gnss, tls_registration.tum); spdlog::info("Saving result to: '{}'", poses_file_name); session.point_clouds_container.save_poses(poses_file_name, false); @@ -2695,6 +2877,7 @@ void saveSubsession() const auto poses_file_name = (dir / (stem + "_poses" + ".mrp")).string(); session.save(fs::path(output_file_name).string(), poses_file_name, initial_poses_file_name, true); + writeGnssTumToSessionFile(fs::path(output_file_name).string(), tls_registration.gnss, tls_registration.tum); spdlog::info("Saving poses to: '{}'", poses_file_name); session.point_clouds_container.save_poses(fs::path(poses_file_name).string(), true); diff --git a/core/include/Core/pose_graph_loop_closure.h b/core/include/Core/pose_graph_loop_closure.h index d8a65f7d..1833ab27 100644 --- a/core/include/Core/pose_graph_loop_closure.h +++ b/core/include/Core/pose_graph_loop_closure.h @@ -50,6 +50,12 @@ class PoseGraphLoopClosure double motion_model_w_fi_1_sigma_deg = 1.0 / 100.0 * 180.0 / M_PI; double motion_model_w_ka_1_sigma_deg = 1.0 / 100.0 * 180.0 / M_PI; + // When false, graph_slam() does not add any GNSS <-> LiDAR-trajectory + // observations to the optimisation (loaded GNSS poses are still drawn, + // they just don't pull on the pose graph). When true (default) every + // GNSS pose contributes, as before. + bool use_gnss_correspondences = true; + PoseGraphLoopClosure() {}; ~PoseGraphLoopClosure() {}; diff --git a/core/src/manual_pose_graph_loop_closure.cpp b/core/src/manual_pose_graph_loop_closure.cpp index 64ba9d2c..1a88502f 100644 --- a/core/src/manual_pose_graph_loop_closure.cpp +++ b/core/src/manual_pose_graph_loop_closure.cpp @@ -114,6 +114,15 @@ void ManualPoseGraphLoopClosure::Gui( ImGui::SameLine(); ImGui::Checkbox("Keep initial trajectory curvature", &keep_initial_trajectory_curvature); + if (gnss.gnss_poses.size() > 0) + { + ImGui::Checkbox("Use GNSS correspondences in Pose Graph SLAM", &use_gnss_correspondences); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "When off, loaded GNSS poses are still shown but do not contribute any observations " + "to 'Compute Pose Graph SLAM'"); + } + ImGui::Separator(); ImGui::Text("Motion model sigmas [m] / [deg]:"); diff --git a/core/src/pose_graph_loop_closure.cpp b/core/src/pose_graph_loop_closure.cpp index a6c8d9dd..6d953e80 100644 --- a/core/src/pose_graph_loop_closure.cpp +++ b/core/src/pose_graph_loop_closure.cpp @@ -275,7 +275,7 @@ void PoseGraphLoopClosure::graph_slam(PointClouds& point_clouds_container, GNSS& // gnss // for (const auto &pc : point_clouds_container.point_clouds) - for (int index_pose = 0; index_pose < point_clouds_container.point_clouds.size(); index_pose++) + for (int index_pose = 0; use_gnss_correspondences && index_pose < point_clouds_container.point_clouds.size(); index_pose++) { const auto& pc = point_clouds_container.point_clouds[index_pose]; for (int i = 0; i < gnss.gnss_poses.size(); i++) diff --git a/core/src/session.cpp b/core/src/session.cpp index 06bcac8a..af012137 100644 --- a/core/src/session.cpp +++ b/core/src/session.cpp @@ -43,6 +43,15 @@ bool Session::load(const std::string& file_name, bool is_decimate, double bucket fs::path p(normalized); if (is_directory(p)) return p.string(); + // Prefer the stored path when it still resolves: this keeps sessions + // working when they were saved to a directory other than the one that + // holds the referenced .laz/.csv files (e.g. session file one level up + // from a lio_result_* folder). Only when the stored path is gone do we + // fall back to a file of the same name sitting next to the session file + // (the "session + data moved together" case). + std::error_code ec; + if (fs::exists(p, ec)) + return p.string(); return (fs::path(directory) / p.filename()).string(); };