From a378b54bb14924b5aac6d8a32d53c812f5bfd901 Mon Sep 17 00:00:00 2001 From: FieldDiTian Date: Mon, 27 Jul 2026 16:17:28 -0700 Subject: [PATCH 1/4] Fix GNSS fit and factor delivery validation --- .../glim/mapping/async_global_mapping.hpp | 1 + GLIM/glim/include/glim/mapping/callbacks.hpp | 13 +- .../include/glim/mapping/global_mapping.hpp | 1 + .../glim/mapping/global_mapping_base.hpp | 11 +- .../mapping/global_mapping_pose_graph.hpp | 1 + .../src/glim/mapping/async_global_mapping.cpp | 5 + GLIM/glim/src/glim/mapping/callbacks.cpp | 3 +- GLIM/glim/src/glim/mapping/global_mapping.cpp | 2 + .../mapping/global_mapping_pose_graph.cpp | 8 +- GLIM/glim_ext/README.md | 6 + GLIM/glim_ext/config/config_gnss_global.json | 7 + .../mapping/gnss_global/CMakeLists.txt | 12 + .../include/glim_ext/gnss_alignment.hpp | 155 +++++++ .../include/glim_ext/gnss_factor_delivery.hpp | 35 ++ .../include/glim_ext/gnss_global_module.hpp | 395 +++++++++++------- .../gnss_global/test/gnss_alignment_test.cpp | 123 ++++++ GLIM/glim_ext/package.xml | 1 + GLIM/glim_ros2/include/glim_ros/glim_ros.hpp | 1 + GLIM/glim_ros2/src/glim_pcap_rosbag.cpp | 7 + GLIM/glim_ros2/src/glim_ros/glim_ros.cpp | 4 + GLIM/glim_ros2/src/glim_rosbag.cpp | 8 + README.md | 12 +- scripts/generate_glim_mapping_config.py | 31 +- 23 files changed, 687 insertions(+), 155 deletions(-) create mode 100644 GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_alignment.hpp create mode 100644 GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_factor_delivery.hpp create mode 100644 GLIM/glim_ext/modules/mapping/gnss_global/test/gnss_alignment_test.cpp diff --git a/GLIM/glim/include/glim/mapping/async_global_mapping.hpp b/GLIM/glim/include/glim/mapping/async_global_mapping.hpp index b545834a..0818de16 100644 --- a/GLIM/glim/include/glim/mapping/async_global_mapping.hpp +++ b/GLIM/glim/include/glim/mapping/async_global_mapping.hpp @@ -73,6 +73,7 @@ class AsyncGlobalMapping { void save(const std::string& path); gtsam_points::PointCloud::Ptr export_points(); + size_t num_submaps(); std::shared_ptr get_global_mapping() { std::lock_guard lock(global_mapping_mutex); diff --git a/GLIM/glim/include/glim/mapping/callbacks.hpp b/GLIM/glim/include/glim/mapping/callbacks.hpp index 0abfff1d..a985c3a5 100644 --- a/GLIM/glim/include/glim/mapping/callbacks.hpp +++ b/GLIM/glim/include/glim/mapping/callbacks.hpp @@ -132,6 +132,17 @@ struct GlobalMappingCallbacks { */ static CallbackSlot on_smoother_update_result; + /** + * @brief Global optimization failure callback + * @param isam2 iSAM2 optimizer + * @param message Exception message from the failed update + * + * This is paired with on_smoother_update_result so extensions can treat a + * factor handoff as a two-phase commit. A batch is not part of the graph + * merely because it was appended to new_factors. + */ + static CallbackSlot on_smoother_update_failure; + /** * @brief Request the global mapping module to perform optimization * @note This is a special inverse-direction callback slot @@ -151,4 +162,4 @@ struct GlobalMappingCallbacks { */ static CallbackSlot request_to_find_overlapping_submaps; }; -} // namespace glim \ No newline at end of file +} // namespace glim diff --git a/GLIM/glim/include/glim/mapping/global_mapping.hpp b/GLIM/glim/include/glim/mapping/global_mapping.hpp index ef4344b7..3c20d3a2 100644 --- a/GLIM/glim/include/glim/mapping/global_mapping.hpp +++ b/GLIM/glim/include/glim/mapping/global_mapping.hpp @@ -70,6 +70,7 @@ class GlobalMapping : public GlobalMappingBase { virtual void save(const std::string& path) override; virtual gtsam_points::PointCloud::Ptr export_points() override; + virtual size_t num_submaps() const override { return submaps.size(); } /** * @brief Load a mapping result from a dumped directory diff --git a/GLIM/glim/include/glim/mapping/global_mapping_base.hpp b/GLIM/glim/include/glim/mapping/global_mapping_base.hpp index 660b166c..1a87ac1a 100644 --- a/GLIM/glim/include/glim/mapping/global_mapping_base.hpp +++ b/GLIM/glim/include/glim/mapping/global_mapping_base.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #ifdef GLIM_USE_OPENCV @@ -73,6 +74,14 @@ class GlobalMappingBase { */ virtual gtsam_points::PointCloud::Ptr export_points() { return nullptr; } + /** + * @brief Number of geometric submaps currently owned by this mapper. + * + * Offline runners use this after save() to reject an empty/filtered run + * instead of treating a serialized zero-submap graph as success. + */ + virtual size_t num_submaps() const { return 0; } + /** * @brief Load a global mapping module from a shared library * @param so_name Shared library name @@ -83,4 +92,4 @@ class GlobalMappingBase { protected: std::shared_ptr logger; }; -} // namespace glim \ No newline at end of file +} // namespace glim diff --git a/GLIM/glim/include/glim/mapping/global_mapping_pose_graph.hpp b/GLIM/glim/include/glim/mapping/global_mapping_pose_graph.hpp index f9f59d13..029ba283 100644 --- a/GLIM/glim/include/glim/mapping/global_mapping_pose_graph.hpp +++ b/GLIM/glim/include/glim/mapping/global_mapping_pose_graph.hpp @@ -181,6 +181,7 @@ class GlobalMappingPoseGraph : public GlobalMappingBase { virtual void save(const std::string& path) override; virtual gtsam_points::PointCloud::Ptr export_points() override; + virtual size_t num_submaps() const override { return submaps.size(); } private: void insert_submap(int current, const SubMap::Ptr& submap); diff --git a/GLIM/glim/src/glim/mapping/async_global_mapping.cpp b/GLIM/glim/src/glim/mapping/async_global_mapping.cpp index 06f4035e..4c0805d1 100644 --- a/GLIM/glim/src/glim/mapping/async_global_mapping.cpp +++ b/GLIM/glim/src/glim/mapping/async_global_mapping.cpp @@ -74,6 +74,11 @@ gtsam_points::PointCloud::Ptr AsyncGlobalMapping::export_points() { return points; } +size_t AsyncGlobalMapping::num_submaps() { + std::lock_guard lock(global_mapping_mutex); + return global_mapping->num_submaps(); +} + void AsyncGlobalMapping::run() { auto last_optimization_time = std::chrono::high_resolution_clock::now(); diff --git a/GLIM/glim/src/glim/mapping/callbacks.cpp b/GLIM/glim/src/glim/mapping/callbacks.cpp index 1ccaced7..2987c894 100644 --- a/GLIM/glim/src/glim/mapping/callbacks.cpp +++ b/GLIM/glim/src/glim/mapping/callbacks.cpp @@ -25,8 +25,9 @@ CallbackSlot& submaps)> GlobalMappingCallbac CallbackSlot GlobalMappingCallbacks::on_smoother_update; CallbackSlot GlobalMappingCallbacks::on_smoother_update_result; +CallbackSlot GlobalMappingCallbacks::on_smoother_update_failure; CallbackSlot GlobalMappingCallbacks::request_to_optimize; CallbackSlot GlobalMappingCallbacks::request_to_recover; CallbackSlot GlobalMappingCallbacks::request_to_find_overlapping_submaps; -} // namespace glim \ No newline at end of file +} // namespace glim diff --git a/GLIM/glim/src/glim/mapping/global_mapping.cpp b/GLIM/glim/src/glim/mapping/global_mapping.cpp index 1911f041..e0e96ce8 100644 --- a/GLIM/glim/src/glim/mapping/global_mapping.cpp +++ b/GLIM/glim/src/glim/mapping/global_mapping.cpp @@ -513,10 +513,12 @@ gtsam_points::ISAM2ResultExt GlobalMapping::update_isam2(const gtsam::NonlinearF } catch (const gtsam::IndeterminantLinearSystemException& e) { logger->error("an indeterminant linear system exception was caught during global map optimization!!"); logger->error(e.what()); + Callbacks::on_smoother_update_failure(*isam2, e.what()); indeterminant_nearby_key = e.nearbyVariable(); } catch (const std::exception& e) { logger->error("an exception was caught during global map optimization!!"); logger->error(e.what()); + Callbacks::on_smoother_update_failure(*isam2, e.what()); } if (indeterminant_nearby_key != 0) { diff --git a/GLIM/glim/src/glim/mapping/global_mapping_pose_graph.cpp b/GLIM/glim/src/glim/mapping/global_mapping_pose_graph.cpp index e58b5af4..fc912933 100644 --- a/GLIM/glim/src/glim/mapping/global_mapping_pose_graph.cpp +++ b/GLIM/glim/src/glim/mapping/global_mapping_pose_graph.cpp @@ -224,8 +224,8 @@ void GlobalMappingPoseGraph::update_optimizer() { return; } + gtsam_points::ISAM2ResultExt result; try { - gtsam_points::ISAM2ResultExt result; #ifdef GTSAM_USE_TBB auto arena = static_cast(tbb_task_arena.get()); arena->execute([&] { @@ -279,9 +279,13 @@ void GlobalMappingPoseGraph::update_optimizer() { } } - } catch (std::exception& e) { + } catch (const std::exception& e) { logger->error("an exception was caught during global map optimization!!"); logger->error(e.what()); + // Extensions may have appended factors in on_smoother_update(). Tell them + // explicitly that this transaction did not complete; otherwise handoff + // accounting can claim factors that were discarded with new_factors below. + Callbacks::on_smoother_update_failure(*isam2, e.what()); } new_values.reset(new gtsam::Values); new_factors.reset(new gtsam::NonlinearFactorGraph); diff --git a/GLIM/glim_ext/README.md b/GLIM/glim_ext/README.md index 0723bd14..f141b301 100644 --- a/GLIM/glim_ext/README.md +++ b/GLIM/glim_ext/README.md @@ -69,6 +69,12 @@ Example (`libflat_earther.so`): ### GNSS constraints (libgnss_global.so, ROS2 only) - GNSS-based constraints for global optimization +- The world/GNSS alignment uses at least `fit_min_samples` training samples and + validates on the newest `fit_validation_samples` excluded from the fit. + Training and held-out prediction RMS must both pass `fit_max_rms`. +- GNSS factor delivery is counted only after iSAM2 reports a successful update + containing the exact handed-off factor identities; failed batches remain + undelivered and reject the run. - Optional orientation priors from pose-bearing GNSS messages can be enabled with `enable_orientation_prior`. - An independent `gravity_prior_sigma_deg` option constrains the measured body-Z direction (roll/pitch) without constraining yaw. It is disabled when `<= 0` diff --git a/GLIM/glim_ext/config/config_gnss_global.json b/GLIM/glim_ext/config/config_gnss_global.json index e155bb4c..062748ba 100644 --- a/GLIM/glim_ext/config/config_gnss_global.json +++ b/GLIM/glim_ext/config/config_gnss_global.json @@ -16,6 +16,13 @@ // suffix that rejects stationary/low-speed startup transients while still // requiring min_baseline on both the estimate and GNSS trajectories. "fit_recent_baseline_window": false, + // The one-shot planar alignment must have enough geometry to estimate yaw + // robustly, then predict samples it did not fit. The newest + // fit_validation_samples are held out; both training and validation RMS + // must pass fit_max_rms before the transform can latch. + "fit_min_samples": 20, + "fit_validation_samples": 10, + "fit_max_rms": 0.25, // Atlas attitude (dual-antenna HEADING) prior on each submap. Position // anchoring (prior_inf_scale) pins WHERE a submap is, but not its yaw -- // the Atlas dual-antenna heading is a drift-free reference that LiDAR+IMU diff --git a/GLIM/glim_ext/modules/mapping/gnss_global/CMakeLists.txt b/GLIM/glim_ext/modules/mapping/gnss_global/CMakeLists.txt index c78a1199..028b2eac 100644 --- a/GLIM/glim_ext/modules/mapping/gnss_global/CMakeLists.txt +++ b/GLIM/glim_ext/modules/mapping/gnss_global/CMakeLists.txt @@ -5,6 +5,7 @@ set(CMAKE_CXX_STANDARD 17) find_package(glim REQUIRED) find_package(GTSAM REQUIRED) +find_package(Eigen3 REQUIRED) find_package(spdlog REQUIRED) find_package(LibXml2 REQUIRED) @@ -44,3 +45,14 @@ target_link_libraries(gnss_global ${catkin_LIBRARIES} ${LIBXML2_LIBRARIES} ) + +if($ENV{ROS_VERSION} EQUAL 2 AND BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(gnss_alignment_test + test/gnss_alignment_test.cpp + ) + target_include_directories(gnss_alignment_test PRIVATE + include + ) + target_link_libraries(gnss_alignment_test Eigen3::Eigen) +endif() diff --git a/GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_alignment.hpp b/GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_alignment.hpp new file mode 100644 index 00000000..65270dfa --- /dev/null +++ b/GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_alignment.hpp @@ -0,0 +1,155 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace glim { +namespace gnss_detail { + +struct AlignmentWindow { + bool ready = false; + size_t begin = 0; + size_t training_end = 0; + size_t end = 0; + double estimate_baseline = 0.0; + double gnss_baseline = 0.0; + + size_t training_count() const { return training_end - begin; } + size_t validation_count() const { return end - training_end; } +}; + +struct AlignmentFit { + bool valid = false; + Eigen::Isometry3d T_gnss_estimate = Eigen::Isometry3d::Identity(); + double training_rms = std::numeric_limits::infinity(); + double validation_rms = std::numeric_limits::infinity(); +}; + +inline AlignmentWindow select_alignment_window( + const std::vector& estimates, + const std::vector& gnss, + double min_baseline, + size_t min_training_samples, + size_t validation_samples, + bool recent_window) { + AlignmentWindow window; + if ( + estimates.size() != gnss.size() || min_training_samples < 3 || + validation_samples < 1 || + estimates.size() < min_training_samples + validation_samples) { + return window; + } + + window.begin = 0; + window.training_end = estimates.size() - validation_samples; + window.end = estimates.size(); + + const auto baselines_from = [&](size_t begin) { + const size_t last_training = window.training_end - 1; + return std::pair{ + (estimates[last_training] - estimates[begin]).norm(), + (gnss[last_training] - gnss[begin]).norm()}; + }; + + auto baselines = baselines_from(window.begin); + if (baselines.first <= min_baseline || baselines.second <= min_baseline) { + return window; + } + + if (recent_window) { + while (window.begin + 1 + min_training_samples <= window.training_end) { + const auto candidate_baselines = baselines_from(window.begin + 1); + if ( + candidate_baselines.first <= min_baseline || + candidate_baselines.second <= min_baseline) { + break; + } + ++window.begin; + baselines = candidate_baselines; + } + } + + window.estimate_baseline = baselines.first; + window.gnss_baseline = baselines.second; + window.ready = true; + return window; +} + +inline double alignment_rms( + const std::vector& estimates, + const std::vector& gnss, + const Eigen::Isometry3d& T_gnss_estimate, + size_t begin, + size_t end) { + if (begin >= end || end > estimates.size() || estimates.size() != gnss.size()) { + return std::numeric_limits::infinity(); + } + + double sum_sq = 0.0; + for (size_t i = begin; i < end; ++i) { + const Eigen::Vector3d prediction = T_gnss_estimate * estimates[i]; + sum_sq += (prediction - gnss[i]).squaredNorm(); + } + return std::sqrt(sum_sq / static_cast(end - begin)); +} + +inline AlignmentFit fit_planar_alignment( + const std::vector& estimates, + const std::vector& gnss, + const AlignmentWindow& window) { + AlignmentFit fit; + if ( + !window.ready || estimates.size() != gnss.size() || + window.training_count() < 3 || window.validation_count() < 1 || + window.end > estimates.size()) { + return fit; + } + + Eigen::Vector3d mean_estimate = Eigen::Vector3d::Zero(); + Eigen::Vector3d mean_gnss = Eigen::Vector3d::Zero(); + for (size_t i = window.begin; i < window.training_end; ++i) { + mean_estimate += estimates[i]; + mean_gnss += gnss[i]; + } + mean_estimate /= static_cast(window.training_count()); + mean_gnss /= static_cast(window.training_count()); + + Eigen::Matrix3d covariance = Eigen::Matrix3d::Zero(); + for (size_t i = window.begin; i < window.training_end; ++i) { + covariance += + (gnss[i] - mean_gnss) * (estimates[i] - mean_estimate).transpose(); + } + covariance /= static_cast(window.training_count()); + + const Eigen::JacobiSVD svd( + covariance.block<2, 2>(0, 0), Eigen::ComputeFullU | Eigen::ComputeFullV); + const Eigen::Matrix2d U = svd.matrixU(); + const Eigen::Matrix2d V = svd.matrixV(); + Eigen::Matrix2d reflection = Eigen::Matrix2d::Identity(); + if (U.determinant() * V.determinant() < 0.0) { + reflection(1, 1) = -1.0; + } + + fit.T_gnss_estimate.linear().block<2, 2>(0, 0) = + U * reflection * V.transpose(); + fit.T_gnss_estimate.translation() = + mean_gnss - fit.T_gnss_estimate.linear() * mean_estimate; + fit.training_rms = alignment_rms( + estimates, gnss, fit.T_gnss_estimate, window.begin, window.training_end); + fit.validation_rms = alignment_rms( + estimates, gnss, fit.T_gnss_estimate, window.training_end, window.end); + fit.valid = + fit.T_gnss_estimate.matrix().allFinite() && + std::isfinite(fit.training_rms) && std::isfinite(fit.validation_rms); + return fit; +} + +} // namespace gnss_detail +} // namespace glim diff --git a/GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_factor_delivery.hpp b/GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_factor_delivery.hpp new file mode 100644 index 00000000..4720d9da --- /dev/null +++ b/GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_factor_delivery.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include + +namespace glim { +namespace gnss_detail { + +template +bool factor_batch_committed( + size_t batch_offset, + const std::vector& expected_factors, + const std::vector& new_factor_indices, + size_t graph_size, + FactorPointerAt factor_pointer_at) { + if ( + expected_factors.empty() || + batch_offset > new_factor_indices.size() || + expected_factors.size() > new_factor_indices.size() - batch_offset) { + return false; + } + + for (size_t i = 0; i < expected_factors.size(); ++i) { + const size_t graph_index = new_factor_indices[batch_offset + i]; + if ( + graph_index >= graph_size || + factor_pointer_at(graph_index) != expected_factors[i]) { + return false; + } + } + return true; +} + +} // namespace gnss_detail +} // namespace glim diff --git a/GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_global_module.hpp b/GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_global_module.hpp index 27913613..140d9f2d 100644 --- a/GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_global_module.hpp +++ b/GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_global_module.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -50,10 +51,14 @@ using ExtensionModuleBase = glim::ExtensionModuleROS; #include #include #include +#include +#include #include #include #include +#include +#include #include namespace glim { @@ -142,11 +147,20 @@ class GNSSGlobal : public ExtensionModuleBase { // this width are left un-anchored (LiDAR+IMU only). <= 0 disables the // bound (legacy behavior). max_interp_gap_sec = config.param("gnss", "max_interp_gap_sec", 1.0); - // [P3 FIX 2026-07-14] Max post-fit RMS residual (m) accepted when latching - // the one-shot T_world_utm. A drifted first-5m or frozen/biased GNSS can - // otherwise latch a garbage rotation forever at ~1cm stiffness. <=0 disables - // the residual gate (legacy behavior). - fit_max_rms = config.param("gnss", "fit_max_rms", 2.0); + // A two-point fit has no useful residual and extremely high yaw variance. + // Keep a substantial training set, then validate on the newest samples + // that were deliberately excluded from the fit. This catches a growing + // estimate-side heading drift that an in-sample rigid fit can absorb. + fit_min_samples = + std::max(3, config.param("gnss", "fit_min_samples", 20)); + fit_validation_samples = + std::max(1, config.param("gnss", "fit_validation_samples", 10)); + // Maximum training AND held-out prediction RMS (m) accepted before the + // one-shot transform can latch. <= 0 disables both gates. + fit_max_rms = config.param("gnss", "fit_max_rms", 0.25); + if (!std::isfinite(fit_max_rms)) { + throw std::invalid_argument("gnss.fit_max_rms must be finite"); + } if (enable_orientation_prior && orientation_prior_inf_scale.minCoeff() < 0.0) { logger->warn("orientation prior enabled but orientation_prior_inf_scale has negative values; disabling orientation prior"); @@ -220,6 +234,10 @@ class GNSSGlobal : public ExtensionModuleBase { using std::placeholders::_3; GlobalMappingCallbacks::on_insert_submap.add(std::bind(&GNSSGlobal::on_insert_submap, this, _1)); GlobalMappingCallbacks::on_smoother_update.add(std::bind(&GNSSGlobal::on_smoother_update, this, _1, _2, _3)); + GlobalMappingCallbacks::on_smoother_update_result.add( + std::bind(&GNSSGlobal::on_smoother_update_result, this, _1, _2)); + GlobalMappingCallbacks::on_smoother_update_failure.add( + std::bind(&GNSSGlobal::on_smoother_update_failure, this, _1, _2)); GlobalMappingCallbacks::on_update_submaps.add(std::bind(&GNSSGlobal::on_update_submaps, this, _1)); } ~GNSSGlobal() { @@ -228,6 +246,10 @@ class GNSSGlobal : public ExtensionModuleBase { } virtual void at_exit(const std::string& dump_path) override { + // No successful result callback may arrive after save() returns. Resolve a + // stranded handoff as failed so it remains visible as undelivered. + fail_pending_factor_delivery("mapping exit before optimizer confirmation"); + // [P3 FIX 2026-07-10] Guarded: after a flush TIMEOUT GlimROS::save() can // reach here while the backend thread is mid-write in the initialization // block — a torn T_world_utm.txt (consumed by the map exporter) and a UB @@ -255,8 +277,10 @@ class GNSSGlobal : public ExtensionModuleBase { // clean exit code. A run anchored only in its last minute now shows a low // coverage ratio instead of looking fully anchored. logger->info( - "gnss_global summary: transformation_initialized={} fit_rms_m={:.3f} position_factors={} " - "orientation_factors={} gravity_factors={} factors_delivered={} factors_undelivered={} yaw_gate_skips={} " + "gnss_global summary: transformation_initialized={} fit_rms_m={:.3f} fit_validation_rms_m={:.3f} " + "fit_training_samples={} fit_validation_samples={} position_factors={} " + "orientation_factors={} gravity_factors={} factors_delivered={} factors_undelivered={} " + "factor_delivery_failures={} yaw_gate_skips={} " "gap_unanchored={} submaps_seen={} submaps_dropped_pre_gnss={} submaps_dropped_no_bracket={} " "submaps_unanchored_pre_fit={} " "submap_anchor_coverage={:.3f} nonmonotonic_drops={} bracket_count={} bracket_max_s={:.3f} " @@ -264,11 +288,15 @@ class GNSSGlobal : public ExtensionModuleBase { "anchor_health_ok={}", transformation_initialized, fit_rms_m.load(), + fit_validation_rms_m.load(), + fit_training_sample_count.load(), + fit_validation_sample_count.load(), pf, of, gf, delivered, undelivered, + factor_delivery_failure_count.load(), yaw_gate_skip_count.load(), gap_unanchored_count.load(), seen, @@ -285,8 +313,8 @@ class GNSSGlobal : public ExtensionModuleBase { healthy_.load()); if (undelivered > 0) { logger->warn("gnss_global: {} GNSS prior factor(s) were EMITTED but never DELIVERED to the " - "graph (save() flushed before on_smoother_update drained them) — the serialized " - "map has fewer anchors than emitted", undelivered); + "graph (no successful optimizer commit was confirmed) — the serialized map has " + "fewer anchors than emitted", undelivered); } } @@ -379,45 +407,152 @@ class GNSSGlobal : public ExtensionModuleBase { size_t submap_id; Eigen::Vector3d position; }; + struct PendingFactorDelivery { + bool active = false; + uint64_t batch_id = 0; + size_t new_factor_offset = 0; + std::vector factors; + std::vector< + PendingPositionAnchor, + Eigen::aligned_allocator> + anchors; + }; void on_insert_submap(const SubMap::ConstPtr& submap) { input_submap_queue.push_back({submap, submap->T_world_origin.translation()}); } void on_smoother_update(gtsam_points::ISAM2Ext& isam2, gtsam::NonlinearFactorGraph& new_factors, gtsam::Values& new_values) { + // A factor batch must receive either a result or failure callback before + // another update starts. Fail closed if a custom mapping backend violates + // that pairing instead of crediting an ambiguous handoff. + fail_pending_factor_delivery( + "new optimizer handoff started before the previous result callback"); + std::vector factors; - std::vector> delivered_anchors; + std::vector< + PendingPositionAnchor, + Eigen::aligned_allocator> + pending_anchors; { - // Drain factors and their position-anchor metadata atomically. The - // health callback must never inspect an anchor that has not entered the - // same optimizer update yet. + // Drain factors and their position-anchor metadata atomically. Anchors + // remain pending until iSAM2 confirms this exact batch. std::lock_guard lock(factor_delivery_mtx_); factors = output_factors.get_all_and_clear(); - delivered_anchors.swap(pending_position_anchors_); + pending_anchors.swap(pending_position_anchors_); } if (!factors.empty()) { logger->debug("insert {} GNSS prior factors", factors.size()); + const size_t batch_offset = new_factors.size(); new_factors.add(factors); - // [P3 FIX 2026-07-14] Count DELIVERED factors. position/orientation counts - // are EMITTED-to-output_factors; after a flush-timeout save() can serialize - // the graph while output_factors still holds undelivered factors, so the - // emitted counts overstate what actually reached the graph. at_exit reports - // factors_undelivered = emitted - delivered so prep_bag can gate on it. - factors_delivered_count += factors.size(); + std::lock_guard lock(factor_delivery_mtx_); + pending_factor_delivery_.active = true; + pending_factor_delivery_.batch_id = ++factor_delivery_batch_sequence_; + pending_factor_delivery_.new_factor_offset = batch_offset; + pending_factor_delivery_.factors = std::move(factors); + pending_factor_delivery_.anchors = std::move(pending_anchors); + } else if (!pending_anchors.empty()) { + logger->error( + "GNSS factor delivery invariant violated: {} pending anchor(s) had no " + "matching factor batch", + pending_anchors.size()); + ++factor_delivery_failure_count; + healthy_ = false; + } + } + + void on_smoother_update_result( + gtsam_points::ISAM2Ext& isam2, + const gtsam_points::ISAM2ResultExt& result) { + std::vector< + PendingPositionAnchor, + Eigen::aligned_allocator> + committed_anchors; + size_t committed_count = 0; + uint64_t batch_id = 0; + { + std::lock_guard lock(factor_delivery_mtx_); + if (!pending_factor_delivery_.active) { + return; + } + + std::vector expected_factors; + expected_factors.reserve(pending_factor_delivery_.factors.size()); + for (const auto& factor : pending_factor_delivery_.factors) { + expected_factors.push_back(factor.get()); + } + const auto& graph = isam2.getFactorsUnsafe(); + const bool committed = gnss_detail::factor_batch_committed( + pending_factor_delivery_.new_factor_offset, + expected_factors, + result.newFactorsIndices, + graph.size(), + [&](size_t index) -> const void* { return graph[index].get(); }); + + batch_id = pending_factor_delivery_.batch_id; + if (committed) { + committed_count = pending_factor_delivery_.factors.size(); + committed_anchors = std::move(pending_factor_delivery_.anchors); + } else { + ++factor_delivery_failure_count; + healthy_ = false; + } + pending_factor_delivery_ = PendingFactorDelivery(); + } + + if (committed_count == 0) { + logger->error( + "GNSS optimizer result did not contain the exact pending factor batch " + "{}; leaving it undelivered and rejecting the run", + batch_id); + return; } - if (!delivered_anchors.empty()) { + + factors_delivered_count += committed_count; + { std::lock_guard lock(delivered_anchor_mtx_); - for (const auto& anchor : delivered_anchors) { + for (const auto& anchor : committed_anchors) { if (delivered_anchor_positions_.size() <= anchor.submap_id) { delivered_anchor_positions_.resize( anchor.submap_id + 1, - Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN())); + Eigen::Vector3d::Constant( + std::numeric_limits::quiet_NaN())); } delivered_anchor_positions_[anchor.submap_id] = anchor.position; } } } + void on_smoother_update_failure( + gtsam_points::ISAM2Ext& isam2, + const std::string& message) { + (void)isam2; + fail_pending_factor_delivery( + "optimizer update threw before commit: " + message); + } + + void fail_pending_factor_delivery(const std::string& reason) { + size_t failed_count = 0; + uint64_t batch_id = 0; + { + std::lock_guard lock(factor_delivery_mtx_); + if (!pending_factor_delivery_.active) { + return; + } + failed_count = pending_factor_delivery_.factors.size(); + batch_id = pending_factor_delivery_.batch_id; + pending_factor_delivery_ = PendingFactorDelivery(); + ++factor_delivery_failure_count; + healthy_ = false; + } + logger->error( + "GNSS factor batch {} failed delivery confirmation ({} factors): {}; " + "the factors remain undelivered and the run is rejected", + batch_id, + failed_count, + reason); + } + void on_update_submaps(const std::vector& updated_submaps) { if (anchor_abort_median_m <= 0.0) { return; @@ -629,129 +764,94 @@ class GNSSGlobal : public ExtensionModuleBase { submaps.push_back(submap); submap_t_snap.push_back(t_snap); submap_coords.push_back(interpolated); + submap_gnss_positions.push_back(interpolated.position); submap_queue.pop_front(); utm_queue.erase(utm_queue.begin(), left); } - // Initialize T_world_utm - // [P3 FIX 2026-07-14] Require BOTH the estimate-side and the GNSS-side - // baseline to exceed min_baseline. The world-side check alone let a - // frozen-position GNSS (all samples ~coincident) reach the one-shot fit, - // latching a garbage rotation forever. - if (!transformation_initialized && !submaps.empty() && - (submap_t_snap.back() - submap_t_snap.front()).norm() > min_baseline && - (submap_coords.back().position - submap_coords.front().position).norm() > min_baseline) { - // Keep the legacy all-history fit unless the run-local quality profile - // opts into startup-transient rejection. In recent-window mode, move - // the left edge forward as far as possible while BOTH endpoint - // displacements remain above min_baseline. This selects the newest - // well-observed segment without assuming a sensor rate or hard-coding - // a dataset-specific time/count window. - size_t fit_begin = 0; - if (fit_recent_baseline_window) { - while (fit_begin + 2 < submaps.size() && - (submap_t_snap.back() - submap_t_snap[fit_begin + 1]).norm() > min_baseline && - (submap_coords.back().position - submap_coords[fit_begin + 1].position).norm() > min_baseline) { - ++fit_begin; - } - } - const size_t fit_count = submaps.size() - fit_begin; - const double estimate_baseline = - (submap_t_snap.back() - submap_t_snap[fit_begin]).norm(); - const double gnss_baseline = - (submap_coords.back().position - submap_coords[fit_begin].position).norm(); - - Eigen::Vector3d mean_est = Eigen::Vector3d::Zero(); - Eigen::Vector3d mean_gnss = Eigen::Vector3d::Zero(); - for (size_t i = fit_begin; i < submaps.size(); i++) { - mean_est += submap_t_snap[i]; - mean_gnss += submap_coords[i].position; - } - mean_est /= static_cast(fit_count); - mean_gnss /= static_cast(fit_count); - - Eigen::Matrix3d cov = Eigen::Matrix3d::Zero(); - for (size_t i = fit_begin; i < submaps.size(); i++) { - const Eigen::Vector3d centered_est = submap_t_snap[i] - mean_est; - const Eigen::Vector3d centered_gnss = submap_coords[i].position - mean_gnss; - cov += centered_gnss * centered_est.transpose(); - } - cov /= static_cast(fit_count); - - const Eigen::JacobiSVD svd(cov.block<2, 2>(0, 0), Eigen::ComputeFullU | Eigen::ComputeFullV); - const Eigen::Matrix2d U = svd.matrixU(); - const Eigen::Matrix2d V = svd.matrixV(); - const Eigen::Matrix2d D = svd.singularValues().asDiagonal(); - Eigen::Matrix2d S = Eigen::Matrix2d::Identity(); - - const double det = U.determinant() * V.determinant(); - if (det < 0.0) { - S(1, 1) = -1; - } - - Eigen::Isometry3d T_utm_world = Eigen::Isometry3d::Identity(); - T_utm_world.linear().block<2, 2>(0, 0) = U * S * V.transpose(); - T_utm_world.translation() = mean_gnss - T_utm_world.linear() * mean_est; - - // [P3 FIX 2026-07-14] Post-fit RMS residual acceptance. The baseline - // gate is purely geometric; a drifted first-5m or a frozen/biased GNSS - // can still yield a garbage rotation that is then LATCHED FOREVER and - // enforced at ~1cm stiffness. Reject the fit when the GNSS-vs-estimate - // RMS residual is too large — retry next cycle with more/better data - // instead of latching a bad transform. - double sum_sq = 0.0; - for (size_t i = fit_begin; i < submaps.size(); i++) { - const Eigen::Vector3d pred = T_utm_world * submap_t_snap[i]; // world -> utm - sum_sq += (pred - submap_coords[i].position).squaredNorm(); - } - const double rms = std::sqrt(sum_sq / static_cast(fit_count)); - if (fit_max_rms > 0.0 && rms > fit_max_rms) { - logger->warn( - "T_world_utm one-shot fit REJECTED: RMS residual {:.3f} m > max {:.3f} m " - "over fit window [{}..{}] ({} samples, estimate/GNSS baselines {:.3f}/{:.3f} m) " - "— not latching; will retry with more data", - rms, - fit_max_rms, - fit_begin, - submaps.size() - 1, - fit_count, - estimate_baseline, - gnss_baseline); - } else { - { - std::lock_guard lock(T_world_utm_mtx_); - T_world_utm = T_utm_world.inverse(); - } - fit_rms_m.store(rms); - - for (size_t i = fit_begin; i < submaps.size(); i++) { - const Eigen::Vector3d gnss = T_world_utm * submap_coords[i].position; - logger->debug("submap={} gnss={}", convert_to_string(submap_t_snap[i]), convert_to_string(gnss)); - } + // Initialize T_world_utm from a substantial training set, then predict a + // held-out newest suffix. An in-sample rigid fit can absorb a growing + // heading error and report a deceptively small residual; extrapolation + // onto unseen samples makes that failure observable before latching. + if (!transformation_initialized && !submaps.empty()) { + const auto fit_window = gnss_detail::select_alignment_window( + submap_t_snap, + submap_gnss_positions, + min_baseline, + static_cast(fit_min_samples), + static_cast(fit_validation_samples), + fit_recent_baseline_window); + if (fit_window.ready) { + const auto fit = gnss_detail::fit_planar_alignment( + submap_t_snap, submap_gnss_positions, fit_window); + const bool residual_rejected = + !fit.valid || + (fit_max_rms > 0.0 && + (fit.training_rms > fit_max_rms || + fit.validation_rms > fit_max_rms)); + if (residual_rejected) { + logger->warn( + "T_world_utm one-shot fit REJECTED: training/validation RMS " + "{:.3f}/{:.3f} m (max {:.3f} m), training [{}..{}] ({} samples), " + "validation [{}..{}] ({} samples), estimate/GNSS training " + "baselines {:.3f}/{:.3f} m — not latching; will retry with more data", + fit.training_rms, + fit.validation_rms, + fit_max_rms, + fit_window.begin, + fit_window.training_end - 1, + fit_window.training_count(), + fit_window.training_end, + fit_window.end - 1, + fit_window.validation_count(), + fit_window.estimate_baseline, + fit_window.gnss_baseline); + } else { + { + std::lock_guard lock(T_world_utm_mtx_); + T_world_utm = fit.T_gnss_estimate.inverse(); + } + fit_rms_m.store(fit.training_rms); + fit_validation_rms_m.store(fit.validation_rms); + fit_training_sample_count.store(fit_window.training_count()); + fit_validation_sample_count.store(fit_window.validation_count()); + + for (size_t i = fit_window.begin; i < fit_window.end; ++i) { + const Eigen::Vector3d gnss = + T_world_utm * submap_coords[i].position; + logger->debug( + "submap={} gnss={}", + convert_to_string(submap_t_snap[i]), + convert_to_string(gnss)); + } - logger->info( - "T_world_utm={} (one-shot fit RMS residual {:.3f} m over window [{}..{}], " - "{} samples, estimate/GNSS baselines {:.3f}/{:.3f} m)", - convert_to_string(T_world_utm), - rms, - fit_begin, - submaps.size() - 1, - fit_count, - estimate_baseline, - gnss_baseline); - { - std::lock_guard lock(T_world_utm_mtx_); - transformation_initialized = true; // published under the same lock as the matrix - } - // Do not backfill startup submaps that were deliberately excluded - // from the accepted fit. They remain connected by LiDAR+IMU odometry - // and later global constraints; forcing them through a transform - // whose fit rejected that transient would immediately recreate the - // map-warp condition the anchor health gate is intended to catch. - if (fit_recent_baseline_window) { - factored_submap_count = fit_begin; - submaps_unanchored_pre_fit.store(fit_begin); + logger->info( + "T_world_utm={} (one-shot training/validation RMS {:.3f}/{:.3f} m, " + "training [{}..{}] {} samples, validation [{}..{}] {} samples, " + "estimate/GNSS training baselines {:.3f}/{:.3f} m)", + convert_to_string(T_world_utm), + fit.training_rms, + fit.validation_rms, + fit_window.begin, + fit_window.training_end - 1, + fit_window.training_count(), + fit_window.training_end, + fit_window.end - 1, + fit_window.validation_count(), + fit_window.estimate_baseline, + fit_window.gnss_baseline); + { + std::lock_guard lock(T_world_utm_mtx_); + transformation_initialized = true; + } + // Do not backfill startup submaps deliberately excluded by the + // accepted recent fit. The held-out suffix is validated and then + // factored normally; only the rejected startup prefix remains LIO. + if (fit_recent_baseline_window) { + factored_submap_count = fit_window.begin; + submaps_unanchored_pre_fit.store(fit_window.begin); + } } } } @@ -1013,11 +1113,14 @@ class GNSSGlobal : public ExtensionModuleBase { ConcurrentVector output_factors; std::vector> pending_position_anchors_; + PendingFactorDelivery pending_factor_delivery_; + uint64_t factor_delivery_batch_sequence_ = 0; std::vector> delivered_anchor_positions_; std::vector submaps; std::vector> submap_coords; + std::vector submap_gnss_positions; // Number of associated submaps that have already had GNSS prior factors // emitted. Everything in [factored_submap_count, submaps.size()) still needs // factors -- this backfills the pre-T_world_utm backlog and every submap in a @@ -1042,7 +1145,9 @@ class GNSSGlobal : public ExtensionModuleBase { double min_baseline; bool fit_recent_baseline_window; double max_interp_gap_sec; // P1 fix: max GNSS bracket width for association (<=0 disables) - double fit_max_rms; // [P3 FIX 2026-07-14] max post-fit RMS residual to latch (<=0 disables) + int fit_min_samples; + int fit_validation_samples; + double fit_max_rms; // max training and holdout RMS to latch (<=0 disables) // [P3 AUDIT 2026-07-14] End-to-end RTK timing/anchoring evidence, reported // in the at_exit summary so run tooling (prep_bag --require-rtk-anchor) can @@ -1054,7 +1159,8 @@ class GNSSGlobal : public ExtensionModuleBase { std::atomic position_factor_count{0}; // GNSS position priors emitted std::atomic orientation_factor_count{0}; // heading priors emitted std::atomic gravity_factor_count{0}; // roll/pitch priors emitted - std::atomic factors_delivered_count{0}; // priors actually inserted into the graph + std::atomic factors_delivered_count{0}; // priors confirmed in the graph after successful update + std::atomic factor_delivery_failure_count{0}; std::atomic gap_unanchored_count{0}; // submaps skipped: bracket > max_interp_gap std::atomic nonmonotonic_drop_count{0}; // GNSS samples dropped: stamp regression std::atomic bracket_max_s{0.0}; // widest accepted GNSS bracket @@ -1067,7 +1173,10 @@ class GNSSGlobal : public ExtensionModuleBase { std::atomic submaps_dropped_pre_gnss{0}; // popped: created before the oldest GNSS std::atomic submaps_dropped_no_bracket{0}; // popped: no valid GNSS bracket std::atomic submaps_unanchored_pre_fit{0}; // startup transient excluded by recent fit window - std::atomic fit_rms_m{-1.0}; // post-fit RMS residual of the latched T_world_utm + std::atomic fit_rms_m{-1.0}; // training RMS of the latched T_world_utm + std::atomic fit_validation_rms_m{-1.0}; // held-out prediction RMS + std::atomic fit_training_sample_count{0}; + std::atomic fit_validation_sample_count{0}; std::atomic anchor_residual_median_m{-1.0}; std::atomic anchor_abort_streak{0}; std::atomic_bool healthy_{true}; diff --git a/GLIM/glim_ext/modules/mapping/gnss_global/test/gnss_alignment_test.cpp b/GLIM/glim_ext/modules/mapping/gnss_global/test/gnss_alignment_test.cpp new file mode 100644 index 00000000..e15f72d2 --- /dev/null +++ b/GLIM/glim_ext/modules/mapping/gnss_global/test/gnss_alignment_test.cpp @@ -0,0 +1,123 @@ +#include +#include + +#include + +#include +#include + +namespace { + +using glim::gnss_detail::fit_planar_alignment; +using glim::gnss_detail::select_alignment_window; + +TEST(GNSSAlignment, WaitsForTrainingAndValidationSamples) { + std::vector estimate(29, Eigen::Vector3d::Zero()); + std::vector gnss(29, Eigen::Vector3d::Zero()); + for (size_t i = 0; i < estimate.size(); ++i) { + estimate[i].x() = static_cast(i); + gnss[i].x() = static_cast(i); + } + + const auto window = + select_alignment_window(estimate, gnss, 5.0, 20, 10, true); + EXPECT_FALSE(window.ready); +} + +TEST(GNSSAlignment, RecentWindowRetainsMinimumTrainingSamples) { + std::vector estimate(80, Eigen::Vector3d::Zero()); + std::vector gnss(80, Eigen::Vector3d::Zero()); + for (size_t i = 0; i < estimate.size(); ++i) { + estimate[i].x() = static_cast(i); + gnss[i].x() = static_cast(i); + } + + const auto window = + select_alignment_window(estimate, gnss, 5.0, 20, 10, true); + ASSERT_TRUE(window.ready); + EXPECT_GE(window.training_count(), 20u); + EXPECT_EQ(window.validation_count(), 10u); + EXPECT_EQ(window.training_count(), 20u); +} + +TEST(GNSSAlignment, HoldoutRejectsGrowingHeadingDrift) { + constexpr size_t kCount = 60; + std::vector estimate(kCount, Eigen::Vector3d::Zero()); + std::vector gnss(kCount, Eigen::Vector3d::Zero()); + + double x = 0.0; + double y = 0.0; + for (size_t i = 0; i < kCount; ++i) { + const double distance = 5.0 * static_cast(i) / + static_cast(kCount - 1); + const double heading_error = 0.50 * distance / 5.0; + if (i > 0) { + const double step = 5.0 / static_cast(kCount - 1); + x += step * std::cos(heading_error); + y += step * std::sin(heading_error); + } + estimate[i] = Eigen::Vector3d(x, y, 0.0); + gnss[i] = Eigen::Vector3d(distance, 0.0, 0.0); + } + + const auto window = + select_alignment_window(estimate, gnss, 2.5, 20, 10, false); + ASSERT_TRUE(window.ready); + const auto fit = fit_planar_alignment(estimate, gnss, window); + ASSERT_TRUE(fit.valid); + EXPECT_LT(fit.training_rms, 0.25); + EXPECT_GT(fit.validation_rms, 0.25); +} + +TEST(GNSSAlignment, StableRigidTransformPassesTrainingAndHoldout) { + constexpr size_t kCount = 50; + std::vector estimate(kCount, Eigen::Vector3d::Zero()); + std::vector gnss(kCount, Eigen::Vector3d::Zero()); + const Eigen::Rotation2Dd rotation(0.2); + for (size_t i = 0; i < kCount; ++i) { + estimate[i] = + Eigen::Vector3d(0.25 * i, 0.02 * std::sin(0.2 * i), 0.01 * i); + gnss[i].head<2>() = + rotation * estimate[i].head<2>() + Eigen::Vector2d(3.0, -2.0); + gnss[i].z() = estimate[i].z() + 0.4; + } + + const auto window = + select_alignment_window(estimate, gnss, 5.0, 20, 10, false); + ASSERT_TRUE(window.ready); + const auto fit = fit_planar_alignment(estimate, gnss, window); + ASSERT_TRUE(fit.valid); + EXPECT_LT(fit.training_rms, 1.0e-10); + EXPECT_LT(fit.validation_rms, 1.0e-10); +} + +TEST(GNSSFactorDelivery, ConfirmsExactFactorIdentities) { + int factor_a = 1; + int factor_b = 2; + int other = 3; + const std::vector graph = {&other, &factor_a, &factor_b}; + const std::vector indices = {0, 1, 2}; + const std::vector expected = {&factor_a, &factor_b}; + + EXPECT_TRUE(glim::gnss_detail::factor_batch_committed( + 1, expected, indices, graph.size(), + [&](size_t index) { return graph[index]; })); +} + +TEST(GNSSFactorDelivery, RejectsMissingOrDifferentFactor) { + int factor_a = 1; + int factor_b = 2; + int replacement = 3; + const std::vector graph = {&factor_a, &replacement}; + const std::vector indices = {0, 1}; + const std::vector expected = {&factor_a, &factor_b}; + + EXPECT_FALSE(glim::gnss_detail::factor_batch_committed( + 0, expected, indices, graph.size(), + [&](size_t index) { return graph[index]; })); + EXPECT_FALSE(glim::gnss_detail::factor_batch_committed( + 1, expected, indices, graph.size(), + [&](size_t index) { return graph[index]; })); +} + +} // namespace diff --git a/GLIM/glim_ext/package.xml b/GLIM/glim_ext/package.xml index 1c5296d3..a580c97a 100644 --- a/GLIM/glim_ext/package.xml +++ b/GLIM/glim_ext/package.xml @@ -11,6 +11,7 @@ ament_cmake glim + ament_cmake_gtest ament_cmake diff --git a/GLIM/glim_ros2/include/glim_ros/glim_ros.hpp b/GLIM/glim_ros2/include/glim_ros/glim_ros.hpp index 0316a245..c7025a1e 100644 --- a/GLIM/glim_ros2/include/glim_ros/glim_ros.hpp +++ b/GLIM/glim_ros2/include/glim_ros/glim_ros.hpp @@ -62,6 +62,7 @@ class GlimROS : public rclcpp::Node { void wait(bool auto_quit = false); void save(const std::string& path); + size_t num_submaps(); const std::vector>& extension_subscriptions(); diff --git a/GLIM/glim_ros2/src/glim_pcap_rosbag.cpp b/GLIM/glim_ros2/src/glim_pcap_rosbag.cpp index a6b2ed84..b57265ad 100644 --- a/GLIM/glim_ros2/src/glim_pcap_rosbag.cpp +++ b/GLIM/glim_ros2/src/glim_pcap_rosbag.cpp @@ -933,6 +933,13 @@ int main(int argc, char** argv) { glim->wait(auto_quit); glim->save(dump_path); + const size_t num_submaps = glim->num_submaps(); + if (num_submaps == 0) { + spdlog::critical( + "mapping produced zero submaps — odometry never initialized despite " + "dispatched primary scans; partial dump kept, exiting nonzero"); + return 1; + } if (!glim->ok()) { spdlog::error("run rejected by a mapping quality/safety extension — partial dump saved, exiting nonzero"); return 1; diff --git a/GLIM/glim_ros2/src/glim_ros/glim_ros.cpp b/GLIM/glim_ros2/src/glim_ros/glim_ros.cpp index 12cbe224..7f89bb5d 100644 --- a/GLIM/glim_ros2/src/glim_ros/glim_ros.cpp +++ b/GLIM/glim_ros2/src/glim_ros/glim_ros.cpp @@ -631,6 +631,10 @@ void GlimROS::save(const std::string& path) { } } +size_t GlimROS::num_submaps() { + return global_mapping ? global_mapping->num_submaps() : 0; +} + } // namespace glim RCLCPP_COMPONENTS_REGISTER_NODE(glim::GlimROS); diff --git a/GLIM/glim_ros2/src/glim_rosbag.cpp b/GLIM/glim_ros2/src/glim_rosbag.cpp index b41ccdd5..46a139a4 100644 --- a/GLIM/glim_ros2/src/glim_rosbag.cpp +++ b/GLIM/glim_ros2/src/glim_rosbag.cpp @@ -1240,6 +1240,14 @@ int main(int argc, char** argv) { glim->wait(auto_quit); glim->save(dump_path); + const size_t num_submaps = glim->num_submaps(); + if (num_submaps == 0) { + spdlog::critical( + "mapping produced zero submaps — input was empty/filtered or odometry " + "never initialized; partial dump kept, exiting nonzero"); + return 1; + } + if (!glim->ok()) { spdlog::error("run rejected by a mapping quality/safety extension — partial dump saved, exiting nonzero"); return 1; diff --git a/README.md b/README.md index 233e41b7..709fdab6 100644 --- a/README.md +++ b/README.md @@ -348,10 +348,14 @@ yaw; leave it at `0` for position-only GNSS publishers or publishers that use an identity quaternion to mean "orientation unavailable". The baseline can be injected with `--gnss-min-baseline`; its default matches the successful perception-ws Laguna configuration. The high-quality profile fits -the newest segment that still spans that baseline on both trajectories, so a -stationary/low-speed startup does not dominate the one-shot alignment. Use -`--no-gnss-recent-fit-window` for the legacy all-history behavior, and inject -its acceptance gate with `--gnss-fit-max-rms` (default `0.25 m`). +the newest segment that still spans that baseline on both trajectories while +retaining at least `--gnss-fit-min-samples 20`. It excludes the newest +`--gnss-fit-validation-samples 10` from the fit and must predict that suffix +within `--gnss-fit-max-rms 0.25 m`, in addition to passing the same in-sample +RMS gate. This prevents a two-point/recent-window fit and catches growing +estimate-side heading drift that a rigid in-sample alignment can absorb. Use +`--no-gnss-recent-fit-window` for an all-history training prefix; held-out +validation and the sample minimum still apply. `--offload-dir` must be an absolute, empty, per-run directory; GLIM refuses stale contents. diff --git a/scripts/generate_glim_mapping_config.py b/scripts/generate_glim_mapping_config.py index b6cace16..4911725c 100755 --- a/scripts/generate_glim_mapping_config.py +++ b/scripts/generate_glim_mapping_config.py @@ -11,7 +11,9 @@ * ``--gnss-min-baseline`` controls when the one-shot world/GNSS alignment is initialized; the default is the 10 m Laguna value validated in perception-ws. * ``--gnss-fit-max-rms`` is the quality gate for that alignment; the generated - high-quality profile fits the newest segment that still spans the baseline. + high-quality profile fits at least ``--gnss-fit-min-samples`` and predicts a + newest ``--gnss-fit-validation-samples`` suffix that was excluded from the + fit. * ``--keyframes-per-submap`` controls how many locally optimized scans are grouped into one rigid geometric submap. The one-scan perception-ws setting remains the default. @@ -107,6 +109,10 @@ def build_configs(args: argparse.Namespace) -> dict[str, Any]: raise ValueError("--gnss-min-baseline must be a finite positive value") if not math.isfinite(args.gnss_fit_max_rms): raise ValueError("--gnss-fit-max-rms must be finite") + if args.gnss_fit_min_samples < 3: + raise ValueError("--gnss-fit-min-samples must be at least 3") + if args.gnss_fit_validation_samples < 1: + raise ValueError("--gnss-fit-validation-samples must be positive") if args.keyframes_per_submap <= 0: raise ValueError("--keyframes-per-submap must be a positive integer") if ( @@ -381,6 +387,8 @@ def build_configs(args: argparse.Namespace) -> dict[str, Any]: "gnss_msg_type": args.gnss_msg_type, "min_baseline": args.gnss_min_baseline, "fit_recent_baseline_window": args.gnss_recent_fit_window, + "fit_min_samples": args.gnss_fit_min_samples, + "fit_validation_samples": args.gnss_fit_validation_samples, # Missing/invalid covariance falls back to this honest floor. "prior_inf_scale": [100.0, 100.0, 25.0], "prior_inf_floor": [100.0, 100.0, 25.0], @@ -432,6 +440,8 @@ def build_configs(args: argparse.Namespace) -> dict[str, Any]: "gnss_msg_type": args.gnss_msg_type, "gnss_min_baseline_m": args.gnss_min_baseline, "gnss_recent_fit_window": args.gnss_recent_fit_window, + "gnss_fit_min_samples": args.gnss_fit_min_samples, + "gnss_fit_validation_samples": args.gnss_fit_validation_samples, "gnss_fit_max_rms_m": args.gnss_fit_max_rms, "keyframes_per_submap": args.keyframes_per_submap, "odom_rotation_stddev_rad": args.odom_rotation_stddev, @@ -511,12 +521,27 @@ def parse_args() -> argparse.Namespace: help="Fit the newest segment that still spans --gnss-min-baseline on " "both trajectories; disable to reproduce the legacy all-history fit.", ) + parser.add_argument( + "--gnss-fit-min-samples", + type=int, + default=20, + help="Minimum number of samples used to estimate the world/GNSS " + "alignment (high-quality default: 20).", + ) + parser.add_argument( + "--gnss-fit-validation-samples", + type=int, + default=10, + help="Newest samples excluded from fitting and used for out-of-sample " + "validation (high-quality default: 10).", + ) parser.add_argument( "--gnss-fit-max-rms", type=float, default=0.25, - help="Maximum RMS residual in metres for latching the world/GNSS fit; " - "<= 0 disables this gate (high-quality default: 0.25).", + help="Maximum training and held-out prediction RMS in metres for " + "latching the world/GNSS fit; <= 0 disables both gates " + "(high-quality default: 0.25).", ) parser.add_argument("--points-topic", default="/luminar_front/points") parser.add_argument("--primary-frame", default="luminar_front") From f0a94f5d0de4726f663f833273fd7001ab75f23c Mon Sep 17 00:00:00 2001 From: FieldDiTian Date: Mon, 27 Jul 2026 23:08:28 -0700 Subject: [PATCH 2/4] Add compressed-map pipeline and lossless replay --- GICP_plusplus/README.md | 120 +- GICP_plusplus/cfg/lidar_reliable_replay.yaml | 12 + GICP_plusplus/cfg/localization.yaml | 108 +- .../include/gicp_plusplus/localization.h | 21 +- .../launch/localization_with_tf.launch.py | 41 + GICP_plusplus/src/localization.cc | 537 ++++++-- README.md | 117 +- gicp_localization/cfg/localization.yaml | 5 +- scripts/build_consistent_pcd.py | 506 ++++++++ scripts/export_glim_dump_to_pcd.py | 113 +- scripts/generate_gicp_topdown.py | 1089 +++++++++++++++++ scripts/offset_odom.py | 115 ++ scripts/run_gicp_replay_audit.sh | 348 ++++++ 13 files changed, 2917 insertions(+), 215 deletions(-) create mode 100644 GICP_plusplus/cfg/lidar_reliable_replay.yaml create mode 100755 scripts/build_consistent_pcd.py create mode 100755 scripts/generate_gicp_topdown.py create mode 100755 scripts/offset_odom.py create mode 100755 scripts/run_gicp_replay_audit.sh diff --git a/GICP_plusplus/README.md b/GICP_plusplus/README.md index 06de102b..03b30373 100644 --- a/GICP_plusplus/README.md +++ b/GICP_plusplus/README.md @@ -31,15 +31,39 @@ local ENU. - **small_gicp GICP scan-to-map matching** against a single pre-built PCD map (no submap stitching at runtime). - **IMU + LiDAR pipeline**: IMU integrates a motion prior between scans; GICP refines; a geometric observer fuses the two and propagates pose at IMU rate (~100 Hz). -- **Multi-LiDAR concatenation** (`lidar_concat`): 3x Luminar (`luminar_front` primary + `luminar_right`/`luminar_left` merged). Luminar sweeps are matched by **absolute per-point time** (endpoint-range error ≤ 10 ms; header time only as tie-break — headers carry 66–92 ms acquisition phase on AV-24 while point clocks agree to <1 ms), transformed via offline-resolved extrinsics, and byte-appended onto the primary. An **asynchronous front/aux synchronizer** (see below) decouples waiting for the point-aligned right sweep from the subscription callback, so aux timing can never cost front scans. A strict merge guard (`require_all_aux` / `abort_on_merge_failure`, identical semantics + defaults to GLIM) controls whether an incomplete merge degrades or skips the scan. -- **Confidence-weighted gating** (P1 rework, 2026-07 — replaces the old binary gates): - - Hard fitness reject (`gicp/fitnessRejectThreshold`) — catastrophic backstop, unchanged. - - **Per-map fitness-ratio gates** (`gicp/fitnessBaseline/*`, `fitnessRatioRejectThreshold`): gates operate on fitness divided by a rolling median of accepted-frame fitness, so they survive cross-run maps whose absolute fitness floor differs 5–10× from the calibration map. `seedBaseline` keeps them live during warm-up. +- **Optional multi-LiDAR concatenation** (`lidar_concat`): the production + perception-ws contract builds the map offline from all three LiDARs but runs + online GICP on `luminar_front` only, so concat is disabled by default to meet + the live 10 Hz deadline. Set `lidar_concat_enabled:=true` explicitly for a + synchronization/diagnostic A/B. In that mode, raw epoch-ns carriers are + matched by **absolute per-point time** (endpoint-range error ≤ 10 ms; header + time only as tie-break). Laguna's ordinary FLOAT64 seconds-since-sweep-start + carrier instead uses GLIM's safe future-header fallback, then rebases each + auxiliary point time onto the primary header before deskew. An + **asynchronous Luminar front worker/aux synchronizer** keeps DDS reception + independent of GICP latency and prevents an early concat release from + selecting the previous side sweep. A strict merge guard (`require_all_aux` / + `abort_on_merge_failure`, identical semantics + defaults to GLIM) controls + whether an incomplete merge degrades or skips the scan. +- **Map-independent deployment gating** (Laguna/perception-ws parity): + - Absolute and rolling-ratio fitness rejection are disabled in the deployed + default because their scale changes with map density, scene and speed. A + finite high ceiling remains so NaN/Inf fail closed. + - A 30% correspondence-support gate, finite-pose validation, physical + jump/yaw limits and RTK candidate sanity/recovery remain active. The optional + `fitnessBaseline/*` machinery is retained for controlled A/B tests. - **Degeneracy partial update** (`gicp/degeneracy/*`): when the hessian condition proxy trips `hessianCondMax`, the correction is projected onto well-constrained eigen-directions of the vehicle-re-centered, unit-scaled 6×6 hessian (full-6D by default — coupled rot/trans null directions included) and the IMU prior is kept along degenerate axes. Accepted-with-projection logs `status=ok_partial`; wholesale `rejected_hessian` remains only for the all-axes-degenerate case. Legacy binary gate available via `degeneracy/partialUpdate: false`. - **Yaw-consistency veto** (`gicp/yawGate/*`, independent of partialUpdate): a GICP yaw correction > `maxCorrDeg` vs. the IMU-integrated prior on a low-confidence match (ratio > `fitnessRatio`) keeps the IMU yaw — the wrong-basin *entry* signature the jump gate can't see. - Large-jump reject (compares the applied candidate to the IMU-predicted prior; speed/scan-dt-aware thresholds). - **IMU dead-reckoning fallback**: any non-accepted scan falls back to the IMU-integrated prior instead of freezing at the last accepted pose, seeded with the *current* IMU-propagated velocity (P2 fixed a stale-velocity bug that made multi-scan rejection streaks cut corners). -- **Ground-truth divergence cross-check** (optional): subscribes to a `gt_odom` topic, computes per-scan `gt_err=[trans,rot,dt]` against the pose actually applied (post-projection), publishes deltas. Diagnostic only — never feeds back into accept/reject. +- **Atlas divergence cross-check + production safety envelope** (optional): + subscribes to `gt_odom`, computes per-scan + `gt_err=[trans,rot,dt]` against the post-projection candidate and publishes + the deltas. With `max_candidate_position_error_m: 0` it is diagnostic only. + The Laguna deployment default is 5 m: a time-matched, RTK-quality Atlas + sample can reject a GICP candidate outside that broad envelope, but is not + blended into healthy poses inside it. This catches long-run repeated-track + wrong basins before they poison the observer. - **GT-driven pose recovery**: when GICP fails for N consecutive scans (default 5), snap pose+twist to a time-matched GT sample (composed through TF into `base_frame`) so GICP can re-acquire from a known-good state. Twist sources resolve independently (P2): angular rate backfills from the live bias-corrected gyro and linear velocity from GT pose finite-differencing when the odom twist is unpopulated — never zeroing a moving vehicle. Falls back to dead-reckoning when GT is unavailable. - **GT-bootstrapped initial pose** (optional): take the first GT message as the initial pose so the node starts at the right location regardless of bag offset. - **Local-ENU output** (operational contract): the primary `map_frame` pose / odom / path are already in the map's frame, which — with the adapter — is a fixed local-ENU datum (Putnam origin from the `race_metadata` TTL). GICP itself is frame-agnostic and simply reports the pose in the map's frame. @@ -95,6 +119,7 @@ ros2 launch gicp_plusplus localization_with_tf.launch.py \ | `odom_topic` | `/odom` | Declared and remapped by the launch but currently **unused** — the node creates no `odom` subscription; `use_odom_init` seeds from the first `gt_odom` message instead. | | `gt_odom_topic` | `/gps_p1/filtered_odom` | Atlas FusionEngine INS odometry, at `gps_antenna_top`. Used when `localization/gt_odom/enable=true` and/or `gt_recovery/enable=true`. Same frame as `base_frame`, so no TF correction is needed. | | `imu_only` | `false` | Disable GICP and propagate pose from IMU only (debug/sanity check). | +| `lidar_concat_enabled` | `false` | Opt in to front+left+right online GICP for synchronization/diagnostic A/B tests. Production uses a three-LiDAR offline map with front-only online GICP to meet 10 Hz. | | `urdf_path` | (auto-found) | Path to the URDF (`av24.urdf`) used for offline extrinsic resolution. The launch resolves it by walking up from the launch dir; `av24.urdf` is also installed into `share/gicp_plusplus`. | | `parent_frame` / `child_frame` | `base_link` / `luminar_front` | `child_frame` overrides `localization/lidar_frame` (the LiDAR link the node resolves extrinsics for); `parent_frame` is declared but currently unused (no static-TF helper is launched — `robot_state_publisher` provides the URDF tree). | | `map_path` | (yaml) | Override the yaml `localization/map_path` from the command line. | @@ -135,7 +160,7 @@ localization/rtk_gate/max_pose_var_z: 1.0 # m^2 (~1.0 m vertical std) |---|---|---| | **`tryRtkCalibrationStep`** — RTK-driven IMU bias calibration at startup | ✓ Yes | Needs cm-level truth to estimate gyro/accel bias residuals. If only degraded samples are available the init machine times out and falls back to stationary calibration. | | **INS heading prior** (`applyInsHeadingPriorToBasePose`, when `ins_prior/require_rtk_fixed` is set) | ✓ Yes | The prior rotates the GICP seed toward the INS heading; a degraded-heading sample would inject the very yaw error the prior exists to remove. | -| **Scan cross-check** — diagnostic `gt_pos_err_m` published on every accepted scan | ✓ Yes | A diagnostic comparing GICP against a sub-cm reference is only meaningful when the reference IS sub-cm. | +| **Scan cross-check / candidate sanity envelope** — `gt_pos_err_m` plus optional `max_candidate_position_error_m` reject | ✓ Yes | A cm-level diagnostic and a hard wrong-basin decision both require a trusted reference. Inside the configured radius Atlas position is not fused into GICP. | | **`maybeSnapPoseToGT`** — recovery after GICP loses LiDAR features | ✗ **No — accepts any sample** | When GICP can't match the LiDAR scan, the next-best truth is Atlas's pose at whatever quality it currently has — not our own software IMU dead-reckoning. See the next subsection. | | **`applyInitialPose` (use_odom_init)** | ✗ No | Falls back to whatever Atlas reports at startup; if RTK FIXED is required for init, set `localization/rtk_init/enable: true` (default) which gates through `tryRtkCalibrationStep`. | @@ -247,19 +272,21 @@ ENU frame. The `map`, the seed, and GICP must all share the one datum the adapte defines — a single-datum consistency requirement. UTM publishing is an optional legacy layer (see below), not the operational contract. -### GICP gating (P1 confidence-weighted rework) +### GICP gating (Laguna/perception-ws deployment default) ```yaml -gicp/fitnessRejectThreshold: 1.0 # hard reject: fitness > threshold (catastrophic backstop) - -# Per-map fitness normalization — gates operate on fitness / rolling-median -# of ACCEPTED-frame fitness, so they survive cross-run maps whose absolute -# floor differs 5-10x from the calibration map: -gicp/fitnessBaseline/enable: true +gicp/maxCorrespondenceDistance: 1.0 +gicp/minCorrespondences: 0 +gicp/minCorrespondenceRatio: 0.3 +gicp/fitnessRejectThreshold: 1000000000.0 # finite ceiling; NaN/Inf still fail closed + +# Optional rolling fitness normalization is retained for A/B experiments, but +# disabled in the deployed Laguna contract: +gicp/fitnessBaseline/enable: false gicp/fitnessBaseline/window: 201 # rolling-median window (~20 s @ 10 Hz) gicp/fitnessBaseline/minSamples: 50 # rolling median takes over after this gicp/fitnessBaseline/seedBaseline: 0.28 # warm-up baseline so gates are live from frame 1 (re-measure per map!) -gicp/fitnessRatioRejectThreshold: 2.0 # wrong-basin gate: reject when ratio exceeds this +gicp/fitnessRatioRejectThreshold: 0.0 # disabled for perception-ws parity # Degeneracy partial update (replaces the old binary hessian reject): gicp/hessianCondMax: 5.0e9 # TRIGGER: when tripped, project instead of reject @@ -288,10 +315,10 @@ well-constrained directions (the IMU prior holds the degenerate ones) — `status=ok_partial`. The old behavior (reject the whole scan) produced 253-frame dead-reckoning streaks on cross-run replays; wholesale `rejected_hessian` now fires only when all six axes are degenerate. The -fitness-ratio gate catches the opposite failure (wrong-basin matches accepted -with good-looking fitness). Score a replay with -`scripts/analyze_scan_debug_log.py`; it reports the accepted-fitness baseline -and suggests ratio thresholds for the map under test. +default support and physical gates catch loss of overlap or impossible motion +without assuming a particular map's fitness scale. Score a replay with +`scripts/analyze_scan_debug_log.py`; it reports accepted fitness and support so +optional ratio thresholds can still be evaluated in an explicit A/B. ### Multi-LiDAR concatenation @@ -304,22 +331,27 @@ coherent only when its endpoint-range error vs the primary distance is only a tie-break. Header-nearest selection is exactly the wrong-sweep failure mode (a one-period-early sweep produced ~149 ms merged spans and corrupted deskew); it is retained solely for non-Luminar sensors. -In Luminar mode a primary with no usable point-time range merges **front-only** -(all aux omitted, `unsupported_point_time`) — header matching is not a safe -substitute. Big-endian clouds are rejected before matching. +In Luminar mode an unsupported time layout merges **front-only** (all aux +omitted, `unsupported_point_time`). The supported Laguna FLOAT64 +seconds-since-sweep-start layout uses a future-header watermark and nearest +header selection; it is not treated as an unsupported absolute-time stream. +Big-endian clouds are rejected before matching. ```yaml localization/lidar_concat/enabled: true +localization/lidar_concat/reliable_qos: false # live BEST_EFFORT default; opt into RELIABLE for lossless bag audits localization/lidar_concat/aux_topics: ["/luminar_right/points", "/luminar_left/points"] localization/lidar_concat/aux_frames: ["luminar_right", "luminar_left"] localization/lidar_concat/luminar_point_time_threshold_s: 0.010 # ABSOLUTE point-time acceptance gate (Luminar) -localization/lidar_concat/time_threshold: 0.1 # non-Luminar fallback matching + tie-break ONLY +localization/lidar_concat/time_threshold: 0.05 # non-Luminar/relative-time header matching gate localization/lidar_concat/buffer_size: 200 # per-aux ring depth (P4: raised from 20 — 2 s of history silently degraded frames) -localization/lidar_concat/aux_time_offsets: [] # measured residual point-clock corrections; keep zero — +localization/lidar_concat/aux_time_offsets: [0.0, 0.0] # measured residual point-clock corrections; keep zero — # header phase is NOT clock evidence. Validated at startup # (finite, |v| <= 0.5 s; refuses to start otherwise). +localization/lidar_concat/float64_time_is_epoch_ns: false # false = FLOAT64 relative seconds (Laguna); + # true only for verified raw uint64 epoch-ns bytes -# Async front/aux synchronizer (Luminar production path): +# Async Luminar front worker (front-only production and concat diagnostic paths): localization/lidar_concat/future_aux_wait_timeout_s: 0.150 # arrival-time release deadline for a pending front localization/lidar_concat/primary_queue_size: 8 # HARD bound; overflow = counted overload drop of the OLDEST front @@ -329,21 +361,24 @@ localization/lidar_concat/abort_on_merge_failure: true # only relev localization/lidar_concat/max_consecutive_aux_merge_failures: 10 ``` -### Async front/aux synchronizer +### Async Luminar front worker and aux synchronizer -The point-coherent right sweep arrives ~92 ms **after** the front cloud -(acquisition phase), so waiting for it inside the subscription callback would -exceed the 20 Hz front period and silently shed front clouds at the QoS layer -(the Result-33 regression: 78 % of front sweeps lost). Instead: +Every Luminar front scan, including the production front-only path, enters a +bounded worker queue. A long GICP iteration therefore cannot block its DDS +subscription callback and silently exhaust the RELIABLE keep-last history. +When concat is enabled, the point-coherent right sweep arrives ~92 ms **after** +the front cloud (acquisition phase), so aux waiting also stays off the +subscription callback. Instead: - The front callback only **validates and enqueues** (microseconds, never blocks). Aux callbacks decode the point-time range once, buffer, and wake the worker. - A dedicated **worker thread owns release order** and runs the unchanged - merge→deskew→GICP pipeline. A front is released when every aux is *matched* - (in-gate) or *final* (watermark: the aux stream's point time has passed the - front's window), or at its `future_aux_wait_timeout_s` deadline — merging - whatever matched. Fronts release in arrival (FIFO) order. + merge→deskew→GICP pipeline. Front-only scans release immediately in FIFO + order. With concat enabled, an absolute-time front releases when every aux + is *matched* (in-gate) or *final* (point-time watermark); relative FLOAT64 + waits until every aux stream reaches the front header, then selects the + nearest header. The timeout remains the live fail-safe. - **Aux state can never drop a front.** The only front drops are: invalid primary data (`front_invalid`), explicit shutdown accounting, the coordinated epoch-reset queue purge (`front_epoch_dropped` — queued fronts @@ -368,8 +403,9 @@ exceed the 20 Hz front period and silently shed front clouds at the QoS layer (0=all_matched 1=watermark 2=timeout 4=shutdown_drain 5=primary_no_abstime, −1=legacy path), `debug/front_wait_ms`, `debug/primary_queue_depth`, alongside the existing `merged_aux_count` / `aux_merge_dt_s` / `scan_time_span_s` records. - Healthy replay: ~all `all_matched`, `front_wait_ms` ≈ 92 ms, - `front_overload_dropped=0`, merged span ≈ 49 ms (never ≥ 100 ms). + Healthy front-only replay: ~all `all_matched`, near-zero `front_wait_ms`, + and `front_overload_dropped=0`. Healthy concat replay waits about 92 ms and + reports a merged span around 49 ms (never ≥ 100 ms). The operational acceptance checks are the conservation invariant above, `front_overload_dropped=0`, mostly `all_matched` releases, merged span below @@ -408,6 +444,7 @@ expected on PTP-synchronized Iris units whose absolute point clocks agree to localization/gt_odom/enable: true localization/gt_odom/buffer_size: 200 # ~2 s of history at 100 Hz localization/gt_odom/max_dt: 0.1 # max scan-to-GT lookup gap +localization/gt_odom/max_candidate_position_error_m: 5.0 # RTK-quality wrong-basin envelope; 0 = diagnostic only localization/gt_recovery/enable: true # snap to GT after sustained GICP failure localization/gt_recovery/min_consecutive_failures: 5 # snap after N consecutive non-accepts (P2: raised from 1 — per-frame snapping masked dead-reckoning quality) @@ -428,6 +465,9 @@ odom/geo/Kq: 4.0 # Orientation odom/geo/Kab: 0.0 # Online accel-bias adaptation disabled odom/geo/Kgb: 0.0 # Online gyro-bias adaptation disabled odom/geo/delta_correction: true # P3: apply GICP as a time-free delta (see below) +odom/geo/max_pos_correction: 2.0 # Bound one accepted scan's observer position injection +odom/geo/max_vel_correction: 5.0 # Bound one accepted scan's observer velocity injection +odom/geo/max_state_speed: 100.0 # Physical fail-safe above Laguna race speed ``` `Kab`/`Kgb` are intentionally zero for the fused Point One (Atlas) INS path. Initial @@ -442,7 +482,9 @@ which is zero-mean on straights but a systematic yaw/position lag in turns run-12 baseline). With `delta_correction: true` the observer instead applies the time-free correction `T_meas · T_prior⁻¹` to the current state: perfect IMU/GICP agreement produces zero correction at any latency. Gains unchanged. - +The three observer bounds prevent a wrong-basin residual from turning directly +into an unphysical prediction and an expensive full-map miss; they are +fail-safes, not normal-operation tuning targets. **Bias path (P3).** IMU biases are subtracted **once, at buffering** in `callbackImu`, so `propagateState`, the scan prior (`integrateImu`), and per-point deskew all integrate the same corrected signal. (Previously only @@ -598,8 +640,10 @@ Look in the log for one of: ### Scan dropouts during sharp turns -If you see SCAN DEBUG gaps > 200 ms during turns, diagnose the front/aux -synchronizer rather than reaching for `time_threshold` — in Luminar mode that +If you see SCAN DEBUG gaps > 200 ms during turns, first inspect the async front +worker counters (`front_overload_dropped`, queue depth, and the conservation +summary). In concat mode, diagnose the aux synchronizer rather than reaching +for `time_threshold` — in Luminar mode that header window is only a fallback/tie-break, and the authoritative match is the decoded per-point endpoint error (`luminar_point_time_threshold_s`), so raising `time_threshold` will not close a real point-time gap. Inspect the release diff --git a/GICP_plusplus/cfg/lidar_reliable_replay.yaml b/GICP_plusplus/cfg/lidar_reliable_replay.yaml new file mode 100644 index 00000000..b28a7c5b --- /dev/null +++ b/GICP_plusplus/cfg/lidar_reliable_replay.yaml @@ -0,0 +1,12 @@ +/luminar_front/points: + reliability: reliable + history: keep_last + depth: 20 +/luminar_right/points: + reliability: reliable + history: keep_last + depth: 20 +/luminar_left/points: + reliability: reliable + history: keep_last + depth: 20 diff --git a/GICP_plusplus/cfg/localization.yaml b/GICP_plusplus/cfg/localization.yaml index 49f66bb8..ff5f6864 100644 --- a/GICP_plusplus/cfg/localization.yaml +++ b/GICP_plusplus/cfg/localization.yaml @@ -120,6 +120,13 @@ localization/gt_odom/enable: true localization/gt_odom/buffer_size: 200 # ring buffer depth (~2s @ 100Hz GT) localization/gt_odom/max_dt: 0.1 # seconds; reject lookups farther than this from scan stamp + # On-car wrong-basin safety envelope. Atlas position is NOT blended into + # every accepted GICP pose: it only rejects a candidate outside this broad + # radius after the RTK covariance/time gates pass. Five consecutive + # rejects invoke the existing recovery path. This prevents the observed + # 20-200 m parallel-structure slides from poisoning observer velocity. + # Set 0 for a deliberately GNSS-independent mapping-quality experiment. + localization/gt_odom/max_candidate_position_error_m: 5.0 # RTK quality gate, applied PER CONSUMER at consumption time — NOT a # buffer filter. Every incoming gt_odom sample enters the buffer; the @@ -156,9 +163,10 @@ # scans in a row, snap current_pose / lidarPose / state.{p,q,v} to the # time-matched GT sample so GICP can re-acquire from a known-good state. # The GT pose+twist is transformed from msg->child_frame_id into base_frame - # via TF (cached on first GT message). IMU biases are preserved. Disabled by - # default — production deployments without a GT topic keep the existing - # dead-reckoning behavior. Setting enable=true forces gt_odom/enable=true. + # via TF (cached on first GT message). IMU biases are preserved. The Laguna + # deployment profile enables this explicitly; deployments without an Atlas + # INS reference should disable it. Setting enable=true forces + # gt_odom/enable=true. localization/gt_recovery/enable: true # P2#3: raised 1 -> 5. With min=1 the snap fired on EVERY rejected frame # (1,860 snaps in run 12), masking dead-reckoning quality in all replay @@ -211,10 +219,20 @@ # sensor frame, rebases per-point timestamps onto the primary clock, and appends # the bytes to the primary PointCloud2. Downstream steps see a single cloud in # primary frame with one coherent timebase. - localization/lidar_concat/enabled: true + # Deployment contract: the OFFLINE GLIM map uses all three LiDARs, while + # ONLINE GICP follows perception-ws and consumes the front LiDAR only. A + # three-cloud GICP frame triples preprocessing/deskew work and cannot meet + # the 10 Hz live deadline on the current CPU. Keep concat as an explicit + # synchronization/diagnostic A/B option via launch + # lidar_concat_enabled:=true; never slow a deployment bag to hide overload. + localization/lidar_concat/enabled: false + # Live default is false (BEST_EFFORT SensorDataQoS). Set true only when an + # offline rosbag publisher is also overridden to RELIABLE; this prevents + # large three-cloud replay bursts from being silently dropped by DDS. + localization/lidar_concat/reliable_qos: false localization/lidar_concat/aux_topics: ["/luminar_right/points", "/luminar_left/points"] localization/lidar_concat/aux_frames: ["luminar_right", "luminar_left"] - localization/lidar_concat/time_threshold: 0.1 # seconds; NON-LUMINAR fallback matching + tie-break only. + localization/lidar_concat/time_threshold: 0.05 # seconds; NON-LUMINAR/relative-time header matching gate. # Luminar acceptance is gated by the absolute point-time # threshold below, never by header proximity (headers carry # 66-92 ms acquisition phase on AV-24 with coherent point times). @@ -222,10 +240,12 @@ # max(|min-min|,|max-max|) must be <= this. Header distance is only a # tie-break between candidates with equal range error. localization/lidar_concat/luminar_point_time_threshold_s: 0.010 - # Async front/aux synchronizer (Luminar production path). The front - # callback only validates + enqueues; a worker releases fronts in order - # when every aux is matched or final (watermark), or at this arrival-time - # deadline — merging whatever matched. Aux state can NEVER drop a front. + # Async Luminar front worker (front-only production and concat diagnostic + # paths). The front callback only validates + enqueues, so a long GICP + # iteration cannot block DDS reception. With concat enabled, the worker + # releases fronts in order when every aux is matched/final (watermark), or + # at this arrival-time deadline, merging whatever matched. Front-only + # releases immediately. Aux state can NEVER drop a front. localization/lidar_concat/future_aux_wait_timeout_s: 0.150 # HARD bound on the pending-front queue (compute-overload policy). If the # GICP pipeline is slower than the front input rate, the OLDEST queued @@ -240,7 +260,17 @@ # header-delta extrema, while the geometric point-time regression measured # |offset| < 11 ms on both runs. Keep zero until a geometric point-time # measurement establishes a value; never copy header phase in here. - localization/lidar_concat/aux_time_offsets: [] + # Keep an explicitly typed vector for ROS 2 parameter parsing. An empty + # YAML sequence has no element type and Jazzy rejects it before node + # startup; zeros preserve the intended "no measured correction" behavior. + localization/lidar_concat/aux_time_offsets: [0.0, 0.0] + # Luminar point-time carrier contract. Laguna's decoder declares FLOAT64 + # and stores ordinary seconds-since-sweep-start (0..~0.1), so keep false: + # aux scans are matched by the safe future-header fallback and their + # relative times are rebased onto the primary header before deskew. + # Set true only for a driver known to mislabel raw uint64 epoch-ns bits as + # FLOAT64. UINT8[8] raw epoch-ns is detected independently. + localization/lidar_concat/float64_time_is_epoch_ns: false localization/lidar_concat/buffer_size: 200 # per-aux ring buffer depth (P4#3: raised 20 -> 200 # for GLIM parity; 20 = only 2 s of aux history at # 10 Hz, so brief stalls degraded frames to fewer @@ -422,8 +452,14 @@ # Optional hard clamps on the per-update correction magnitude (0 = disabled). # The dt cap above already bounds the gain; enable these only if you also want # an absolute ceiling on how far one scan can move the state. - odom/geo/max_pos_correction: 0.0 # m — clamp per-update position correction - odom/geo/max_vel_correction: 0.0 # m/s — clamp per-update velocity correction + # Long-run Laguna failure audit: a wrong-basin candidate created a 25 m + # residual and the unbounded Kv path injected >120 m/s into the observer, + # turning subsequent local searches into second-scale full-map misses. + # These limits are well above healthy per-scan corrections but prevent a + # bad candidate from converting directly into an unstable prediction. + odom/geo/max_pos_correction: 2.0 # m — clamp per-update position correction + odom/geo/max_vel_correction: 5.0 # m/s — clamp per-update velocity correction + odom/geo/max_state_speed: 100.0 # m/s — above Laguna race speed, finite safety cap # P1 yaw-safety fix #3: ORIENTATION got the clamp position/velocity always # had. The observer pulls dt_eff*Kq (~45%) of the orientation error per # update; unclamped, one bad accepted scan injects tens of degrees of @@ -488,27 +524,30 @@ localization/jump/yaw_total_max_deg: 15.0 # GICP Registration Parameters - # Maximum number of iterations (increased for better convergence) - gicp/maxIterations: 128 + # Match the deployed perception-ws Laguna localizer. Fifty iterations + # leaves ample convergence margin while bounding the 10 Hz CPU deadline. + gicp/maxIterations: 50 # Number of neighbors used to compute per-point covariances (typical: 20) - gicp/correspondenceRandomness: 20 + gicp/correspondenceRandomness: 10 # Maximum correspondence distance (meters) # Points further apart than this won't be considered correspondences # Tightened from 5.0 to cut spurious matches against the dense full map. - gicp/maxCorrespondenceDistance: 4.0 + gicp/maxCorrespondenceDistance: 1.0 # Convergence criteria — tightened now that downsampling is re-enabled - gicp/transformationEpsilon: 0.004 - gicp/rotationEpsilon: 0.004 - - # GICP result gating: reject poor registrations and fall back to IMU dead-reckoning. - # Good alignments on the full map hit fitness ~0.01–0.05 (p90 0.04, p99 0.39). - # Keep this at 1.0, not tighter: during the first ~4 scans the initial pose is still - # settling, so fitness spikes to 1.2–1.4 on correct geometry; a 0.3 threshold rejects - # those and the node gets stuck at initial pose with no GICP updates to recover from. - gicp/fitnessRejectThreshold: 1.0 # Reject GICP if fitness score exceeds this value + gicp/transformationEpsilon: 0.01 + gicp/rotationEpsilon: 0.004363323 + + # Laguna/perception-ws parity: do not gate a registration on the absolute + # fitness magnitude. That value changes with map density, scene and speed: + # on the compressed 32.54M map, candidates with 99% correspondence support + # and only 0.9–1.1 m TTL error reached fitness 6–8 in the first fast turn. + # The old 1.0 gate rejected those correct poses and created a cascade. + # Keep a finite high ceiling because this value also bounds the optional + # non-converged fallback; NaN/Inf still fail closed. + gicp/fitnessRejectThreshold: 1000000000.0 # PR#6: bounds on the NON-CONVERGED low-fitness fallback ("effectively # converged"). The fallback exists for max-iterations-at-speed scans whose # correction is small; unbounded, it also admitted wrong-basin results @@ -518,8 +557,10 @@ # classified failed_to_converge instead. <=0 disables a bound. # PR validation: nonconv accepts 1145->397; accepts with INS err >=50 m # 99->3; p90 INS err 21.6->12.7 m. - gicp/nonConvergedFitnessOkMaxTransM: 3.0 - gicp/nonConvergedFitnessOkMaxRotDeg: 5.0 + # perception-ws lets support and physical gates judge a final candidate + # even when small_gicp exhausts its iteration budget. + gicp/nonConvergedFitnessOkMaxTransM: 0.0 + gicp/nonConvergedFitnessOkMaxRotDeg: 0.0 # Minimum correspondence-support gate (REVIEW FIX 2026-07-08). Mean # fitness over a handful of inliers can look excellent while the scan @@ -530,8 +571,10 @@ # it (status: rejected_support; SCAN DEBUG: support=[count,ratio,reeval]). # <=0 disables either bound. Re-baseline against a replay scorecard if the # scan preprocessing (voxel size, crop) changes materially. - gicp/minCorrespondences: 500 - gicp/minCorrespondenceRatio: 0.2 + # perception-ws uses a normalized support gate, which remains meaningful + # when scan point count changes with crop/voxel settings. + gicp/minCorrespondences: 0 + gicp/minCorrespondenceRatio: 0.3 gicp/rejectLargeJumps: true # Hard-reject results that exceed the jump thresholds below # ========== P1 gating rework (docs/action_plan_turn_error_20260704.md) ========== @@ -562,7 +605,10 @@ # (fitness_ratio > yawGate/fitnessRatio) keeps the IMU yaw instead. # This is the wrong-basin ENTRY signature the jump gate can't see # (bad accepts had jump medians of only 1.87 m / 1.38 deg). - gicp/fitnessBaseline/enable: true + # The deployed perception-ws path has no rolling fitness-ratio rejection. + # Support, finite-pose, physical jump/yaw and GT sanity/recovery guards + # remain active. + gicp/fitnessBaseline/enable: false gicp/fitnessBaseline/window: 201 # accepted-frame samples in the rolling median (~20 s @ 10 Hz) gicp/fitnessBaseline/minSamples: 50 # rolling median takes over after this many accepted frames # Warm-up seed (review fix): baseline used until minSamples accepted frames @@ -572,7 +618,7 @@ # (scripts/analyze_scan_debug_log.py; run-12 sparse cross-run map: 0.28). # RE-MEASURE after the dense-map rebuild. 0 = off (absolute-only warm-up). gicp/fitnessBaseline/seedBaseline: 0.28 - gicp/fitnessRatioRejectThreshold: 2.0 # <=0 disables the wrong-basin ratio reject + gicp/fitnessRatioRejectThreshold: 0.0 # disabled for perception-ws parity gicp/degeneracy/partialUpdate: true # false = restore legacy binary hessian gate below # Full 6D coupled solution remapping (default). The re-centered hessian's # rotation coordinates are scaled by couplingLengthM (the typical diff --git a/GICP_plusplus/include/gicp_plusplus/localization.h b/GICP_plusplus/include/gicp_plusplus/localization.h index d8219ca0..a0cfe3fe 100644 --- a/GICP_plusplus/include/gicp_plusplus/localization.h +++ b/GICP_plusplus/include/gicp_plusplus/localization.h @@ -253,6 +253,11 @@ class LocalizationNode : public rclcpp::Node { bool gt_odom_enabled_; size_t gt_odom_buffer_size_; double gt_odom_max_dt_; // seconds; reject lookups farther than this from scan stamp + // Optional production sanity gate: reject a GICP candidate that is farther + // than this from a time-matched, RTK-quality Atlas pose. This is not a + // per-frame position fusion term; it only prevents a repeated-geometry + // wrong basin from entering the observer. 0 disables. + double gt_max_candidate_pos_error_m_ = 0.0; double gt_interp_max_gap_ = 0.5; // [P2 FIX 2026-07-14] max bracket width for GT interpolation std::deque gt_odom_buffer_; std::mutex gt_odom_mtx_; @@ -311,6 +316,10 @@ class LocalizationNode : public rclcpp::Node { std::vector> aux_lidars_; std::vector::SharedPtr> aux_subs_; rclcpp::CallbackGroup::SharedPtr aux_cb_group_; + // Live sensors commonly publish BEST_EFFORT, while lossless offline audits + // need RELIABLE delivery for multi-megabyte PointCloud2 bursts. The default + // remains the live-compatible sensor profile; replay opts in explicitly. + bool lidar_reliable_qos_ = false; bool concat_enabled_; double concat_time_threshold_; // Luminar acceptance gate: absolute point-time endpoint-range error @@ -330,8 +339,13 @@ class LocalizationNode : public rclcpp::Node { // dropped, and never for aux reasons. size_t concat_primary_queue_size_ = 8; size_t concat_buffer_size_; + // Luminar FLOAT64 time fields are scan-relative seconds by default (the + // Laguna decoder contract). Some drivers mislabel raw uint64 epoch-ns bits + // as FLOAT64; those require an explicit opt-in so ordinary doubles are + // never reinterpreted as multi-billion-second timestamps. + bool concat_float64_time_is_epoch_ns_ = false; - // ---- Async front/aux synchronizer (Luminar production path) ---- + // ---- Async Luminar front worker / aux synchronizer ---- // Contract: every valid front cloud is released exactly once, in order, // with 0..N_aux auxiliaries. Aux state can only change the source set; it // can never cause a front drop (front_dropped_due_to_aux == 0 by @@ -339,6 +353,7 @@ class LocalizationNode : public rclcpp::Node { struct PendingPrimaryCloud { sensor_msgs::msg::PointCloud2::ConstSharedPtr msg; LuminarTimestampRangeNs range; // decoded ONCE in the front callback + bool relative_float64_time = false; std::chrono::steady_clock::time_point enqueued; std::chrono::steady_clock::time_point deadline; uint64_t arrival_seq = 0; @@ -359,7 +374,7 @@ class LocalizationNode : public rclcpp::Node { // RELEASE_ALL_MATCHED, which reported a broken-schema stream as healthy. RELEASE_PRIMARY_NO_ABSTIME = 5, }; - bool sync_active_ = false; // Luminar + concat: worker owns release order + bool sync_active_ = false; // Luminar: worker owns bounded front processing std::deque primary_queue_; // guarded by sync_mtx_ std::mutex sync_mtx_; std::condition_variable sync_cv_; @@ -436,6 +451,7 @@ class LocalizationNode : public rclcpp::Node { // path. See deskewPointcloud(). uint64_t luminar_primary_min_ts_ns_ = 0; bool luminar_primary_min_ts_valid_ = false; + bool luminar_scan_time_is_epoch_ns_ = false; // Publishers rclcpp::Publisher::SharedPtr pose_pub; @@ -805,6 +821,7 @@ class LocalizationNode : public rclcpp::Node { double geo_observer_dt_max_; // s — cap on dt used in updateState corrections double geo_max_pos_correction_; // m — clamp per-update position correction (0=off) double geo_max_vel_correction_; // m/s — clamp per-update velocity correction (0=off) + double geo_max_state_speed_; // m/s — hard physical bound on observer speed (0=off) double geo_max_yaw_correction_deg_; // deg — clamp per-update yaw error before gain (0=off) — P1 yaw-safety double geo_max_rot_correction_deg_; // deg — clamp per-update total rotation error (0=off) diff --git a/GICP_plusplus/launch/localization_with_tf.launch.py b/GICP_plusplus/launch/localization_with_tf.launch.py index 594c3b27..bf91af06 100644 --- a/GICP_plusplus/launch/localization_with_tf.launch.py +++ b/GICP_plusplus/launch/localization_with_tf.launch.py @@ -44,6 +44,12 @@ def generate_launch_description(): odom_topic = LaunchConfiguration('odom_topic', default='/odom') gt_odom_topic = LaunchConfiguration('gt_odom_topic', default='/gps_p1/filtered_odom') imu_only = LaunchConfiguration('imu_only', default='false') + lidar_concat_enabled = LaunchConfiguration('lidar_concat_enabled', default='false') + require_all_aux = LaunchConfiguration('require_all_aux', default='false') + lidar_reliable_qos = LaunchConfiguration('lidar_reliable_qos', default='false') + future_aux_wait_timeout_s = LaunchConfiguration( + 'future_aux_wait_timeout_s', default='0.150') + primary_queue_size = LaunchConfiguration('primary_queue_size', default='8') urdf_path = LaunchConfiguration( 'urdf_path', default='') @@ -75,6 +81,27 @@ def generate_launch_description(): declare_imu_only_arg = DeclareLaunchArgument( 'imu_only', default_value=imu_only, description='If true, disable GICP and run IMU-only propagation') + declare_lidar_concat_enabled_arg = DeclareLaunchArgument( + 'lidar_concat_enabled', default_value=lidar_concat_enabled, + description='Merge configured auxiliary LiDARs into each online GICP scan. ' + 'Keep false for the production perception-ws contract: the offline ' + 'map uses three LiDARs, while live localization uses the front ' + 'LiDAR only to meet the 10 Hz deadline.') + declare_require_all_aux_arg = DeclareLaunchArgument( + 'require_all_aux', default_value=require_all_aux, + description='If true, skip any primary scan that does not merge every configured auxiliary LiDAR') + declare_lidar_reliable_qos_arg = DeclareLaunchArgument( + 'lidar_reliable_qos', default_value=lidar_reliable_qos, + description='Use RELIABLE keep-last(20) subscriptions for lossless offline LiDAR replay. ' + 'Keep false for BEST_EFFORT live sensors.') + declare_future_aux_wait_timeout_arg = DeclareLaunchArgument( + 'future_aux_wait_timeout_s', default_value=future_aux_wait_timeout_s, + description='Wall-clock aux deadline for the asynchronous Luminar front worker. ' + 'Front-only releases immediately; keep 0.150 s online for concat.') + declare_primary_queue_size_arg = DeclareLaunchArgument( + 'primary_queue_size', default_value=primary_queue_size, + description='Bounded pending-primary queue. Keep 8 online; a lossless slowed ' + 'offline audit may use a deeper queue.') declare_urdf_path_arg = DeclareLaunchArgument( 'urdf_path', default_value=urdf_path, description='Absolute path to the vehicle URDF used by robot_state_publisher ' @@ -140,6 +167,15 @@ def make_localization_node(context): localization_yaml_path, {'localization/lidar_frame': child_frame_value}, {'localization/imu_only': LaunchConfiguration('imu_only')}, + {'localization/lidar_concat/enabled': + LaunchConfiguration('lidar_concat_enabled')}, + {'localization/lidar_concat/require_all_aux': LaunchConfiguration('require_all_aux')}, + {'localization/lidar_concat/reliable_qos': + LaunchConfiguration('lidar_reliable_qos')}, + {'localization/lidar_concat/future_aux_wait_timeout_s': + LaunchConfiguration('future_aux_wait_timeout_s')}, + {'localization/lidar_concat/primary_queue_size': + LaunchConfiguration('primary_queue_size')}, {'localization/lidar_concat/urdf_path': urdf_file}, ] if map_path_value: @@ -200,6 +236,11 @@ def make_rviz_node(context): declare_odom_topic_arg, declare_gt_odom_topic_arg, declare_imu_only_arg, + declare_lidar_concat_enabled_arg, + declare_require_all_aux_arg, + declare_lidar_reliable_qos_arg, + declare_future_aux_wait_timeout_arg, + declare_primary_queue_size_arg, declare_urdf_path_arg, declare_parent_frame_arg, declare_child_frame_arg, diff --git a/GICP_plusplus/src/localization.cc b/GICP_plusplus/src/localization.cc index b578d1f4..f10ea465 100644 --- a/GICP_plusplus/src/localization.cc +++ b/GICP_plusplus/src/localization.cc @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -672,15 +673,13 @@ inline void clearPointTimeUnion(PointType& pt) { } // Decode a Luminar per-point ABSOLUTE epoch timestamp (uint64 ns) directly from -// PointCloud2 bytes. Single source of truth for which Luminar time encodings are -// accepted, shared by copyPointTimeFromCloud() (the per-point reader) and the -// multi-LiDAR deskew anchor capture in mergeAuxClouds(), so the two can never -// diverge on accepted formats. Returns false for an unsupported datatype. -// * UINT8[8] / FLOAT64 -> raw uint64 epoch ns (8 bytes, little-endian) +// PointCloud2 bytes. UINT8[8] is always raw epoch-ns; FLOAT64 is raw epoch-ns +// only under the explicit driver contract. Ordinary FLOAT64 is handled +// separately as scan-relative seconds. // -// Only 8-byte carriers are accepted because the whole Luminar path treats these -// times as ABSOLUTE epoch ns: mergeAuxClouds() leaves them unshifted and -// deskewPointcloud() anchors on (ts - primary_min). A 32-bit field (UINT32) +// Only 8-byte raw carriers are accepted on this absolute path because +// mergeAuxClouds() leaves them unshifted and deskewPointcloud() anchors on +// (ts - primary_min). A 32-bit field (UINT32) // cannot hold an absolute epoch (it wraps every ~4.29 s) -- it would be a // scan-relative counter, which this absolute path would silently misinterpret // (dropping the inter-scan offset between aux and primary). So UINT32 is @@ -688,9 +687,36 @@ inline void clearPointTimeUnion(PointType& pt) { // and degrades to "no per-point time" (rigid transform) rather than corrupting // deskew. `bytes_avail` (the field's room within point_step) guards the 8-byte // read against a malformed/short time field. -inline bool luminarRawTimestampNsFromBytes(const uint8_t* tp, uint8_t datatype, int count, size_t bytes_avail, uint64_t& out) { - if ((datatype == sensor_msgs::msg::PointField::FLOAT64 || - (datatype == sensor_msgs::msg::PointField::UINT8 && count == 8)) && +inline bool luminarUsesRawEpochCarrier( + uint8_t datatype, int count, bool float64_time_is_epoch_ns) { + return (datatype == sensor_msgs::msg::PointField::UINT8 && count == 8) || + (float64_time_is_epoch_ns && + datatype == sensor_msgs::msg::PointField::FLOAT64 && count == 1); +} + +inline bool luminarUsesRelativeFloat64Carrier( + uint8_t datatype, int count, bool float64_time_is_epoch_ns) { + return !float64_time_is_epoch_ns && + datatype == sensor_msgs::msg::PointField::FLOAT64 && count == 1; +} + +bool luminarCloudUsesRelativeFloat64( + const sensor_msgs::msg::PointCloud2& msg, bool float64_time_is_epoch_ns) { + int time_off = -1; + uint8_t datatype = 0; + int count = 0; + return findTimeField(msg, time_off, datatype, count) && + time_off >= 0 && + static_cast(time_off) + sizeof(double) <= msg.point_step && + luminarUsesRelativeFloat64Carrier( + datatype, count, float64_time_is_epoch_ns); +} + +inline bool luminarRawTimestampNsFromBytes( + const uint8_t* tp, uint8_t datatype, int count, size_t bytes_avail, + bool float64_time_is_epoch_ns, uint64_t& out) { + if (luminarUsesRawEpochCarrier( + datatype, count, float64_time_is_epoch_ns) && bytes_avail >= sizeof(uint64_t)) { std::memcpy(&out, tp, sizeof(uint64_t)); return true; @@ -703,7 +729,8 @@ inline bool luminarRawTimestampNsFromBytes(const uint8_t* tp, uint8_t datatype, // is acquisition phase and must not gate the merge. Fails closed (invalid // range) on any malformed point, short buffer, or unsupported time encoding. gicp_plusplus::LuminarTimestampRangeNs luminarTimestampRangeFromCloud( - const sensor_msgs::msg::PointCloud2& msg) { + const sensor_msgs::msg::PointCloud2& msg, + bool float64_time_is_epoch_ns) { gicp_plusplus::LuminarTimestampRangeNs range; // The decode assumes little-endian payloads; a big-endian cloud would yield @@ -733,7 +760,8 @@ gicp_plusplus::LuminarTimestampRangeNs luminarTimestampRangeFromCloud( uint64_t timestamp_ns = 0; if (!luminarRawTimestampNsFromBytes( msg.data.data() + i * msg.point_step + time_off, - time_datatype, time_count, bytes_avail, timestamp_ns)) { + time_datatype, time_count, bytes_avail, + float64_time_is_epoch_ns, timestamp_ns)) { return gicp_plusplus::LuminarTimestampRangeNs{}; } // [P3 FIX 2026-07-14] A zero per-point timestamp is the "no valid time" @@ -753,7 +781,8 @@ gicp_plusplus::LuminarTimestampRangeNs luminarTimestampRangeFromCloud( // Copy per-point time from PointCloud2 into the dlio::Point union for the configured sensor. // `point_step` bounds the field read so a malformed/short time field cannot read past the point. void copyPointTimeFromCloud(const uint8_t* src, int time_off, uint8_t time_datatype, int time_count, - uint32_t point_step, dlio::SensorType sensor, PointType& dst) { + uint32_t point_step, dlio::SensorType sensor, + bool float64_time_is_epoch_ns, PointType& dst) { if (time_off < 0 || static_cast(time_off) >= point_step) { return; } @@ -763,10 +792,21 @@ void copyPointTimeFromCloud(const uint8_t* src, int time_off, uint8_t time_datat switch (sensor) { case dlio::SensorType::LUMINAR: { uint64_t ts_raw = 0; - if (!luminarRawTimestampNsFromBytes(tp, time_datatype, time_count, bytes_avail, ts_raw)) { + if (luminarRawTimestampNsFromBytes( + tp, time_datatype, time_count, bytes_avail, + float64_time_is_epoch_ns, ts_raw)) { + std::memcpy(&dst.timestamp, &ts_raw, sizeof(uint64_t)); return; } - std::memcpy(&dst.timestamp, &ts_raw, sizeof(uint64_t)); + if (luminarUsesRelativeFloat64Carrier( + time_datatype, time_count, float64_time_is_epoch_ns) && + bytes_avail >= sizeof(double)) { + double relative_seconds = 0.0; + std::memcpy(&relative_seconds, tp, sizeof(double)); + if (std::isfinite(relative_seconds)) { + dst.timestamp = relative_seconds; + } + } return; } case dlio::SensorType::OUSTER: { @@ -853,10 +893,29 @@ void copyPointTimeFromCloud(const uint8_t* src, int time_off, uint8_t time_datat } void logLuminarTimestampStats(size_t num_points, const pcl::PointCloud& cloud, - size_t unique_ros_times) { + size_t unique_ros_times, bool raw_epoch_ns) { if (cloud.points.empty()) { return; } + if (!raw_epoch_ns) { + double tmin = std::numeric_limits::infinity(); + double tmax = -std::numeric_limits::infinity(); + for (const auto& pt : cloud.points) { + if (!std::isfinite(pt.timestamp)) continue; + tmin = std::min(tmin, pt.timestamp); + tmax = std::max(tmax, pt.timestamp); + } + std::fprintf( + stderr, + "[LUMINAR_DBG] %zu pts, %zu unique_ros_times, relative FLOAT64 " + "span_s=%.9f (min=%.9f max=%.9f)\n", + num_points, unique_ros_times, + (std::isfinite(tmin) && std::isfinite(tmax)) ? tmax - tmin + : -1.0, + tmin, tmax); + std::fflush(stderr); + return; + } uint64_t tmin = std::numeric_limits::max(); uint64_t tmax = 0; for (const auto& pt : cloud.points) { @@ -1155,9 +1214,20 @@ gicp_plusplus::LocalizationNode::LocalizationNode() : Node("gicp_plusplus_node") this->pointcloud_cb_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto pointcloud_sub_opt = rclcpp::SubscriptionOptions(); pointcloud_sub_opt.callback_group = this->pointcloud_cb_group; - // Use sensor-data QoS so rosbag/sensor publishers with BEST_EFFORT are compatible. + // Live default: sensor-data BEST_EFFORT. A lossless offline audit can opt + // into RELIABLE + a deeper queue, provided the rosbag publishers use the + // same reliable profile. This prevents silent DDS loss when three + // multi-megabyte clouds are published as one replay burst. + auto lidar_qos = rclcpp::SensorDataQoS(); + if (this->lidar_reliable_qos_) { + lidar_qos.reliable(); + lidar_qos.keep_last(20); + RCLCPP_INFO(this->get_logger(), + "LiDAR subscriptions use RELIABLE keep-last(20) QoS " + "(lossless offline replay mode)"); + } this->pointcloud_sub = this->create_subscription( - "pointcloud", rclcpp::SensorDataQoS(), + "pointcloud", lidar_qos, std::bind(&gicp_plusplus::LocalizationNode::callbackPointCloud, this, std::placeholders::_1), pointcloud_sub_opt); @@ -1187,7 +1257,7 @@ gicp_plusplus::LocalizationNode::LocalizationNode() : Node("gicp_plusplus_node") for (size_t i = 0; i < this->aux_lidars_.size(); ++i) { const int idx = static_cast(i); auto sub = this->create_subscription( - this->aux_lidars_[i]->topic, rclcpp::SensorDataQoS(), + this->aux_lidars_[i]->topic, lidar_qos, [this, idx](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { this->callbackAuxPointCloud(idx, std::move(msg)); }, @@ -1448,23 +1518,25 @@ gicp_plusplus::LocalizationNode::LocalizationNode() : Node("gicp_plusplus_node") this->applyInitialPoseFromParams(); - // Async front/aux synchronizer (Luminar production path). The front callback - // only validates + enqueues; this worker owns release order and runs the - // scan pipeline, so waiting for a future point-aligned aux sweep never - // blocks the subscription callback (and therefore never turns into QoS - // front-cloud drops). Non-Luminar sensors keep the legacy synchronous path. + // Async Luminar front worker. The subscription callback only validates and + // enqueues; this worker owns processing order. This is required even for the + // production front-only localizer: a single slow GICP iteration must not + // block the DDS callback long enough for keep-last history to discard later + // 10 Hz clouds silently. With aux LiDARs enabled, the same worker also owns + // the point-time synchronizer wait. Non-Luminar sensors keep the legacy + // synchronous path. // Started LAST, after every throwing constructor step: if the constructor // throws after a thread starts, the destructor never runs and the thread // would touch destroyed members. Callbacks cannot fire before spin, so no // front cloud can arrive between subscription creation and this point. - if (this->concat_enabled_ && !this->aux_lidars_.empty() && - this->sensor == dlio::SensorType::LUMINAR) { + if (!this->imu_only_mode_ && this->sensor == dlio::SensorType::LUMINAR) { this->sync_active_ = true; this->sync_worker_ = std::thread(&gicp_plusplus::LocalizationNode::syncWorkerLoop, this); RCLCPP_INFO(this->get_logger(), - "front sync: async point-time synchronizer active " - "(point gate %.3fs, deadline %.3fs, primary queue %zu)", + "front sync: async Luminar worker active " + "(aux=%zu, point gate %.3fs, deadline %.3fs, primary queue %zu)", + this->aux_lidars_.size(), this->concat_luminar_point_threshold_, this->concat_future_aux_wait_s_, this->concat_primary_queue_size_); @@ -1668,6 +1740,12 @@ void gicp_plusplus::LocalizationNode::getParams() { this->declare_parameter("localization/gt_odom/enable", true); // [P3 FIX 2026-07-10] yaml-aligned this->declare_parameter("localization/gt_odom/buffer_size", 200); this->declare_parameter("localization/gt_odom/max_dt", 0.1); + // Runtime GNSS/INS guard, not a scoring shortcut: an RTK-quality candidate + // outside this radius is rejected before it can inject a false correction + // into the geometric observer. Disabled in the C++ fallback; the Laguna + // production yaml enables it explicitly alongside GT recovery. + this->declare_parameter( + "localization/gt_odom/max_candidate_position_error_m", 0.0); // [P2 FIX 2026-07-14] Bracket-width bound for GT interpolation (see // getGtPoseAt / getGtFiniteDiffVelWorld). Independent of max_dt, which only // bounds the nearer endpoint. Default 0.5 s: at nominal 10 Hz GT this is 5× @@ -1678,6 +1756,9 @@ void gicp_plusplus::LocalizationNode::getParams() { this->get_parameter("localization/gt_odom/enable", gt_enable); this->get_parameter("localization/gt_odom/buffer_size", gt_buf); this->get_parameter("localization/gt_odom/max_dt", gt_max_dt); + this->get_parameter( + "localization/gt_odom/max_candidate_position_error_m", + this->gt_max_candidate_pos_error_m_); this->get_parameter("localization/gt_odom/interp_max_gap", gt_interp_gap); this->gt_odom_enabled_ = gt_enable; this->gt_odom_buffer_size_ = static_cast(std::max(gt_buf, 1)); @@ -1689,6 +1770,15 @@ void gicp_plusplus::LocalizationNode::getParams() { this->gt_odom_max_dt_); this->gt_odom_max_dt_ = 0.15; } + if (!std::isfinite(this->gt_max_candidate_pos_error_m_) || + this->gt_max_candidate_pos_error_m_ < 0.0) { + RCLCPP_WARN( + this->get_logger(), + "localization/gt_odom/max_candidate_position_error_m=%.3f invalid; " + "disabling the candidate sanity gate", + this->gt_max_candidate_pos_error_m_); + this->gt_max_candidate_pos_error_m_ = 0.0; + } this->gt_interp_max_gap_ = gt_interp_gap; if (!std::isfinite(this->gt_interp_max_gap_) || this->gt_interp_max_gap_ <= 0.0) { RCLCPP_WARN(this->get_logger(), "localization/gt_odom/interp_max_gap=%.3f invalid; using 0.5", @@ -1958,6 +2048,7 @@ void gicp_plusplus::LocalizationNode::getParams() { // the primary sensor frame via TF (URDF), and per-point timestamps are rebased // by the inter-header dt so the merged sweep shares one clock. this->declare_parameter("localization/lidar_concat/enabled", false); + this->declare_parameter("localization/lidar_concat/reliable_qos", false); this->declare_parameter>("localization/lidar_concat/aux_topics", std::vector{}); this->declare_parameter>("localization/lidar_concat/aux_frames", std::vector{}); this->declare_parameter("localization/lidar_concat/time_threshold", 0.05); @@ -1975,6 +2066,12 @@ void gicp_plusplus::LocalizationNode::getParams() { // missing entries = 0. this->declare_parameter>("localization/lidar_concat/aux_time_offsets", std::vector{}); + // Mirror GLIM's explicit driver contract. FLOAT64 normally carries + // scan-relative seconds (Laguna); only reinterpret its raw bytes as uint64 + // epoch nanoseconds when this is true. UINT8[8] remains an absolute carrier + // regardless of this setting. + this->declare_parameter( + "localization/lidar_concat/float64_time_is_epoch_ns", false); // Luminar acceptance gate: absolute point-time endpoint-range error // max(|min-min|,|max-max|) <= this. Header time is only a tie-break; the // 0.1 s header threshold stays solely for non-Luminar fallback matching. @@ -2001,19 +2098,24 @@ void gicp_plusplus::LocalizationNode::getParams() { this->declare_parameter("localization/lidar_concat/max_consecutive_aux_merge_failures", 10); this->get_parameter("localization/lidar_concat/enabled", this->concat_enabled_); + this->get_parameter( + "localization/lidar_concat/reliable_qos", this->lidar_reliable_qos_); std::vector aux_topics_param, aux_frames_param; this->get_parameter("localization/lidar_concat/aux_topics", aux_topics_param); this->get_parameter("localization/lidar_concat/aux_frames", aux_frames_param); this->get_parameter("localization/lidar_concat/time_threshold", this->concat_time_threshold_); // [P3 FIX 2026-07-10] Negative threshold silently drops every aux merge. if (!std::isfinite(this->concat_time_threshold_) || this->concat_time_threshold_ < 0.0) { - RCLCPP_WARN(this->get_logger(), "localization/lidar_concat/time_threshold=%.3f invalid; using 0.1", + RCLCPP_WARN(this->get_logger(), "localization/lidar_concat/time_threshold=%.3f invalid; using 0.05", this->concat_time_threshold_); - this->concat_time_threshold_ = 0.1; + this->concat_time_threshold_ = 0.05; } int concat_buffer_size_int = 20; this->get_parameter("localization/lidar_concat/buffer_size", concat_buffer_size_int); this->get_parameter("localization/lidar_concat/aux_time_offsets", this->concat_aux_time_offsets_); + this->get_parameter( + "localization/lidar_concat/float64_time_is_epoch_ns", + this->concat_float64_time_is_epoch_ns_); // Fail LOUD on invalid offsets (GLIM config-loader policy): a NaN/inf or // extreme value would flow into point-range matching and the int64 ns // conversion in shiftCloudTimestamps (UB / corrupted absolute timestamps). @@ -2099,8 +2201,13 @@ void gicp_plusplus::LocalizationNode::getParams() { this->aux_lidars_.push_back(std::move(aux)); } RCLCPP_INFO(this->get_logger(), - "lidar_concat enabled: %zu aux lidars, time_threshold=%.3fs, buffer_size=%zu", - this->aux_lidars_.size(), this->concat_time_threshold_, this->concat_buffer_size_); + "lidar_concat enabled: %zu aux lidars, time_threshold=%.3fs, " + "buffer_size=%zu, FLOAT64 time=%s", + this->aux_lidars_.size(), this->concat_time_threshold_, + this->concat_buffer_size_, + this->concat_float64_time_is_epoch_ns_ + ? "raw uint64 epoch-ns (explicit opt-in)" + : "scan-relative seconds"); if (this->concat_aux_time_offsets_.size() < this->aux_lidars_.size()) { RCLCPP_WARN(this->get_logger(), "lidar_concat: aux_time_offsets has %zu/%zu entries; missing entries default to 0.0. " @@ -2235,6 +2342,7 @@ void gicp_plusplus::LocalizationNode::getParams() { this->declare_parameter("odom/geo/observer_dt_max", 0.15); this->declare_parameter("odom/geo/max_pos_correction", 0.0); this->declare_parameter("odom/geo/max_vel_correction", 0.0); + this->declare_parameter("odom/geo/max_state_speed", 0.0); // P1 yaw-safety fix #3: per-update ORIENTATION clamps (position/velocity // already had them). Yaw clamp ON by default — the failure mode it bounds // (one bad accepted scan yanking heading tens of degrees) is exactly the @@ -2244,12 +2352,19 @@ void gicp_plusplus::LocalizationNode::getParams() { this->get_parameter("odom/geo/observer_dt_max", this->geo_observer_dt_max_); this->get_parameter("odom/geo/max_pos_correction", this->geo_max_pos_correction_); this->get_parameter("odom/geo/max_vel_correction", this->geo_max_vel_correction_); + this->get_parameter("odom/geo/max_state_speed", this->geo_max_state_speed_); this->get_parameter("odom/geo/max_yaw_correction_deg", this->geo_max_yaw_correction_deg_); this->get_parameter("odom/geo/max_rot_correction_deg", this->geo_max_rot_correction_deg_); if (this->geo_observer_dt_max_ <= 0.0) { this->geo_observer_dt_max_ = 0.15; // guard against a non-positive cap disabling all corrections } - + if (!std::isfinite(this->geo_max_state_speed_) || + this->geo_max_state_speed_ < 0.0) { + RCLCPP_WARN(this->get_logger(), + "odom/geo/max_state_speed=%.3f invalid; disabling speed clamp", + this->geo_max_state_speed_); + this->geo_max_state_speed_ = 0.0; + } // Time/speed-based dead-reckoning covariance growth (P3). this->declare_parameter("odom/geo/dr_cov_time_rate", 0.5); this->declare_parameter("odom/geo/dr_cov_dist_frac", 0.05); @@ -2332,6 +2447,18 @@ void gicp_plusplus::LocalizationNode::getParams() { "GT recovery: %s (min consecutive failures=%d)", this->gt_recovery_enabled_ ? "ENABLED" : "DISABLED", this->gt_recovery_min_consecutive_failures_); + RCLCPP_INFO( + this->get_logger(), + "RTK candidate sanity: %s (max position error=%.2fm; quality-gated, no " + "per-frame position fusion)", + this->gt_max_candidate_pos_error_m_ > 0.0 ? "ENABLED" : "DISABLED", + this->gt_max_candidate_pos_error_m_); + RCLCPP_INFO( + this->get_logger(), + "Observer stability bounds: dt<=%.3fs pos_step<=%.2fm vel_step<=%.2fm/s " + "state_speed<=%.1fm/s", + this->geo_observer_dt_max_, this->geo_max_pos_correction_, + this->geo_max_vel_correction_, this->geo_max_state_speed_); RCLCPP_INFO(this->get_logger(), "Debug: publish=%s jump_log=%s thresholds=[%.2fm, %.1fdeg]", this->debug_pub_enabled_ ? "ENABLED" : "DISABLED", this->debug_jump_log_enabled_ ? "ENABLED" : "DISABLED", @@ -2491,30 +2618,30 @@ bool gicp_plusplus::LocalizationNode::loadMap() { // Downsample the GICP TARGET map (in place) before it becomes the kd-tree. // A dense map (e.g. a ~49M-point GLIM export) otherwise builds a multi-GB - // kd-tree that exhausts RAM/swap and stalls registration for seconds. Voxel - // downsampling to ~0.3 m cuts the point count (and kd-tree memory) by ~10x - // with negligible accuracy impact at the matching 0.3 m scan voxel - // (dlio/preprocessing/voxelFilter/res). The dense cloud is - // released as soon as the filter swaps in the downsampled result. + // kd-tree that exhausts RAM/swap and stalls registration for seconds. Do + // not use pcl::VoxelGrid here: its dense bounding-box cell-count guard uses + // a 32-bit product and silently leaves sparse, large-extent Laguna maps + // unchanged ("integer indices would overflow"). small_gicp's sparse 64-bit + // voxel keys depend on occupied points instead of the bounding-box volume. + // Downsampling to ~0.3 m cuts the point count (and kd-tree memory) with + // negligible accuracy impact at the matching 0.3 m scan voxel. The dense + // cloud is released as soon as the filter swaps in the downsampled result. if (this->map_voxel_size_ > 0.0) { const size_t before = this->map_cloud->points.size(); - auto map_ds = std::make_shared>(); - pcl::VoxelGrid vg; - vg.setLeafSize(static_cast(this->map_voxel_size_), - static_cast(this->map_voxel_size_), - static_cast(this->map_voxel_size_)); - vg.setInputCloud(this->map_cloud); - vg.filter(*map_ds); + auto map_ds = small_gicp::voxelgrid_sampling_tbb( + *this->map_cloud, this->map_voxel_size_); if (map_ds->points.empty()) { - RCLCPP_WARN(this->get_logger(), - "map_voxel_size=%.3f produced an empty map; keeping the full-resolution map", - this->map_voxel_size_); - } else { - this->map_cloud = map_ds; // releases the dense cloud - RCLCPP_INFO(this->get_logger(), - "Downsampled GICP target map: %lu -> %lu points (voxel=%.3f m)", - before, this->map_cloud->points.size(), this->map_voxel_size_); + RCLCPP_ERROR(this->get_logger(), + "sparse map voxelization at %.3f m produced an empty map; " + "refusing to build a full-resolution target", + this->map_voxel_size_); + return false; } + this->map_cloud = map_ds; // releases the dense cloud + RCLCPP_INFO(this->get_logger(), + "Downsampled GICP target map with sparse 64-bit voxel keys: " + "%lu -> %lu points (voxel=%.3f m)", + before, this->map_cloud->points.size(), this->map_voxel_size_); } // Downsample map for visualization if needed @@ -2924,15 +3051,15 @@ void gicp_plusplus::LocalizationNode::callbackPointCloud( } if (this->sync_active_) { - // Luminar production path: validate + enqueue only. The synchronizer - // worker owns release order and runs the pipeline; this callback must - // never block on aux state (a 100-150 ms wait exceeds the 20 Hz front - // period and turns into QoS front drops — the Result-33 regression). + // Luminar production path: validate + enqueue only. The worker owns + // release order and runs the pipeline; this callback must never block on + // either aux state or a long GICP iteration, because blocking can exhaust + // DDS keep-last history and silently lose front clouds. this->enqueuePrimary(pc_in); return; } - // Legacy synchronous path (concat disabled or non-Luminar sensor). + // Legacy synchronous path (non-Luminar sensor). // [P3 FIX 2026-07-14] Catch a pipeline exception (e.g. strict-merge abort) // HERE, on whatever executor thread ran this callback. The try/catch around // executor.spin() in main() only covers the single spin-calling thread; under @@ -2971,11 +3098,14 @@ void gicp_plusplus::LocalizationNode::enqueuePrimary( // Decode ONCE. An invalid range (unsupported time field) means point-time // matching is impossible: release immediately and let mergeAuxClouds record // the per-aux outcome — the front cloud itself is still processed. - pending.range = luminarTimestampRangeFromCloud(*pc); + pending.range = luminarTimestampRangeFromCloud( + *pc, this->concat_float64_time_is_epoch_ns_); + pending.relative_float64_time = luminarCloudUsesRelativeFloat64( + *pc, this->concat_float64_time_is_epoch_ns_); const auto now = std::chrono::steady_clock::now(); pending.enqueued = now; pending.deadline = - pending.range.valid + (pending.range.valid || pending.relative_float64_time) ? now + std::chrono::duration( static_cast(this->concat_future_aux_wait_s_ * 1e9)) : now; @@ -3085,6 +3215,36 @@ void gicp_plusplus::LocalizationNode::syncWorkerLoop() { break; } } + } else if (front.relative_float64_time) { + // Laguna's decoder publishes FLOAT64 seconds-since-sweep-start. There + // is no absolute point range to compare, so mirror GLIM's safe + // header-fallback watermark: wait until every aux stream has reached + // this primary header before selecting the nearest header. Releasing + // immediately would always choose the latest past side sweep. + const double primary_header = + rclcpp::Time(front.msg->header.stamp).seconds(); + for (size_t i = 0; i < this->aux_lidars_.size(); ++i) { + auto& aux = *this->aux_lidars_[i]; + const double clock_off = + (i < this->concat_aux_time_offsets_.size()) + ? this->concat_aux_time_offsets_[i] + : 0.0; + double newest_header = -std::numeric_limits::infinity(); + { + std::lock_guard alk(aux.mtx); + for (const auto& buffered : aux.buffer) { + newest_header = std::max( + newest_header, + rclcpp::Time(buffered.msg->header.stamp).seconds() + + clock_off); + } + } + if (newest_header < primary_header) { + all_matched = false; + ready = false; + break; + } + } } // [P2 FIX 2026-07-14] Copy the deadline before waiting. wait_until takes @@ -3102,7 +3262,7 @@ void gicp_plusplus::LocalizationNode::syncWorkerLoop() { // Decide the release reason before popping. int reason; - if (!front.range.valid) { + if (!front.range.valid && !front.relative_float64_time) { // [P3 FIX 2026-07-14] Primary had no decodable absolute point time: the // aux-matching block above was skipped entirely, so "all_matched" is // vacuously true. Report it distinctly instead of as a healthy match. @@ -3341,6 +3501,11 @@ void gicp_plusplus::LocalizationNode::processScan( uint8_t time_datatype = 0; int time_count = 0; const bool has_time_field = findTimeField(*pc, time_off, time_datatype, time_count); + this->luminar_scan_time_is_epoch_ns_ = + this->sensor == dlio::SensorType::LUMINAR && has_time_field && + luminarUsesRawEpochCarrier( + time_datatype, time_count, + this->concat_float64_time_is_epoch_ns_); // One-shot timestamp-field diagnostic. Fires exactly once across the whole // node lifetime (std::call_once) and dumps every PointField + the first few @@ -3403,7 +3568,9 @@ void gicp_plusplus::LocalizationNode::processScan( dst.intensity = read_intensity(src); clearPointTimeUnion(dst); if (has_time_field) { - copyPointTimeFromCloud(src, time_off, time_datatype, time_count, point_step, this->sensor, dst); + copyPointTimeFromCloud( + src, time_off, time_datatype, time_count, point_step, this->sensor, + this->concat_float64_time_is_epoch_ns_, dst); } } }; @@ -3456,7 +3623,9 @@ void gicp_plusplus::LocalizationNode::processScan( if (this->sensor == dlio::SensorType::LUMINAR && has_time_field && this->verbose_ && !raw_scan->points.empty()) { - logLuminarTimestampStats(raw_scan->points.size(), *raw_scan, 0); + logLuminarTimestampStats( + raw_scan->points.size(), *raw_scan, 0, + this->luminar_scan_time_is_epoch_ns_); } // Store as original scan for deskewing @@ -3529,7 +3698,8 @@ void gicp_plusplus::LocalizationNode::callbackAuxPointCloud( if (this->sensor == dlio::SensorType::LUMINAR) { // Decode once here (Reentrant aux group, cheap ~ms scan) so matching and // the synchronizer readiness test never re-read cloud bytes. - buffered.luminar_range = luminarTimestampRangeFromCloud(*buffered.msg); + buffered.luminar_range = luminarTimestampRangeFromCloud( + *buffered.msg, this->concat_float64_time_is_epoch_ns_); } auto& aux = *this->aux_lidars_[aux_index]; { @@ -3716,7 +3886,8 @@ gicp_plusplus::LocalizationNode::mergeAuxClouds( // (copyPointTimeFromCloud) on the accepted absolute encodings (UINT8[8] / // FLOAT64); it internally guards short/truncated buffers, and this capture // runs BEFORE the tight-cloud guard further down. - primary_luminar_range = luminarTimestampRangeFromCloud(*primary); + primary_luminar_range = luminarTimestampRangeFromCloud( + *primary, this->concat_float64_time_is_epoch_ns_); if (primary_luminar_range.valid) { this->luminar_primary_min_ts_ns_ = primary_luminar_range.min_ns; this->luminar_primary_min_ts_valid_ = true; @@ -3772,12 +3943,15 @@ gicp_plusplus::LocalizationNode::mergeAuxClouds( // wrong-sweep / 149 ms-span failure mode. Release the front alone and // explicitly omit every aux; the header fallback below exists only for // non-Luminar sensors. - if (this->sensor == dlio::SensorType::LUMINAR && !primary_luminar_range.valid) { + const bool relative_float64_time = luminarCloudUsesRelativeFloat64( + *primary, this->concat_float64_time_is_epoch_ns_); + if (this->sensor == dlio::SensorType::LUMINAR && + !primary_luminar_range.valid && !relative_float64_time) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, - "lidar_concat: primary cloud has no usable absolute point-time range " - "(unsupported_point_time); omitting '%s' and merging front-only — " - "header-nearest matching is not a safe Luminar fallback", + "lidar_concat: primary cloud has neither a usable absolute point-time " + "range nor the supported relative FLOAT64 time contract " + "(unsupported_point_time); omitting '%s' and merging front-only", aux.topic.c_str()); continue; } @@ -3927,7 +4101,11 @@ gicp_plusplus::LocalizationNode::mergeAuxClouds( uint8_t time_dt_type; int time_count; const bool has_time_field = findTimeField(*match, time_off, time_dt_type, time_count); - const bool luminar_u64 = (this->sensor == dlio::SensorType::LUMINAR); + const bool luminar_raw_epoch = + this->sensor == dlio::SensorType::LUMINAR && + luminarUsesRawEpochCarrier( + time_dt_type, time_count, + this->concat_float64_time_is_epoch_ns_); // [REVIEW FIX 2026-07-08 P3] For Luminar, "a time field exists" is not // "the time field is usable": the decoder accepts ONLY the 8-byte // absolute carriers (UINT8[8] raw uint64 epoch ns, or the same bits @@ -3937,7 +4115,7 @@ gicp_plusplus::LocalizationNode::mergeAuxClouds( // unsupported Luminar schema like a missing time field here so the aux is // DROPPED under deskew instead. const bool usable_time_field = has_time_field && - (!luminar_u64 || + (this->sensor != dlio::SensorType::LUMINAR || time_dt_type == sensor_msgs::msg::PointField::FLOAT64 || (time_dt_type == sensor_msgs::msg::PointField::UINT8 && time_count == 8)); if (usable_time_field) { @@ -3945,7 +4123,7 @@ gicp_plusplus::LocalizationNode::mergeAuxClouds( // absolute carrier ignores dt and applies only the measured residual // clock offset — header acquisition phase never touches point times. const double dt = rclcpp::Time(match->header.stamp).seconds() + aux_clock_off - t_primary; - shiftCloudTimestamps(appended, aux_pts, point_step, time_off, time_dt_type, time_count, dt, luminar_u64, + shiftCloudTimestamps(appended, aux_pts, point_step, time_off, time_dt_type, time_count, dt, luminar_raw_epoch, aux_clock_off); } else if (this->deskew_) { // Without per-point timestamps the aux rays would deskew against the @@ -3955,7 +4133,8 @@ gicp_plusplus::LocalizationNode::mergeAuxClouds( if (has_time_field) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "lidar_concat: skipping '%s' — unsupported Luminar time schema " - "(datatype=%u count=%d; need UINT8[8] or FLOAT64 epoch-ns)", + "(datatype=%u count=%d; need UINT8[8] epoch-ns or " + "FLOAT64 relative seconds/explicit epoch-ns)", aux.topic.c_str(), static_cast(time_dt_type), time_count); } else { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, @@ -4268,7 +4447,8 @@ void gicp_plusplus::LocalizationNode::deskewPointcloud() { point_time_cmp = [](const PointType& p1, const PointType& p2) { return p1.timestamp < p2.timestamp; }; extract_point_time_from_point = [](const PointType& pt) { return pt.timestamp * 1e-9; }; deskew_time_ready = true; - } else if (this->sensor == dlio::SensorType::LUMINAR) { + } else if (this->sensor == dlio::SensorType::LUMINAR && + this->luminar_scan_time_is_epoch_ns_) { // Per-point value is absolute PTP epoch ns (driver reconstruction of the // packet-header 48-bit seconds + per-ray 32-bit sub-second nanoseconds; // see Luminar Iris Data Output Specification v1.3.0 §2.1 and §2.2/§2.6.3). @@ -4314,6 +4494,18 @@ void gicp_plusplus::LocalizationNode::deskewPointcloud() { return sweep_ref_time + static_cast(static_cast(ts) - static_cast(min_ts_captured)) * 1e-9; }; deskew_time_ready = true; + } else if (this->sensor == dlio::SensorType::LUMINAR) { + // Laguna's decoder publishes FLOAT64 seconds since the start of each + // sweep. mergeAuxClouds() rebases auxiliary values by + // (T_aux_header - T_primary_header), so every merged point is already + // expressed relative to the primary header. + point_time_cmp = [](const PointType& p1, const PointType& p2) { + return p1.timestamp < p2.timestamp; + }; + extract_point_time_from_point = [&sweep_ref_time](const PointType& pt) { + return sweep_ref_time + pt.timestamp; + }; + deskew_time_ready = true; } if (!deskew_time_ready) { @@ -4349,7 +4541,9 @@ void gicp_plusplus::LocalizationNode::deskewPointcloud() { unique_time_indices.push_back(deskewed_scan_->points.size()); if (this->sensor == dlio::SensorType::LUMINAR && this->verbose_ && !deskewed_scan_->points.empty()) { - logLuminarTimestampStats(deskewed_scan_->points.size(), *deskewed_scan_, timestamps.size()); + logLuminarTimestampStats( + deskewed_scan_->points.size(), *deskewed_scan_, timestamps.size(), + this->luminar_scan_time_is_epoch_ns_); } if (timestamps.empty()) { @@ -4386,6 +4580,12 @@ void gicp_plusplus::LocalizationNode::deskewPointcloud() { if (this->prev_scan_stamp == 0.0) { this->prev_scan_stamp = this->scan_stamp.seconds(); this->T_prior = this->basePoseMatrix(); // [REVIEW FIX 2026-07-08] basePose (INS-prior-corrected), not current_pose + // Although the seed pose has not been IMU-advanced on this first frame, + // the GICP candidate produced from the placed cloud is a measurement at + // this scan's median point time. Label the candidate accordingly so an + // accepted first scan advances base_pose_stamp_ instead of pinning every + // subsequent integration request to the pre-replay GT seed timestamp. + this->t_prior_stamp_ = timestamps[median_pt_index]; pcl::transformPointCloud(*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T); this->current_scan = deskewed_scan_; this->scan_in_world_frame_ = true; @@ -4415,6 +4615,11 @@ void gicp_plusplus::LocalizationNode::deskewPointcloud() { "Waiting for sufficient IMU history (oldest: %.3f, need: %.3f). Skipping deskewing.", oldest_imu_time, this->prev_scan_stamp); this->T_prior = this->basePoseMatrix(); // [REVIEW FIX 2026-07-08] basePose (INS-prior-corrected), not current_pose + // As above, a successful registration against this scan is a + // median-time measurement even though its initial guess was not + // propagated. Advancing this stamp lets the next frame use the newly + // accepted pose as its honest IMU integration seed. + this->t_prior_stamp_ = timestamps[median_pt_index]; pcl::transformPointCloud(*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T); this->current_scan = deskewed_scan_; this->scan_in_world_frame_ = true; @@ -5038,9 +5243,11 @@ void gicp_plusplus::LocalizationNode::performLocalization() { final_jump_rot_deg <= this->gicp_nonconv_ok_max_rot_deg_); const bool effectively_converged = converged || nonconv_fallback_ok; - // Ground-truth divergence cross-check (optional). Compares the scan's accepted-or-candidate - // pose to a time-matched ground-truth odom sample. Only computes; does NOT influence - // accept/reject decisions — purely a diagnostic. + // Atlas divergence cross-check (optional). Compares the scan's + // accepted-or-candidate pose to a time-matched odom sample. It is purely a + // diagnostic when max_candidate_position_error_m=0; otherwise the + // RTK-quality position error also feeds the broad wrong-basin safety gate + // below (never a per-frame position blend). double gt_pos_err = -1.0; double gt_rot_err_deg = -1.0; double gt_dt = 0.0; @@ -5275,8 +5482,19 @@ void gicp_plusplus::LocalizationNode::performLocalization() { bool gicp_rejected_yaw = false; bool gicp_rejected_hessian = false; bool gicp_rejected_support = false; + bool gicp_rejected_gt_sanity = false; if (effectively_converged && candidate_pose_valid) { - if (!analysis_hessian.allFinite()) { + if (this->gt_max_candidate_pos_error_m_ > 0.0 && + gt_pos_err >= 0.0 && + gt_pos_err > this->gt_max_candidate_pos_error_m_) { + // Atlas is already an on-car input for initialization, heading and + // recovery. Use its RTK-quality position only as a broad wrong-basin + // safety envelope: healthy GICP remains untouched inside the radius, + // while a parallel-wall/ghost match cannot be fed into the observer for + // tens of seconds. gt_pos_err is available only after the covariance + // quality gate and a time-bounded interpolation succeeded. + gicp_rejected_gt_sanity = true; + } else if (!analysis_hessian.allFinite()) { // [REVIEW FIX 2026-07-08 P3] Non-finite Hessian: hessianConditionProxy // returns +inf, but every Hessian gate below requires // std::isfinite(hessian_condition) — so these scans previously skipped @@ -5354,13 +5572,20 @@ void gicp_plusplus::LocalizationNode::performLocalization() { const bool gicp_accepted = effectively_converged && candidate_pose_valid && !gicp_rejected_fitness && !gicp_rejected_fitness_ratio && !gicp_rejected_hessian && !gicp_rejected_jump && - !gicp_rejected_yaw && !gicp_rejected_support; + !gicp_rejected_yaw && !gicp_rejected_support && + !gicp_rejected_gt_sanity; const bool gicp_partial = gicp_accepted && degen.valid && degen.modified; if (!candidate_pose_valid) { RCLCPP_WARN(this->get_logger(), "%s", build_scan_debug_log("invalid_solution").c_str()); } else if (!effectively_converged) { RCLCPP_WARN(this->get_logger(), "%s", build_scan_debug_log("failed_to_converge").c_str()); + } else if (gicp_rejected_gt_sanity) { + RCLCPP_WARN( + this->get_logger(), + "GICP REJECTED (RTK candidate sanity: position error %.3fm > %.3fm): %s", + gt_pos_err, this->gt_max_candidate_pos_error_m_, + build_scan_debug_log("rejected_gt_sanity").c_str()); } else if (gicp_rejected_fitness) { RCLCPP_WARN(this->get_logger(), "GICP REJECTED (fitness=%.4f > threshold=%.4f): %s", @@ -5494,7 +5719,6 @@ void gicp_plusplus::LocalizationNode::performLocalization() { std::lock_guard geo_lock(this->geo.mtx); this->prev_vel = this->geo.prev_vel; } - if (this->debug_jump_log_enabled_ && gicp_valid && this->last_gicp_valid_) { if (large_jump) { const Eigen::Vector3f t_prior = this->T_prior.block<3, 1>(0, 3); @@ -5549,6 +5773,7 @@ void gicp_plusplus::LocalizationNode::performLocalization() { ++this->consecutive_failures_; const char* reason = !candidate_pose_valid ? "invalid solution" : !effectively_converged ? "failed to converge" + : gicp_rejected_gt_sanity ? "RTK candidate sanity rejected (wrong basin)" : gicp_rejected_support ? "insufficient correspondence support" : gicp_rejected_fitness ? "fitness rejected" : gicp_rejected_yaw ? "yaw-innovation rejected (impossible heading)" @@ -5562,22 +5787,24 @@ void gicp_plusplus::LocalizationNode::performLocalization() { q.normalize(); // [P2 FIX 2026-07-09] Seed writes under the owner lock (order: // pose -> seed -> geo, consistent with the accept path and deskew). - std::lock_guard seed_lock(this->seed_mtx_); - this->basePose.p = new_p; - this->basePose.q = q; - // [REVIEW FIX 2026-07-08] T_prior is also a median-point-time pose. - this->base_pose_stamp_ = this->t_prior_stamp_; { - // P2#1 (stale-velocity bug): seed the next scan's IMU integration from - // the CURRENT IMU-propagated velocity, not geo.prev_vel. geo.prev_vel - // is only refreshed by updateState() (accepted scans) or a GT snap, so - // during an N-frame rejection streak it stayed frozen at the last - // accepted scan's velocity while the vehicle's velocity vector rotated - // through the turn — every per-scan prior then extrapolated straight - // ("corner cutting", run-12 webm). state.v.lin.w is maintained at IMU - // rate by propagateState() and is the correct dead-reckoning velocity. - std::lock_guard geo_lock(this->geo.mtx); - this->prev_vel = this->state.v.lin.w; + std::lock_guard seed_lock(this->seed_mtx_); + this->basePose.p = new_p; + this->basePose.q = q; + // [REVIEW FIX 2026-07-08] T_prior is also a median-point-time pose. + this->base_pose_stamp_ = this->t_prior_stamp_; + { + // P2#1 (stale-velocity bug): seed the next scan's IMU integration from + // the CURRENT IMU-propagated velocity, not geo.prev_vel. geo.prev_vel + // is only refreshed by updateState() (accepted scans) or a GT snap, so + // during an N-frame rejection streak it stayed frozen at the last + // accepted scan's velocity while the vehicle's velocity vector rotated + // through the turn — every per-scan prior then extrapolated straight + // ("corner cutting", run-12 webm). state.v.lin.w is maintained at IMU + // rate by propagateState() and is the correct dead-reckoning velocity. + std::lock_guard geo_lock(this->geo.mtx); + this->prev_vel = this->state.v.lin.w; + } } RCLCPP_WARN(this->get_logger(), "Localization: ⚠ GICP %s — holding IMU dead-reckoning pose [%.2f, %.2f, %.2f] | fitness=%.4f time=%.2fms", @@ -6927,38 +7154,77 @@ bool gicp_plusplus::LocalizationNode::imuMeasFromTimeRange( std::lock_guard lock(this->mtx_imu); - if (this->imu_buffer.empty() || this->imu_buffer.front().stamp < end_time) { - // Not enough IMU data yet + out.clear(); + const bool empty = this->imu_buffer.empty(); + const bool invalid_range = + !std::isfinite(start_time) || !std::isfinite(end_time) || + start_time > end_time; + const bool missing_newer = + !empty && this->imu_buffer.front().stamp < end_time; + const double missing_older_s = + empty ? std::numeric_limits::infinity() + : this->imu_buffer.back().stamp - start_time; + const bool missing_older = + !empty && missing_older_s > 0.002; + if (empty || invalid_range || missing_newer || missing_older) { + // Need a monotone window with one real IMU sample on or before start_time + // and one on or after end_time. imu_buffer is newest -> oldest. + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "IMU range unavailable: request=[%.6f,%.6f] buffer=[oldest=%.6f," + "newest=%.6f,size=%zu] empty=%d invalid=%d missing_older=%d " + "missing_newer=%d", + start_time, end_time, + empty ? -1.0 : this->imu_buffer.back().stamp, + empty ? -1.0 : this->imu_buffer.front().stamp, + this->imu_buffer.size(), empty, invalid_range, missing_older, + missing_newer); return false; } - auto imu_it = this->imu_buffer.begin(); - - auto last_imu_it = imu_it; - imu_it++; - while (imu_it != this->imu_buffer.end() && imu_it->stamp >= end_time) { - last_imu_it = imu_it; - imu_it++; - } - - while (imu_it != this->imu_buffer.end() && imu_it->stamp >= start_time) { - imu_it++; - } + // Walk oldest -> newest while the lock is held. Keep only the newest sample + // at/before start_time, then every sample through the first one at/after + // end_time. The former reverse_iterator-range construction mixed a forward + // iterator boundary with reverse_iterator base semantics and could produce + // an empty slice even though the circular buffer visibly bracketed the + // requested interval. With real FLOAT64 Luminar point times that disabled + // IMU prediction/deskew on every frame and eventually caused high-speed + // GICP loss. + bool have_start_bracket = false; + for (auto it = this->imu_buffer.rbegin(); it != this->imu_buffer.rend(); ++it) { + if (!have_start_bracket) { + if (it->stamp <= start_time || out.empty()) { + // There can be many older samples. Retain only the nearest one so the + // returned slice starts at the interpolation bracket, not at the + // circular buffer's oldest entry. The out.empty() case admits at most + // 2 ms of start-side extrapolation (guarded above), covering the + // sub-millisecond seed-vs-first-IMU phase seen at replay startup. + out.clear(); + out.push_back(*it); + } else if (!out.empty()) { + have_start_bracket = true; + out.push_back(*it); + if (it->stamp >= end_time) { + return out.size() >= 2; + } + } + continue; + } - if (imu_it == this->imu_buffer.end()) { - // not enough IMU measurements - return false; + out.push_back(*it); + if (it->stamp >= end_time) { + return out.size() >= 2; + } } - imu_it++; - // [REVIEW FIX 2026-07-08 P1] Copy the slice out (forward time order: from - // the sample just before start_time through the sample at/after end_time) - // while STILL holding mtx_imu. Iterators into the circular buffer must not - // survive past the lock: a concurrent IMU push_front invalidates them. - out.assign(boost::circular_buffer::reverse_iterator(imu_it), - boost::circular_buffer::reverse_iterator(last_imu_it)); - - return true; + out.clear(); + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "IMU range traversal failed despite bracket guard: request=[%.6f,%.6f] " + "buffer=[oldest=%.6f,newest=%.6f,size=%zu]", + start_time, end_time, this->imu_buffer.back().stamp, + this->imu_buffer.front().stamp, this->imu_buffer.size()); + return false; } std::vector> @@ -7322,6 +7588,18 @@ void gicp_plusplus::LocalizationNode::propagateState(const ImuMeas& imu_local) { // Ground vehicle Z-velocity damping (same as in updateState) new_v_lin_w[2] *= (1.0f - dt * static_cast(this->geo_Kz_damping_)); + if (this->geo_max_state_speed_ > 0.0) { + const float speed = new_v_lin_w.norm(); + if (std::isfinite(speed) && + speed > static_cast(this->geo_max_state_speed_)) { + new_v_lin_w *= static_cast(this->geo_max_state_speed_) / speed; + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "propagateState: observer speed %.1fm/s exceeded physical cap %.1fm/s; " + "clamped (inspect GICP rejection/recovery)", + static_cast(speed), this->geo_max_state_speed_); + } + } // Orientation propagation omega.w() = 0; @@ -7771,6 +8049,19 @@ void gicp_plusplus::LocalizationNode::updateState() { // A ground vehicle's true Z-velocity is ~0; residual gravity miscompensation // causes vel_z to drift. Apply exponential decay each update. this->state.v.lin.w[2] *= (1.0f - dt_eff * this->geo_Kz_damping_); + if (this->geo_max_state_speed_ > 0.0) { + const float speed = this->state.v.lin.w.norm(); + if (std::isfinite(speed) && + speed > static_cast(this->geo_max_state_speed_)) { + this->state.v.lin.w *= + static_cast(this->geo_max_state_speed_) / speed; + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "updateState: observer speed %.1fm/s exceeded physical cap %.1fm/s; " + "clamped", + static_cast(speed), this->geo_max_state_speed_); + } + } // Orientation correction this->state.q.w() += dt_eff * this->geo_Kq_ * qcorr.w(); diff --git a/README.md b/README.md index 709fdab6..b82209a4 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,7 @@ ROS 2 perception stack for the AV-24 Cybertruck autonomous race car. Pairs a GPU | [`adapter/`](adapter/) | new in this repo | Point One Atlas normalization boundary. Converts raw Atlas WGS84 pose/IMU into the `/gps_p1/*` (and optional `/gnss*`) streams in a **local ENU** `map` frame consumed by GLIM/GICP. | | [`GLIM/`](GLIM/) | [`koide3/GLIM`](https://github.com/koide3/glim) (+ `glim_ext`, `glim_ros2`) | LiDAR-inertial SLAM. Builds a 3D map from IMU + multi-LiDAR + GNSS. | | [`gicp_localization/`](gicp_localization/) | Vendored from the `vectr-ucla` DLIO line (uses `nano_gicp`) | Current production/default GICP scan-to-map localizer against a PCD map produced by GLIM. | -| [`GICP_plusplus/`](GICP_plusplus/) | new in this repo (vendored `small_gicp`) | A/B localizer with the same ENU input contract and an asynchronous front/aux synchronizer. Run it instead of—not alongside—`gicp_localization`. | +| [`GICP_plusplus/`](GICP_plusplus/) | new in this repo (vendored `small_gicp`) | A/B localizer with the same ENU input contract and an asynchronous Luminar front worker/aux synchronizer. Run it instead of—not alongside—`gicp_localization`. | | [`dlio/`](dlio/) | new in this repo | Convenience metapackage that pulls the packages into a single colcon build. | `scripts/prep_bag.py` ties the adapter to offline mapping: by default it normalizes a raw bag/PCAP, copies raw Luminar topics through untouched, **and runs GLIM** into a dump directory. Pass `--skip-glim` only when you intend to run GLIM manually afterward. @@ -38,7 +38,16 @@ All sensor extrinsics are resolved at startup from [`av24.urdf`](av24.urdf) (off ### Coordinate frames — local ENU -The `map` frame is a **local ENU** tangent frame anchored at a fixed geodetic **datum** (the Putnam origin read from `race_metadata`'s TTL), matching race_common's convention. The [`adapter`](adapter/) package is the **single authority** that converts raw Atlas WGS84 fixes into that ENU frame and republishes `/gps_p1/*` (and optional `/gnss*`) already in ENU, so GLIM and GICP consume ENU directly. GICP is **frame-agnostic** — it localizes the scan against the PCD map and reports the pose in whatever frame the map is in; because the map is built in ENU and the seed is ENU, its output is ENU with no extra transform. +The `map` frame is a **local ENU** tangent frame anchored at the current +dataset's fixed geodetic **datum**, matching race_common's convention. The +datum must come from that dataset's map metadata (for example, Putnam reads its +own origin from `race_metadata`'s TTL); an origin from another venue must never +be reused. The [`adapter`](adapter/) package is the **single authority** that +converts raw Atlas WGS84 fixes into that ENU frame and republishes `/gps_p1/*` +(and optional `/gnss*`) already in ENU, so GLIM and GICP consume ENU directly. +GICP is **frame-agnostic** — it localizes the scan against the PCD map and +reports the pose in whatever frame the map is in; because the map is built in +ENU and the seed is ENU, its output is ENU with no extra transform. The one hard constraint is a **single shared datum**: the map, the seed (`/gps_p1/filtered_odom`), and GICP must all use the origin the adapter defines, or the frames silently disagree. @@ -89,7 +98,10 @@ In low-feature stretches the localizer first falls back to IMU dead-reckoning. I **The RTK quality gate is applied per-consumer, not globally, and snap recovery is intentionally exempt.** `callbackGtOdom()` buffers *every* Atlas sample regardless of FIXED/FLOAT/dead-reckoning state; the covariance gate (`gtSampleIsRtkFixed`) is then applied at each consumer: - **Bias calibration / seed** (`tryRtkCalibrationStep`) → **requires RTK-FIXED**. -- **GT divergence cross-check** (`gt_pos_err` diagnostic) → **requires RTK-FIXED**. +- **GT divergence cross-check / candidate sanity envelope** (`gt_pos_err`, + deployed `max_candidate_position_error_m: 5.0`) → **requires RTK-FIXED**. + Atlas position is not blended into healthy GICP poses inside the envelope; + the gate only rejects a repeated-geometry wrong basin before observer update. - **Snap recovery** (`maybeSnapPoseToGT`) → **accepts any-quality Atlas sample**. The rationale is that Atlas FusionEngine already runs a coupled GNSS+IMU INS with calibrated sensors, so during RTK loss its degraded pose is still the better truth source than the node's own software IMU dead-reckoning. This means recovery can snap toward an RTK-float/GPS-only fix when GICP has failed — a deliberate trade. It is enabled by default (`gt_recovery/enable: true`, `min_consecutive_failures: 5` — raised from 1 in the P2 turn-error fixes: per-frame snapping masked dead-reckoning quality in replay metrics); raise `min_consecutive_failures` further, or disable `gt_recovery` if you require the snap to be strictly RTK-gated. The joint low-feature-LiDAR and degraded-RTK case remains an operational watch condition. @@ -139,8 +151,14 @@ Neither phase applies to `gicp_localization` — that pipeline does RTK-driven I The cross-run replay campaign (run 3 ↔ run 5, July 2026) diagnosed and fixed a family of turn-localization errors. Use `gicp_localization/scripts/analyze_scan_debug_log.py` to score a replay from its `SCAN DEBUG` evidence. Headlines: -- **P1** — GICP binary accept/reject gates replaced with confidence-weighted gating: per-map rolling-median fitness ratios, full-6D degeneracy partial updates (solution remapping on the vehicle-re-centered hessian), and a turn-aware yaw-consistency veto. -- **P2** — state-continuity fixes on the rejected-scan path (stale-velocity bug), GT-snap twist continuity (adapter now populates `twist.angular` from the Atlas gyro), recovery threshold 1 → 5. +- **P1** — GICP binary accept/reject gates replaced with support/physics-aware + gating and full-6D degeneracy partial updates. The deployed Laguna default + follows perception-ws and disables map-density-dependent absolute and rolling + fitness rejection; the ratio machinery remains available for explicit A/B. +- **P2** — state-continuity fixes on the rejected-scan path (stale-velocity + bug), GT-snap twist continuity, RTK-quality candidate safety envelope, and + observer position/velocity/speed bounds that prevent a wrong basin from + producing an unphysical prediction; recovery threshold 1 → 5. - **P3** — delta-form observer correction (removes the 0.1–0.3 s stale-measurement yaw lag in turns) and a unified IMU bias path (bias applied once, at buffering). - **P4** — geometry densification: scan voxel 0.5 → 0.3 m, dense GLIM map profile (**active default**, see below), per-frame merge diagnostics in both stacks, concat buffer parity (200). - **P5** — dual-antenna heading priors in GLIM mapping hardened with a per-sample yaw-quality gate. @@ -187,7 +205,7 @@ Note: `scripts/prep_bag.py` deliberately does **not** rebase LiDAR — it copies | `localization/sensor_type` | Field encodings handled | Notes | |---|---|---| -| `luminar` | `UINT8[8]` (uint64 epoch ns; validated default — field `timestamp`, offset 0, point_step 56), `FLOAT64` (raw uint64 bits in a mislabelled FLOAT64 wrapper) | Iris PTP-synced output, reconstructed to full epoch ns by the driver (see the definitive account above). Only these two **8-byte absolute-epoch** carriers are accepted; `UINT32` is **intentionally rejected** — 32 bits cannot hold an absolute epoch (it wraps every ~4.29 s), so it would be a scan-relative counter the absolute path would misread. A `UINT32` Luminar therefore degrades to no per-point time (rigid transform) rather than corrupting deskew. | +| `luminar` | `UINT8[8]` (uint64 epoch ns), `FLOAT64` (scan-relative seconds by default; raw uint64 epoch-ns bits only with the explicit driver opt-in) | Laguna's decoder publishes ordinary FLOAT64 seconds-since-sweep-start. The localizer rebases auxiliary relative times onto the primary header before deskew. `UINT8[8]` remains an absolute PTP carrier. `UINT32` is intentionally rejected on the Luminar path because it cannot carry an absolute epoch and its unit/anchor would otherwise be ambiguous. | | `ouster` | `UINT32`, `FLOAT32`, `FLOAT64` (all scan-relative ns or s) | Standard Ouster ROS driver layouts. | | `velodyne` | `FLOAT32`, `UINT32` (scan-relative s or ns) | VLP-16/32 and similar. | | `hesai` | `FLOAT64`, `FLOAT32` (absolute or relative seconds) | Pandar / XT line. | @@ -239,8 +257,84 @@ If you ever switch sensors and the deskew looks wrong, use the one-shot diagnost python3 scripts/export_glim_dump_to_pcd.py /tmp/dump /path/to/track_map.pcd --voxel-size 0.1 ``` The exporter defaults to `--frame enu`: it applies `inverse(T_world_utm)` so the PCD is genuinely in the Atlas local-ENU frame, fails closed when the transform is missing, and writes a `*.manifest.yaml` recording the frame and transform (check it before shipping a map). It reads the datum from `/enu_origin.txt` automatically (written by the all-in-one `prep_bag.py` route); for a **hand-run GLIM dump** that file does not exist, so pass the datum explicitly: `--enu-origin ""` (the same origin the adapter used). + Do not deploy a dense union of every repeated lap. For a perception-ws-style + deployment map, export two or more representative laps independently as XYZ + at the final voxel size, then retain repeatable voxels inside the driven + corridor: + ```bash + python3 scripts/export_glim_dump_to_pcd.py /tmp/dump /tmp/lap1.pcd \ + --submap-range START1:END1 --voxel-size 0.15 --pcd-fields xyz \ + --gnss-enu-origin "INPUT_LAT,LON,ALT" --enu-origin "OUTPUT_LAT,LON,ALT" + python3 scripts/export_glim_dump_to_pcd.py /tmp/dump /tmp/lap2.pcd \ + --submap-range START2:END2 --voxel-size 0.15 --pcd-fields xyz \ + --gnss-enu-origin "INPUT_LAT,LON,ALT" --enu-origin "OUTPUT_LAT,LON,ALT" + python3 scripts/export_glim_dump_to_pcd.py /tmp/dump /tmp/lap3.pcd \ + --submap-range START3:END3 --voxel-size 0.15 --pcd-fields xyz \ + --gnss-enu-origin "INPUT_LAT,LON,ALT" --enu-origin "OUTPUT_LAT,LON,ALT" + python3 scripts/export_glim_dump_to_pcd.py /tmp/dump /tmp/staging.pcd \ + --submap-range STAGING_START:STAGING_END --submap-step 10 \ + --voxel-size 0.15 --pcd-fields xyz \ + --gnss-enu-origin "INPUT_LAT,LON,ALT" --enu-origin "OUTPUT_LAT,LON,ALT" + python3 scripts/build_consistent_pcd.py /path/to/deploy_map.pcd \ + /tmp/lap1.pcd /tmp/lap2.pcd /tmp/lap3.pcd \ + --coverage-pcd /tmp/staging.pcd \ + --corridor-trajectory /tmp/dump/traj_lidar.txt \ + --corridor-index-range START1:END3 \ + --corridor-radius 75 --min-sessions 2 --voxel-size 0.15 + ``` + `build_consistent_pcd.py` globally deduplicates each lap, requires + cross-lap support inside the corridor, and keeps unique distant structure + outside it. A sparse pit/staging export may be added with + `--coverage-pcd`; use `--submap-step` when creating that export so a long + stationary period does not dominate map size. The output manifest embeds + every source range, datum, transform, and filter count. 6. **Localize** online against that PCD with `gicp_localization`/`GICP_plusplus`, using the adapter's ENU `/gps_p1/*` streams as IMU + seed. Because the exported map is genuinely ENU, Atlas seeds/GT are frame-correct directly — and `localization/utm_transform_path` must stay **EMPTY** (it exists only for legacy world-frame maps and would double-transform an ENU map). +7. **Audit the compressed map at real time** with the repository runner. It + derives `DATASET_ROOT` from `--map-dir`, refuses to overwrite an existing + result, and writes the debug/reference bags, logs, resource samples, + machine-readable run status and scan scorecard under that dataset's + `gicp_result/`: + ```bash + scripts/run_gicp_replay_audit.sh \ + --map-dir /path/to/DATASET_ROOT/maps/ \ + --bag /path/to/lidar-bag \ + --bag /path/to/navigation-bag \ + --run-name _compressed_full_1x \ + --overlay /path/to/gicp/install/setup.bash \ + --start-offset 0 \ + --duration \ + --rate 1.0 \ + --primary-queue-size 8 + ``` + The offline audit uses RELIABLE LiDAR publication/subscription on both + sides so a large PointCloud2 cannot disappear in DDS without accounting. + Live sensors keep the default BEST_EFFORT profile and the same queue depth. + The online localization contract remains front LiDAR only; the map itself + is built from all configured LiDARs. + + If a recorded odometry stream has documented map-axis translation relative + to the map datum (for example an ellipsoid/geoid height convention), pass + `scripts/offset_odom.py` through `--bridge-script` and repeat + `--bridge-arg` for its explicit input topic, output topic, XYZ offset and + frame. The runner never embeds a site-specific transform. + +8. **Render the audited result from above.** The plotting tool reads the + runner's two output bags and map directly, writes a full-run image plus + complete-lap images, and records the exact input hashes and lap boundaries + in `trajectory_manifest.json`: + ```bash + python3 scripts/generate_gicp_topdown.py \ + --debug-bag /path/to/DATASET_ROOT/gicp_result//debug_topics_bag \ + --reference-bag /path/to/DATASET_ROOT/gicp_result//reference_topics_bag \ + --localization-log /path/to/DATASET_ROOT/gicp_result//localization.log \ + --map /path/to/DATASET_ROOT/maps//map.pcd \ + --output-dir /path/to/DATASET_ROOT/gicp_result//topdown \ + --reference-topic /path/to/reference/topic \ + --run-label " , compressed map, full 1.0x" \ + --map-label "three-LiDAR consistent map" + ``` + ### High-quality mapping profile For a localization map, generate a run-local configuration instead of editing @@ -447,7 +541,7 @@ ros2 launch gicp_localization localization_with_tf.launch.py rviz:=true \ > **Two localizers, an A/B pair.** `gicp_localization` (vendored DLIO / `nano_gicp`) > is the current production/default online localizer and is what this quick > command launches. `GICP_plusplus` is the A/B alternative (a `small_gicp` -> backend with the front/aux synchronizer) used for replay comparison; launch it +> backend with the asynchronous Luminar front worker/aux synchronizer) used for replay comparison; launch it > with `ros2 launch gicp_plusplus localization_with_tf.launch.py map_path:=… …`. > They consume the same ENU map + `/gps_p1/*` streams — pick one per run; they > are not meant to run simultaneously. @@ -498,11 +592,12 @@ Upstream GLIM publishes `glim`, `glim_ext`, and `glim_ros2` as three sibling rep **Robustness against degenerate geometry** (reworked in P1, 2026-07) -- **Confidence-weighted gating** on every GICP solve (replaces the old binary gates, which simultaneously mass-rejected fine corner scans into 25 s dead-reckoning streaks *and* accepted wrong-basin matches): - 1. Hard fitness reject (`gicp/fitnessRejectThreshold`) — unchanged catastrophic backstop. - 2. **Per-map fitness-ratio gates.** A rolling median of accepted-frame fitness normalizes the map's own floor (absolute thresholds go stale on cross-run maps); `fitnessRatioRejectThreshold` catches wrong-basin matches, with `seedBaseline` keeping the gates live during warm-up. +- **Map-independent deployment gating** on every GICP solve: + 1. A 30% correspondence-support gate plus finite-pose validation. + 2. Absolute and rolling-ratio fitness rejection disabled for + perception-ws parity; a finite high ceiling still rejects NaN/Inf. 3. **Degeneracy partial updates** (solution remapping): when the hessian condition proxy trips, the correction is projected onto well-constrained eigen-directions of the vehicle-re-centered, unit-scaled 6×6 hessian (coupled rot/trans null directions included; `degeneracy/full6d`), and the IMU prior is kept along degenerate axes — status `ok_partial` instead of a rejected scan. - 4. **Turn-aware yaw-consistency veto** (`yawGate/*`): a large GICP yaw correction vs. the IMU-integrated prior on a low-confidence match keeps the IMU yaw. + 4. **Turn-aware yaw-consistency veto** (`yawGate/*`) and hard physical yaw bounds. 5. Large-jump reject vs. the IMU-predicted prior (speed/scan-dt-aware thresholds). - **IMU dead-reckoning fallback.** Rejected scans propagate from the IMU-integrated prior — seeded with the *current* IMU-propagated velocity (P2 fixed a stale-velocity bug that made dead-reckoned priors cut corners). - **GT-driven pose recovery** (enabled by default, `min_consecutive_failures: 5`). When GICP rejects N scans in a row, snap pose + twist to a time-matched GT odom sample; angular rate backfills from the live gyro and linear velocity from GT finite-differencing when the odom twist is unpopulated (P2). diff --git a/gicp_localization/cfg/localization.yaml b/gicp_localization/cfg/localization.yaml index 3c9b9520..50213c72 100644 --- a/gicp_localization/cfg/localization.yaml +++ b/gicp_localization/cfg/localization.yaml @@ -214,7 +214,10 @@ # (false yaw pressure). Read the value off the per-aux offset diagnostic # ("header offset vs primary ... mean=+X ms" -> enter -X/1000 here); # runs 19/20 measured 80-90 ms offsets. - localization/lidar_concat/aux_time_offsets: [] + # Keep an explicitly typed vector for ROS 2 parameter parsing. An empty + # YAML sequence has no element type and Jazzy rejects it before node + # startup; zeros preserve the intended "no measured correction" behavior. + localization/lidar_concat/aux_time_offsets: [0.0, 0.0] localization/lidar_concat/buffer_size: 200 # per-aux ring buffer depth (P4#3: raised 20 -> 200 # for GLIM parity; 20 = only 2 s of aux history at # 10 Hz, so brief stalls degraded frames to fewer diff --git a/scripts/build_consistent_pcd.py b/scripts/build_consistent_pcd.py new file mode 100755 index 00000000..5c1e4bb4 --- /dev/null +++ b/scripts/build_consistent_pcd.py @@ -0,0 +1,506 @@ +#!/usr/bin/env python3 +"""Build a compact, repeatability-filtered deployment map from session PCDs. + +The perception-ws mapping pipeline does not deploy a naive union of all mapped +laps. It first makes each input globally unique at a fixed voxel size, +then keeps: + +* voxels observed by at least ``--min-sessions`` inside the driven corridor; +* the union outside that corridor, where distant static structure may only be + visible from one lap/session. + +Inputs must be binary PCDs in the same local-ENU frame and have provenance +manifests named ``.pcd.manifest.yaml``. The output is an XYZ-only binary +PCD suitable for the sparse map loader in GICP++. +""" + +from __future__ import annotations + +import argparse +import datetime +import hashlib +import math +import os +import re +from pathlib import Path +import numpy as np +import yaml +from scipy.spatial import cKDTree + + +PACK_BITS = 21 +PACK_BIAS = 1 << (PACK_BITS - 1) + + +def parse_index_range(text: str) -> tuple[int, int]: + match = re.fullmatch(r"\s*(\d+)\s*:\s*(\d+)\s*", text) + if match is None: + raise ValueError(f"expected START:END, got {text!r}") + start, end = (int(value) for value in match.groups()) + if end <= start: + raise ValueError(f"range must be non-empty and half-open, got [{start}, {end})") + return start, end + + +def invert_se3(transform: np.ndarray) -> np.ndarray: + output = np.eye(4, dtype=np.float64) + output[:3, :3] = transform[:3, :3].T + output[:3, 3] = -transform[:3, :3].T @ transform[:3, 3] + return output + + +def matrix_from_manifest(manifest: dict, key: str) -> np.ndarray: + matrix = np.asarray(manifest.get(key), dtype=np.float64) + if matrix.shape != (4, 4) or not np.all(np.isfinite(matrix)): + raise ValueError(f"manifest {key} must be a finite 4x4 matrix") + if not np.allclose(matrix[3], [0.0, 0.0, 0.0, 1.0], atol=1e-8): + raise ValueError(f"manifest {key} is not a homogeneous transform") + return matrix + + +def read_manifest(pcd_path: Path) -> tuple[Path, dict]: + path = pcd_path.with_suffix(pcd_path.suffix + ".manifest.yaml") + if not path.is_file(): + raise FileNotFoundError(f"required map provenance manifest not found: {path}") + with path.open("r", encoding="utf-8") as handle: + manifest = yaml.safe_load(handle) + if not isinstance(manifest, dict): + raise ValueError(f"{path}: expected a YAML mapping") + if manifest.get("frame") != "enu": + raise ValueError(f"{path}: input map frame must be 'enu'") + if not manifest.get("enu_origin"): + raise ValueError(f"{path}: input map must declare enu_origin") + return path, manifest + + +def source_export_summary(pcd_path: Path, manifest_path: Path, manifest: dict) -> dict: + """Keep dump/range provenance even if a generated slice is later removed.""" + keys = ( + "source_dump", + "submap_range", + "submap_start", + "submap_end_exclusive", + "submap_step", + "selected_submaps", + "points", + "voxel_size", + "pcd_fields", + "frame", + "enu_origin", + "gnss_enu_origin", + "applied_transform", + ) + summary = { + "pcd": str(pcd_path.resolve()), + "manifest": str(manifest_path.resolve()), + } + summary.update({key: manifest[key] for key in keys if key in manifest}) + summary.setdefault("submap_step", 1) + return summary + + +def pcd_memmap(path: Path) -> tuple[np.memmap, dict]: + """Open a binary PCD as a structured memory map.""" + header: dict[str, list[str]] = {} + with path.open("rb") as handle: + while True: + raw = handle.readline() + if not raw: + raise ValueError(f"{path}: missing DATA line") + try: + line = raw.decode("ascii").strip() + except UnicodeDecodeError as exc: + raise ValueError(f"{path}: non-ASCII PCD header") from exc + if not line or line.startswith("#"): + continue + parts = line.split() + key = parts[0].upper() + header[key] = parts[1:] + if key == "DATA": + data_offset = handle.tell() + break + + if header.get("DATA") != ["binary"]: + raise ValueError(f"{path}: only DATA binary PCD is supported") + fields = header.get("FIELDS", []) + sizes = [int(value) for value in header.get("SIZE", [])] + types = header.get("TYPE", []) + counts = [int(value) for value in header.get("COUNT", ["1"] * len(fields))] + if not (len(fields) == len(sizes) == len(types) == len(counts)): + raise ValueError(f"{path}: inconsistent FIELDS/SIZE/TYPE/COUNT header") + if any(count != 1 for count in counts): + raise ValueError(f"{path}: vector-valued PCD fields are not supported") + for axis in ("x", "y", "z"): + if axis not in fields: + raise ValueError(f"{path}: missing {axis!r} field") + index = fields.index(axis) + if sizes[index] != 4 or types[index].upper() != "F": + raise ValueError(f"{path}: {axis} must be a float32 scalar") + + offsets = np.cumsum([0] + sizes[:-1]).tolist() + formats = [] + for scalar_type, size in zip(types, sizes): + code = { + ("F", 4): " np.ndarray: + output = np.empty((len(points), 3), dtype=np.float32) + output[:, 0] = points["x"] + output[:, 1] = points["y"] + output[:, 2] = points["z"] + return output + + +def pack_voxels(points: np.ndarray, voxel_size: float) -> np.ndarray: + voxels = np.floor(points / voxel_size).astype(np.int64) + if np.any(voxels < -PACK_BIAS) or np.any(voxels >= PACK_BIAS): + minimum = voxels.min(axis=0).tolist() + maximum = voxels.max(axis=0).tolist() + raise ValueError( + f"voxel coordinates exceed signed {PACK_BITS}-bit packing range: " + f"min={minimum} max={maximum}" + ) + unsigned = (voxels + PACK_BIAS).astype(np.uint64) + return ( + (unsigned[:, 0] << np.uint64(2 * PACK_BITS)) + | (unsigned[:, 1] << np.uint64(PACK_BITS)) + | unsigned[:, 2] + ) + + +def unique_session(path: Path, voxel_size: float) -> tuple[np.ndarray, np.ndarray, int]: + mapped, _ = pcd_memmap(path) + raw_points = len(mapped) + points = xyz_array(mapped) + del mapped + keys = pack_voxels(points, voxel_size) + unique_keys, first_indices = np.unique(keys, return_index=True) + unique_points = points[first_indices] + print( + f"[consistent_pcd] {path}: raw={raw_points} " + f"unique_{voxel_size:g}m={len(unique_keys)}", + flush=True, + ) + return unique_keys, unique_points, raw_points + + +def transformed_centerline( + trajectory_path: Path, + index_range: tuple[int, int], + source_manifest: dict, + sample_stride: int, +) -> np.ndarray: + trajectory = np.loadtxt(trajectory_path, dtype=np.float64) + if trajectory.ndim != 2 or trajectory.shape[1] < 4: + raise ValueError(f"{trajectory_path}: expected stamp x y z ... rows") + start, end = index_range + if end > len(trajectory): + raise ValueError( + f"centerline range [{start}, {end}) exceeds {len(trajectory)} trajectory rows" + ) + world_points = trajectory[start:end:sample_stride, 1:4] + if (end - start - 1) % sample_stride: + world_points = np.vstack([world_points, trajectory[end - 1, 1:4]]) + T_world_utm = matrix_from_manifest(source_manifest, "T_world_utm") + T_output_input = matrix_from_manifest( + source_manifest, "T_output_enu_input_enu" + ) + T_output_world = T_output_input @ invert_se3(T_world_utm) + output = world_points @ T_output_world[:3, :3].T + T_output_world[:3, 3] + return output + + +def write_xyz_pcd(path: Path, points: np.ndarray) -> None: + header = ( + "# .PCD v0.7 - Point Cloud Data file format\n" + "VERSION 0.7\n" + "FIELDS x y z\n" + "SIZE 4 4 4\n" + "TYPE F F F\n" + "COUNT 1 1 1\n" + f"WIDTH {len(points)}\n" + "HEIGHT 1\n" + "VIEWPOINT 0 0 0 1 0 0 0\n" + f"POINTS {len(points)}\n" + "DATA binary\n" + ) + with path.open("wb") as handle: + handle.write(header.encode("ascii")) + points.astype(" str: + digest = hashlib.sha256() + with path.open("rb") as handle: + for chunk in iter(lambda: handle.read(8 * 1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("output_pcd", type=Path) + parser.add_argument("input_pcd", type=Path, nargs="+") + parser.add_argument( + "--coverage-pcd", + type=Path, + action="append", + default=[], + help="Additional preselected staging/pit coverage. These maps are globally " + "deduplicated into the output but do not count toward cross-session support.", + ) + parser.add_argument("--voxel-size", type=float, default=0.15) + parser.add_argument("--min-sessions", type=int, default=2) + parser.add_argument("--corridor-trajectory", type=Path, required=True) + parser.add_argument( + "--corridor-index-range", + type=str, + required=True, + metavar="START:END", + help="Half-open trajectory row range corresponding to the mapped representative laps.", + ) + parser.add_argument("--corridor-radius", type=float, default=35.0) + parser.add_argument("--centerline-stride", type=int, default=10) + parser.add_argument( + "--query-chunk", + type=int, + default=1_000_000, + help="Number of union points per nearest-centerline query chunk.", + ) + parser.add_argument( + "--force", + action="store_true", + help="Replace an existing output PCD/manifest.", + ) + args = parser.parse_args() + + if len(args.input_pcd) < 2: + parser.error("at least two input PCDs are required for a consistency map") + if not math.isfinite(args.voxel_size) or args.voxel_size <= 0.0: + parser.error("--voxel-size must be finite and > 0") + if args.min_sessions < 2 or args.min_sessions > len(args.input_pcd): + parser.error("--min-sessions must be between 2 and the number of input maps") + if not math.isfinite(args.corridor_radius) or args.corridor_radius <= 0.0: + parser.error("--corridor-radius must be finite and > 0") + if args.centerline_stride < 1 or args.query_chunk < 1: + parser.error("--centerline-stride and --query-chunk must be >= 1") + try: + corridor_range = parse_index_range(args.corridor_index_range) + except ValueError as exc: + parser.error(f"--corridor-index-range invalid: {exc}") + + manifest_pairs = [read_manifest(path) for path in args.input_pcd] + reference_origin = str(manifest_pairs[0][1]["enu_origin"]).split("#", 1)[0].strip() + coverage_manifest_pairs = [read_manifest(path) for path in args.coverage_pcd] + for manifest_path, manifest in manifest_pairs[1:] + coverage_manifest_pairs: + origin = str(manifest["enu_origin"]).split("#", 1)[0].strip() + if origin != reference_origin: + raise SystemExit( + f"ENU datum mismatch: {manifest_path} has {origin!r}, " + f"expected {reference_origin!r}" + ) + + output_manifest = args.output_pcd.with_suffix( + args.output_pcd.suffix + ".manifest.yaml" + ) + if not args.force and (args.output_pcd.exists() or output_manifest.exists()): + raise SystemExit( + f"output exists: {args.output_pcd} or {output_manifest}; use --force to replace" + ) + args.output_pcd.parent.mkdir(parents=True, exist_ok=True) + + session_keys: list[np.ndarray] = [] + session_points: list[np.ndarray] = [] + raw_counts: list[int] = [] + session_unique_counts: list[int] = [] + for path in args.input_pcd: + keys, points, raw_count = unique_session(path, args.voxel_size) + session_keys.append(keys) + session_points.append(points) + raw_counts.append(raw_count) + session_unique_counts.append(len(keys)) + + all_keys = np.concatenate(session_keys) + all_points = np.concatenate(session_points) + union_keys, first_indices, support_counts = np.unique( + all_keys, return_index=True, return_counts=True + ) + union_count = len(union_keys) + representatives = all_points[first_indices] + del all_keys, all_points, session_keys, session_points, first_indices + + repeated = support_counts >= args.min_sessions + consistent_count = int(np.count_nonzero(repeated)) + centerline = transformed_centerline( + args.corridor_trajectory, + corridor_range, + manifest_pairs[0][1], + args.centerline_stride, + ) + tree = cKDTree(centerline[:, :2]) + keep = repeated.copy() + outside_unique_count = 0 + for start in range(0, len(union_keys), args.query_chunk): + end = min(start + args.query_chunk, len(union_keys)) + candidate = ~repeated[start:end] + if not np.any(candidate): + continue + distances = tree.query( + representatives[start:end][candidate, :2], k=1, workers=-1 + )[0] + outside = distances > args.corridor_radius + indices = np.flatnonzero(candidate) + keep[start + indices[outside]] = True + outside_unique_count += int(np.count_nonzero(outside)) + print( + f"[consistent_pcd] corridor query {end}/{len(union_keys)}", + flush=True, + ) + + output_keys = union_keys[keep] + output_points = representatives[keep] + coverage_unique_counts: list[int] = [] + coverage_added_counts: list[int] = [] + for coverage_index, coverage_path in enumerate(args.coverage_pcd): + coverage_keys, coverage_points, _ = unique_session( + coverage_path, args.voxel_size + ) + coverage_unique_counts.append(len(coverage_keys)) + add = ~np.isin(coverage_keys, output_keys, assume_unique=True) + coverage_added_counts.append(int(np.count_nonzero(add))) + output_keys = np.concatenate([output_keys, coverage_keys[add]]) + output_points = np.concatenate([output_points, coverage_points[add]]) + # Keep output_keys unique before processing another coverage map. + if coverage_index + 1 < len(args.coverage_pcd): + order = np.argsort(output_keys) + output_keys = output_keys[order] + output_points = output_points[order] + del representatives, keep, union_keys, support_counts, repeated, output_keys + if len(output_points) == 0: + raise SystemExit("consistency filter produced zero points") + + temporary_pcd = args.output_pcd.with_suffix(args.output_pcd.suffix + ".tmp") + temporary_manifest = output_manifest.with_suffix(output_manifest.suffix + ".tmp") + temporary_pcd.unlink(missing_ok=True) + temporary_manifest.unlink(missing_ok=True) + success = False + try: + write_xyz_pcd(temporary_pcd, output_points) + output_bytes = temporary_pcd.stat().st_size + output_sha256 = sha256_file(temporary_pcd) + manifest = { + "format": "consistent_pcd_v1", + "built_utc": datetime.datetime.now(datetime.timezone.utc).isoformat(), + "frame": "enu", + "enu_origin": reference_origin, + "pcd_fields": "xyz", + "points": int(len(output_points)), + "pcd_bytes": int(output_bytes), + "pcd_sha256": output_sha256, + "voxel_size": float(args.voxel_size), + "source_maps": [str(path.resolve()) for path in args.input_pcd], + "source_manifests": [ + str(path.resolve()) for path, _ in manifest_pairs + ], + "source_exports": [ + source_export_summary(pcd, manifest_path, source_manifest) + for pcd, (manifest_path, source_manifest) in zip( + args.input_pcd, manifest_pairs + ) + ], + "coverage_maps": [str(path.resolve()) for path in args.coverage_pcd], + "coverage_manifests": [ + str(path.resolve()) for path, _ in coverage_manifest_pairs + ], + "coverage_exports": [ + source_export_summary(pcd, manifest_path, source_manifest) + for pcd, (manifest_path, source_manifest) in zip( + args.coverage_pcd, coverage_manifest_pairs + ) + ], + "source_raw_points": raw_counts, + "source_unique_voxels": session_unique_counts, + "union_voxels": int(union_count), + "consistent_voxels": consistent_count, + "minimum_sessions": int(args.min_sessions), + "unique_voxels_kept_outside_corridor": outside_unique_count, + "coverage_unique_voxels": coverage_unique_counts, + "coverage_voxels_added": coverage_added_counts, + "corridor_radius_m": float(args.corridor_radius), + "corridor_trajectory": str(args.corridor_trajectory.resolve()), + "corridor_index_range": args.corridor_index_range, + "corridor_centerline_stride": int(args.centerline_stride), + "algorithm": ( + "global voxel union; require min-session support inside driven " + "corridor; retain unique coverage outside" + ), + "gicp_note": "leave localization/utm_transform_path EMPTY for this local-ENU map", + } + # Keep the exact upstream transforms, so the driven centerline and map + # frame remain independently auditable. + manifest["T_world_utm"] = manifest_pairs[0][1]["T_world_utm"] + manifest["T_output_enu_input_enu"] = manifest_pairs[0][1][ + "T_output_enu_input_enu" + ] + manifest["output_formula"] = ( + "consistent_voxels + unique_voxels_kept_outside_corridor " + "+ globally-new coverage_voxels_added" + ) + with temporary_manifest.open("w", encoding="utf-8") as handle: + yaml.safe_dump(manifest, handle, sort_keys=False) + handle.flush() + os.fsync(handle.fileno()) + temporary_manifest.replace(output_manifest) + temporary_pcd.replace(args.output_pcd) + success = True + finally: + if not success: + temporary_pcd.unlink(missing_ok=True) + temporary_manifest.unlink(missing_ok=True) + + print( + f"[consistent_pcd] wrote {len(output_points)} points " + f"({consistent_count} repeated + {outside_unique_count} unique outside corridor " + f"+ {sum(coverage_added_counts)} staging coverage) " + f"to {args.output_pcd}", + flush=True, + ) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/export_glim_dump_to_pcd.py b/scripts/export_glim_dump_to_pcd.py index ff0f66f2..c929c3ed 100644 --- a/scripts/export_glim_dump_to_pcd.py +++ b/scripts/export_glim_dump_to_pcd.py @@ -163,6 +163,19 @@ def submap_dirs(dump_dir: Path) -> list[Path]: return sorted(dirs, key=lambda p: int(p.name)) +def parse_submap_range(text: str) -> tuple[int, int]: + """Parse a half-open ``START:END`` submap-index range.""" + match = re.fullmatch(r"\s*(\d+)\s*:\s*(\d+)\s*", text) + if match is None: + raise ValueError(f"expected START:END, got {text!r}") + start, end = (int(value) for value in match.groups()) + if end <= start: + raise ValueError( + f"submap range must be non-empty and half-open, got [{start}, {end})" + ) + return start, end + + def point_count(path: Path) -> int: size = path.stat().st_size if size % 12 != 0: @@ -185,6 +198,7 @@ def transformed_chunks( dirs: Iterable[Path], voxel_size: float, stride: int, + pcd_fields: str, pre_transform: Optional[np.ndarray] = None, ) -> Iterable[np.ndarray]: seen: Optional[set[tuple[int, int, int]]] = set() if voxel_size > 0.0 else None @@ -219,23 +233,37 @@ def transformed_chunks( world = world[keep] intensities = intensities[keep] - out = np.empty(world.shape[0], dtype=[("x", " {len(out)} points", flush=True) yield out -def write_header(handle, count: int) -> None: +def write_header(handle, count: int, pcd_fields: str) -> None: + if pcd_fields == "xyz": + fields = "x y z" + scalar_columns = "4 4 4" + scalar_types = "F F F" + scalar_counts = "1 1 1" + else: + fields = "x y z intensity" + scalar_columns = "4 4 4 4" + scalar_types = "F F F F" + scalar_counts = "1 1 1 1" header = ( "# .PCD v0.7 - Point Cloud Data file format\n" "VERSION 0.7\n" - "FIELDS x y z intensity\n" - "SIZE 4 4 4 4\n" - "TYPE F F F F\n" - "COUNT 1 1 1 1\n" + f"FIELDS {fields}\n" + f"SIZE {scalar_columns}\n" + f"TYPE {scalar_types}\n" + f"COUNT {scalar_counts}\n" f"WIDTH {count}\n" "HEIGHT 1\n" "VIEWPOINT 0 0 0 1 0 0 0\n" @@ -251,6 +279,30 @@ def main() -> int: parser.add_argument("output_pcd", type=Path) parser.add_argument("--voxel-size", type=float, default=0.0) parser.add_argument("--stride", type=int, default=1) + parser.add_argument( + "--pcd-fields", + choices=("xyz", "xyzi"), + default="xyzi", + help="PCD payload fields. Use 'xyz' for perception-ws-compatible deployment maps; " + "'xyzi' preserves the historical dense-map output.", + ) + parser.add_argument( + "--submap-range", + type=str, + default="", + metavar="START:END", + help="Export only the half-open numeric submap range [START, END). Use this to " + "export representative complete laps independently instead of accumulating " + "an entire multi-lap session.", + ) + parser.add_argument( + "--submap-step", + type=int, + default=1, + help="After --submap-range selection, export every Nth submap. This is useful " + "for low-speed/stationary staging coverage where adjacent 10 Hz submaps are " + "almost identical. Default 1 keeps every selected submap.", + ) parser.add_argument( "--frame", choices=("enu", "world"), @@ -327,10 +379,37 @@ def main() -> int: ) if args.stride < 1: parser.error("--stride must be >= 1") + if args.submap_step < 1: + parser.error("--submap-step must be >= 1") dirs = submap_dirs(args.dump_dir) if not dirs: raise SystemExit(f"no GLIM submap dirs found under {args.dump_dir}") + selected_submap_range: Optional[tuple[int, int]] = None + if args.submap_range: + try: + selected_submap_range = parse_submap_range(args.submap_range) + except ValueError as exc: + parser.error(f"--submap-range invalid: {exc}") + start, end = selected_submap_range + dirs = [path for path in dirs if start <= int(path.name) < end] + if not dirs: + raise SystemExit( + f"--submap-range [{start}, {end}) selected no valid submaps " + f"under {args.dump_dir}" + ) + print( + f"[export_glim_dump_to_pcd] selected {len(dirs)} submaps in " + f"half-open range [{start}, {end})", + flush=True, + ) + if args.submap_step > 1: + dirs = dirs[:: args.submap_step] + print( + f"[export_glim_dump_to_pcd] submap step {args.submap_step}: " + f"retained {len(dirs)} submaps", + flush=True, + ) pre_transform: Optional[np.ndarray] = None enu_reanchor = np.eye(4, dtype=np.float64) @@ -395,7 +474,13 @@ def main() -> int: success = False try: with data_tmp.open("wb") as data_handle: - for chunk in transformed_chunks(dirs, args.voxel_size, args.stride, pre_transform): + for chunk in transformed_chunks( + dirs, + args.voxel_size, + args.stride, + args.pcd_fields, + pre_transform, + ): chunk.tofile(data_handle) total += len(chunk) @@ -403,7 +488,7 @@ def main() -> int: raise SystemExit("export produced zero points") with tmp.open("wb") as handle: - write_header(handle, total) + write_header(handle, total, args.pcd_fields) with data_tmp.open("rb") as data_handle: shutil.copyfileobj(data_handle, handle, length=8 * 1024 * 1024) # [SELF-AUDIT FIX 2026-07-10] Manifest FIRST, then finalize the PCD: @@ -422,6 +507,16 @@ def main() -> int: mh.write(f"points: {total}\n") mh.write(f"voxel_size: {args.voxel_size}\n") mh.write(f"stride: {args.stride}\n") + mh.write(f"submap_step: {args.submap_step}\n") + mh.write(f"pcd_fields: {args.pcd_fields}\n") + if selected_submap_range is None: + mh.write("submap_range: all\n") + else: + start, end = selected_submap_range + mh.write(f"submap_range: \"{start}:{end}\" # half-open [start, end)\n") + mh.write(f"submap_start: {start}\n") + mh.write(f"submap_end_exclusive: {end}\n") + mh.write(f"selected_submaps: {len(dirs)}\n") if args.enu_origin: mh.write(f"enu_origin: {args.enu_origin} # output map datum\n") mh.write(f"gnss_enu_origin: {args.gnss_enu_origin} # mapping input datum\n") diff --git a/scripts/generate_gicp_topdown.py b/scripts/generate_gicp_topdown.py new file mode 100755 index 00000000..2ccedaef --- /dev/null +++ b/scripts/generate_gicp_topdown.py @@ -0,0 +1,1089 @@ +#!/usr/bin/env python3 +"""Generate dataset-independent full-run and per-lap GICP top-down plots. + +The tool intentionally does not infer a dataset root or embed a map/run path. +Callers inject four independent inputs: + +* ``--debug-bag``: localization output poses. +* ``--reference-bag``: the GNSS/reference trajectory. +* ``--localization-log``: optional GICP status and reset evidence. +* ``--map``: the PCD drawn in the background. + +Topic names, reference-to-map transform, labels, lap detector thresholds, map +sampling, plot margin, and output directory are also command-line parameters. +This keeps one plotting implementation reusable across Laguna, Putnam, and +future datasets without borrowing another dataset's ENU origin or paths. + +Typical injection pattern:: + + python3 scripts/generate_gicp_topdown.py \ + --debug-bag "$DATASET_ROOT/gicp_result//debug_topics_bag" \ + --reference-bag "$DATASET_ROOT/prep_bag/" \ + --localization-log "$DATASET_ROOT/gicp_result//localization.log" \ + --map "$DATASET_ROOT/maps/.pcd" \ + --output-dir "$DATASET_ROOT/gicp_result//topdown" \ + --reference-topic /gnss \ + --reference-offset \ + --run-label " " --map-label "" + +The shell expands these paths before Python starts. The tool reads only the +injected locations and creates files only below ``--output-dir``. +""" + +from __future__ import annotations + +import argparse +import bisect +import hashlib +import json +import math +import re +from dataclasses import dataclass +from pathlib import Path + +import matplotlib + +matplotlib.use("Agg") +import matplotlib.pyplot as plt +import numpy as np +from rclpy.serialization import deserialize_message +from rosbag2_py import ConverterOptions, SequentialReader, StorageFilter, StorageOptions +from rosidl_runtime_py.utilities import get_message + + +DEFAULT_FINAL_TOPIC = "/gicp/localization/debug/final_pose" +DEFAULT_GUESS_TOPIC = "/gicp/localization/debug/initial_guess_pose" +DEFAULT_REFERENCE_TOPIC = "/gnss" +STATUS_RE = re.compile( + r"\[[A-Z]+\] \[([0-9]+\.[0-9]+)\] \[[^\]]+\]: " + r".*SCAN DEBUG \| status=([^ ]+) stamp=([0-9.]+)" +) +RESET_RE = re.compile( + r"\[[A-Z]+\] \[([0-9]+\.[0-9]+)\] \[[^\]]+\]: .*" + r"absolute state reset at \[([-+0-9.eE]+), ([-+0-9.eE]+), ([-+0-9.eE]+)\]" +) + + +@dataclass(frozen=True) +class Interval: + index: int + start: float + end: float + complete: bool + kind: str + + +@dataclass(frozen=True) +class PlotConfig: + """Injected labels and rendering choices shared by every output image.""" + + run_label: str + map_label: str + trajectory_label: str + reference_label: str + reference_offset: tuple[float, float, float] + reference_yaw_deg: float + margin_m: float + dpi: int + + +def arguments() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="Render a GICP trajectory, reference trajectory, and PCD from above.", + formatter_class=argparse.ArgumentDefaultsHelpFormatter, + epilog=( + "All paths are injected explicitly. The reference transform is applied as " + "map_xyz = Rz(reference_yaw_deg) * reference_xyz + reference_offset. " + "Pass the transform belonging to the selected map/dataset; never reuse an " + "ENU offset from another site." + ), + ) + + paths = parser.add_argument_group("input and output paths") + paths.add_argument( + "--debug-bag", + required=True, + type=Path, + help="rosbag2 directory containing the final and initial-guess pose topics", + ) + paths.add_argument( + "--reference-bag", + "--input-bag", + dest="reference_bag", + required=True, + type=Path, + help="rosbag2 directory containing the GNSS or other absolute reference topic", + ) + paths.add_argument( + "--localization-log", + type=Path, + help=( + "optional GICP log with SCAN DEBUG status rows and absolute-reset rows; " + "without it, final poses are used directly and reset markers are omitted" + ), + ) + paths.add_argument( + "--map", + dest="map_path", + required=True, + type=Path, + help="binary PCD map; x/y/z may coexist with additional point fields", + ) + paths.add_argument( + "--output-dir", + required=True, + type=Path, + help="destination outside the source repository for PNGs and trajectory_manifest.json", + ) + + topics = parser.add_argument_group("topic injection") + topics.add_argument( + "--final-pose-topic", + default=DEFAULT_FINAL_TOPIC, + help="accepted/final localization pose topic in --debug-bag", + ) + topics.add_argument( + "--initial-guess-topic", + default=DEFAULT_GUESS_TOPIC, + help="initial-guess pose topic used for rejected frames in --debug-bag", + ) + topics.add_argument( + "--reference-topic", + default=DEFAULT_REFERENCE_TOPIC, + help="PoseStamped, PoseWithCovarianceStamped, or Odometry-like reference topic", + ) + + transform = parser.add_argument_group("reference-to-map transform injection") + transform.add_argument( + "--reference-offset", + nargs=3, + type=float, + metavar=("X", "Y", "Z"), + default=(0.0, 0.0, 0.0), + help="translation in meters after rotating the reference coordinates", + ) + transform.add_argument( + "--reference-yaw-deg", + type=float, + default=0.0, + help="counter-clockwise yaw applied to reference XY before translation", + ) + + labels = parser.add_argument_group("plot labels") + labels.add_argument("--run-label", default="GICP replay", help="run/dataset title") + labels.add_argument("--map-label", default="PCD map", help="map legend/title label") + labels.add_argument( + "--trajectory-label", + default="GICP trajectory", + help="localization trajectory legend/title label", + ) + labels.add_argument( + "--reference-label", + default="absolute reference", + help="GNSS/reference trajectory legend label", + ) + + rendering = parser.add_argument_group("rendering parameters") + rendering.add_argument( + "--map-sample-count", + type=int, + default=900_000, + help="deterministic maximum number of PCD points rendered", + ) + rendering.add_argument( + "--map-sample-seed", + type=int, + default=325, + help="random seed used only for deterministic PCD subsampling", + ) + rendering.add_argument( + "--plot-margin-m", + type=float, + default=45.0, + help="XY margin around all trajectory samples", + ) + rendering.add_argument("--dpi", type=int, default=190, help="output PNG resolution") + + laps = parser.add_argument_group("automatic lap segmentation parameters") + laps.add_argument( + "--min-lap-duration-s", + type=float, + default=60.0, + help="minimum time between two crossings accepted as one complete lap", + ) + laps.add_argument( + "--max-lap-duration-s", + type=float, + default=600.0, + help="maximum time between two crossings accepted as one complete lap", + ) + laps.add_argument( + "--start-line-half-width-m", + type=float, + default=25.0, + help="lateral half-width of each candidate start/finish line", + ) + laps.add_argument( + "--min-crossing-speed-mps", + type=float, + default=3.0, + help="minimum forward speed for accepting a start/finish crossing", + ) + laps.add_argument( + "--min-partial-duration-s", + type=float, + default=5.0, + help="minimum leading/trailing duration emitted as a partial segment", + ) + laps.add_argument( + "--no-lap-split", + action="store_true", + help="write only the full-run plot when the dataset is not a closed-course replay", + ) + return parser.parse_args() + + +def sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def open_reader(path: Path, topics: list[str]): + reader = SequentialReader() + reader.open(StorageOptions(uri=str(path), storage_id=""), ConverterOptions("cdr", "cdr")) + topic_types = {item.name: item.type for item in reader.get_all_topics_and_types()} + missing = [topic for topic in topics if topic not in topic_types] + if missing: + raise RuntimeError(f"missing topics in {path}: {missing}") + reader.set_filter(StorageFilter(topics=topics)) + return reader, {topic: get_message(topic_types[topic]) for topic in topics} + + +def stamped_position(message) -> tuple[float, float, float, float]: + """Read a stamped position from common ROS pose container messages.""" + + stamp = message.header.stamp.sec + message.header.stamp.nanosec * 1e-9 + pose = message.pose + while not hasattr(pose, "position"): + if not hasattr(pose, "pose"): + raise RuntimeError(f"message {type(message).__name__} has no pose.position") + pose = pose.pose + return stamp, float(pose.position.x), float(pose.position.y), float(pose.position.z) + + +def read_debug_poses(path: Path, final_topic: str, guess_topic: str): + reader, message_types = open_reader(path, [final_topic, guess_topic]) + series = {final_topic: [], guess_topic: []} + while reader.has_next(): + topic, data, _ = reader.read_next() + message = deserialize_message(data, message_types[topic]) + series[topic].append(stamped_position(message)) + return series[final_topic], series[guess_topic] + + +def read_reference( + path: Path, + topic_name: str, + offset: tuple[float, float, float], + yaw_deg: float, +): + """Load a reference trajectory and place it in the selected map frame. + + The transform is deliberately injected by the caller because its values + belong to the selected dataset/map contract. Rotation is applied before + translation: ``map_xyz = Rz(yaw) * reference_xyz + offset``. + """ + + reader, message_types = open_reader(path, [topic_name]) + yaw = math.radians(yaw_deg) + cosine = math.cos(yaw) + sine = math.sin(yaw) + result = [] + while reader.has_next(): + topic, data, _ = reader.read_next() + message = deserialize_message(data, message_types[topic]) + stamp, x, y, z = stamped_position(message) + result.append( + ( + stamp, + cosine * x - sine * y + offset[0], + sine * x + cosine * y + offset[1], + z + offset[2], + ) + ) + result.sort(key=lambda row: row[0]) + return result + + +def parse_log(path: Path | None): + """Parse optional log evidence emitted by the repository's GICP node.""" + + if path is None: + return [], [] + statuses = [] + reset_wall_rows = [] + for line in path.read_text(errors="replace").splitlines(): + match = STATUS_RE.search(line) + if match: + statuses.append((float(match.group(3)), match.group(2), float(match.group(1)))) + match = RESET_RE.search(line) + if match: + reset_wall_rows.append( + ( + float(match.group(1)), + float(match.group(2)), + float(match.group(3)), + float(match.group(4)), + ) + ) + statuses.sort(key=lambda row: row[0]) + if reset_wall_rows and not statuses: + raise RuntimeError( + f"{path} has absolute-reset rows but no SCAN DEBUG rows to align them" + ) + wall_sorted = sorted(statuses, key=lambda row: row[2]) + status_walls = np.asarray([row[2] for row in wall_sorted], dtype=np.float64) + status_stamps = np.asarray([row[0] for row in wall_sorted], dtype=np.float64) + resets = [] + for wall, x, y, z in reset_wall_rows: + source_stamp = float(np.interp(wall, status_walls, status_stamps)) + resets.append((source_stamp, x, y, z)) + return statuses, resets + + +def closest_status(stamps: list[float], rows, stamp: float): + if not stamps: + return None + index = bisect.bisect_left(stamps, stamp) + candidates = [index] + if index: + candidates.append(index - 1) + if index + 1 < len(stamps): + candidates.append(index + 1) + if not candidates: + return None + nearest = min(candidates, key=lambda item: abs(stamps[item] - stamp)) + if abs(stamps[nearest] - stamp) > 0.005: + return None + return rows[nearest][1] + + +def official_trajectory(finals, guesses, statuses): + if len(finals) != len(guesses): + raise RuntimeError(f"debug pose count mismatch: final={len(finals)} guess={len(guesses)}") + status_stamps = [row[0] for row in statuses] + official = [] + rejected = 0 + matched = 0 + for final, guess in zip(finals, guesses): + if abs(final[0] - guess[0]) > 0.005: + raise RuntimeError(f"debug pose stamp mismatch: {final[0]} vs {guess[0]}") + status = closest_status(status_stamps, statuses, final[0]) + if status is not None: + matched += 1 + if status is not None and not status.startswith("ok"): + official.append(guess) + rejected += 1 + else: + official.append(final) + return official, rejected, matched + + +def pcd_scalar_dtype(type_code: str, size: int) -> np.dtype: + """Translate one PCD scalar declaration to a little-endian NumPy dtype.""" + + codes = { + ("F", 4): " float: + return float(np.percentile(values, q)) + + +def merge_crossings(crossings: list[tuple[float, float]], min_gap_s: float) -> list[float]: + if not crossings: + return [] + groups = [[crossings[0]]] + for crossing in crossings[1:]: + if crossing[0] - groups[-1][-1][0] < min_gap_s: + groups[-1].append(crossing) + else: + groups.append([crossing]) + return [min(group, key=lambda item: item[1])[0] for group in groups] + + +def crossings_for_line( + stamps: np.ndarray, + xy: np.ndarray, + anchor: np.ndarray, + tangent: np.ndarray, + line_half_width_m: float, + min_speed_mps: float, + min_lap_duration_s: float, +) -> list[float]: + normal = np.asarray((-tangent[1], tangent[0]), dtype=np.float64) + relative = xy - anchor + along = relative @ tangent + indices = np.flatnonzero((along[:-1] <= 0.0) & (along[1:] > 0.0)) + result = [] + for index in indices: + denominator = float(along[index + 1] - along[index]) + dt = float(stamps[index + 1] - stamps[index]) + if denominator <= 1e-9 or dt <= 0.0: + continue + alpha = float(np.clip(-along[index] / denominator, 0.0, 1.0)) + point = xy[index] + alpha * (xy[index + 1] - xy[index]) + lateral = abs(float((point - anchor) @ normal)) + forward_speed = float((xy[index + 1] - xy[index]) @ tangent) / dt + if lateral <= line_half_width_m and forward_speed >= min_speed_mps: + result.append((float(stamps[index] + alpha * dt), lateral)) + return merge_crossings(result, min_lap_duration_s) + + +def detect_intervals( + reference: np.ndarray, + min_lap_duration_s: float = 60.0, + max_lap_duration_s: float = 600.0, + line_half_width_m: float = 25.0, + min_speed_mps: float = 3.0, + min_partial_duration_s: float = 5.0, +): + """Find complete laps from repeated same-direction reference crossings.""" + + stamps = reference[:, 0] + xy = reference[:, 1:3] + dt = np.gradient(stamps) + velocity = np.gradient(xy, axis=0) / dt[:, None] + speed = np.linalg.norm(velocity, axis=1) + valid = np.flatnonzero( + (speed >= min_speed_mps) + & (np.arange(len(reference)) >= 5) + & (np.arange(len(reference)) < len(reference) - 5) + ) + if len(valid) > 180: + valid = valid[np.linspace(0, len(valid) - 1, 180, dtype=np.int64)] + + best = None + for candidate in valid: + delta = xy[candidate + 5] - xy[candidate - 5] + norm = float(np.linalg.norm(delta)) + if norm < 1e-6: + continue + tangent = delta / norm + crossings = crossings_for_line( + stamps, + xy, + xy[candidate], + tangent, + line_half_width_m, + min_speed_mps, + min_lap_duration_s, + ) + if len(crossings) < 2: + continue + durations = np.diff(np.asarray(crossings)) + plausible = durations[ + (durations >= min_lap_duration_s) & (durations <= max_lap_duration_s) + ] + if not len(plausible): + continue + median = float(np.median(plausible)) + regular = int( + np.count_nonzero(np.abs(plausible - median) <= max(10.0, 0.30 * median)) + ) + cv = float(np.std(plausible) / median) if len(plausible) > 1 else 0.0 + score = (regular, len(plausible), -cv, crossings[-1] - crossings[0]) + if best is None or score > best[0]: + best = (score, crossings, xy[candidate].copy(), tangent.copy(), cv) + + if best is None: + return [Interval(1, float(stamps[0]), float(stamps[-1]), False, "partial_run")], { + "method": "whole-run fallback", + "confidence": "partial", + "crossing_times": [], + "note": "No repeated same-direction crossing found.", + } + + _, crossings, anchor, tangent, cv = best + complete = [ + Interval(index + 1, start, end, True, "lap") + for index, (start, end) in enumerate(zip(crossings[:-1], crossings[1:])) + if min_lap_duration_s <= end - start <= max_lap_duration_s + ] + if not complete: + return [Interval(1, float(stamps[0]), float(stamps[-1]), False, "partial_run")], { + "method": "whole-run fallback", + "confidence": "partial", + "crossing_times": crossings, + "note": "Crossings did not form a plausible lap.", + } + + intervals = [] + partial_index = 1 + if complete[0].start - stamps[0] >= min_partial_duration_s: + intervals.append( + Interval(partial_index, float(stamps[0]), complete[0].start, False, "partial_segment") + ) + partial_index += 1 + intervals.extend(complete) + if stamps[-1] - complete[-1].end >= min_partial_duration_s: + intervals.append( + Interval(partial_index, complete[-1].end, float(stamps[-1]), False, "partial_segment") + ) + confidence = "high" if len(complete) >= 2 and cv <= 0.20 else "medium" + return intervals, { + "method": "automatic repeated same-direction reference crossing", + "confidence": confidence, + "anchor_xy_m": [float(anchor[0]), float(anchor[1])], + "tangent_xy": [float(tangent[0]), float(tangent[1])], + "crossing_times": crossings, + "interval_cv": cv, + "note": "The injected reference trajectory is used after replay to segment laps.", + } + + +def make_plot( + map_points: np.ndarray, + map_total: int, + official: np.ndarray, + reference: np.ndarray, + resets: np.ndarray, + output: Path, + config: PlotConfig, +): + x_all = np.concatenate((official[:, 1], reference[:, 1])) + y_all = np.concatenate((official[:, 2], reference[:, 2])) + margin = config.margin_m + x_min, x_max = float(x_all.min() - margin), float(x_all.max() + margin) + y_min, y_max = float(y_all.min() - margin), float(y_all.max() + margin) + mask = ( + (map_points[:, 0] >= x_min) + & (map_points[:, 0] <= x_max) + & (map_points[:, 1] >= y_min) + & (map_points[:, 1] <= y_max) + ) + visible_map = map_points[mask] + + reference_t = reference[:, 0] + valid = (official[:, 0] >= reference_t[0]) & (official[:, 0] <= reference_t[-1]) + official_scored = official[valid] + if not len(official_scored): + raise RuntimeError("localization and reference trajectories do not overlap in time") + reference_x = np.interp(official_scored[:, 0], reference_t, reference[:, 1]) + reference_y = np.interp(official_scored[:, 0], reference_t, reference[:, 2]) + errors = np.hypot( + official_scored[:, 1] - reference_x, + official_scored[:, 2] - reference_y, + ) + + figure, axis = plt.subplots(figsize=(16, 11), constrained_layout=True) + if len(visible_map): + axis.scatter( + visible_map[:, 0], + visible_map[:, 1], + s=0.12, + c="#7c8794", + alpha=0.20, + linewidths=0, + rasterized=True, + label=f"{config.map_label} (sampled {len(visible_map):,})", + ) + axis.plot( + reference[:, 1], + reference[:, 2], + color="#d62728", + linewidth=2.0, + alpha=0.90, + label=config.reference_label, + ) + axis.plot( + official[:, 1], + official[:, 2], + color="#12a33a", + linewidth=1.15, + alpha=0.94, + label=config.trajectory_label, + ) + if len(resets): + axis.scatter( + resets[:, 1], + resets[:, 2], + marker="X", + s=70, + c="#ffd21f", + edgecolors="#222222", + linewidths=0.8, + zorder=8, + label=f"absolute resets ({len(resets)})", + ) + axis.scatter( + [official[0, 1]], + [official[0, 2]], + marker="o", + s=72, + facecolors="white", + edgecolors="#111111", + linewidths=1.3, + zorder=9, + label="start", + ) + axis.scatter( + [official[-1, 1]], + [official[-1, 2]], + marker="s", + s=64, + facecolors="#4f7cff", + edgecolors="#111111", + linewidths=1.0, + zorder=9, + label="end", + ) + axis.set_title( + f"{config.run_label} — {config.trajectory_label} on {config.map_label}\n" + f"full replay {official[-1, 0] - official[0, 0]:.1f}s | " + f"reference XY error median {percentile(errors, 50):.2f}m, " + f"P95 {percentile(errors, 95):.2f}m, max {errors.max():.2f}m" + ) + axis.set_xlabel("Map X / East [m]") + axis.set_ylabel("Map Y / North [m]") + axis.set_xlim(x_min, x_max) + axis.set_ylim(y_min, y_max) + axis.set_aspect("equal", adjustable="box") + axis.grid(True, alpha=0.22) + axis.legend(loc="best", framealpha=0.93) + axis.text( + 0.008, + 0.008, + f"PCD total points: {map_total:,} | reference transform: " + f"yaw {config.reference_yaw_deg:g} deg, offset " + f"({config.reference_offset[0]:+.4f}, {config.reference_offset[1]:+.4f}, " + f"{config.reference_offset[2]:+.4f}) m", + transform=axis.transAxes, + fontsize=8.5, + color="#333333", + ) + figure.savefig(output, dpi=config.dpi) + plt.close(figure) + return errors, len(visible_map) + + +def make_interval_plot( + map_points: np.ndarray, + map_total: int, + official: np.ndarray, + reference: np.ndarray, + resets: np.ndarray, + interval: Interval, + global_bounds: tuple[float, float, float, float], + output: Path, + config: PlotConfig, +): + include_end = interval.end >= reference[-1, 0] - 1e-6 + official_mask = (official[:, 0] >= interval.start) & ( + official[:, 0] <= interval.end if include_end else official[:, 0] < interval.end + ) + reference_mask = (reference[:, 0] >= interval.start) & ( + reference[:, 0] <= interval.end + if include_end + else reference[:, 0] < interval.end + ) + reset_mask = (resets[:, 0] >= interval.start) & (resets[:, 0] < interval.end) + official_segment = official[official_mask] + reference_segment = reference[reference_mask] + reset_segment = resets[reset_mask] + if len(official_segment) < 2 or len(reference_segment) < 2: + raise RuntimeError(f"interval has insufficient samples: {interval}") + + reference_x = np.interp( + official_segment[:, 0], + reference_segment[:, 0], + reference_segment[:, 1], + ) + reference_y = np.interp( + official_segment[:, 0], + reference_segment[:, 0], + reference_segment[:, 2], + ) + errors = np.hypot( + official_segment[:, 1] - reference_x, + official_segment[:, 2] - reference_y, + ) + x_min, x_max, y_min, y_max = global_bounds + map_mask = ( + (map_points[:, 0] >= x_min) + & (map_points[:, 0] <= x_max) + & (map_points[:, 1] >= y_min) + & (map_points[:, 1] <= y_max) + ) + visible_map = map_points[map_mask] + label = f"Lap {interval.index:03d}" if interval.complete else f"Partial {interval.index:03d}" + + figure, axis = plt.subplots(figsize=(16, 11), constrained_layout=True) + axis.scatter( + visible_map[:, 0], + visible_map[:, 1], + s=0.12, + c="#7c8794", + alpha=0.20, + linewidths=0, + rasterized=True, + label=f"{config.map_label} (sampled {len(visible_map):,})", + ) + axis.plot( + reference_segment[:, 1], + reference_segment[:, 2], + color="#d62728", + linewidth=2.0, + alpha=0.90, + label=config.reference_label, + ) + axis.plot( + official_segment[:, 1], + official_segment[:, 2], + color="#12a33a", + linewidth=1.35, + alpha=0.95, + label=config.trajectory_label, + ) + if len(reset_segment): + axis.scatter( + reset_segment[:, 1], + reset_segment[:, 2], + marker="X", + s=74, + c="#ffd21f", + edgecolors="#222222", + linewidths=0.8, + zorder=8, + label=f"absolute resets ({len(reset_segment)})", + ) + axis.scatter( + [official_segment[0, 1]], + [official_segment[0, 2]], + marker="o", + s=72, + facecolors="white", + edgecolors="#111111", + linewidths=1.3, + zorder=9, + label="lap start", + ) + axis.scatter( + [official_segment[-1, 1]], + [official_segment[-1, 2]], + marker="s", + s=64, + facecolors="#4f7cff", + edgecolors="#111111", + linewidths=1.0, + zorder=9, + label="lap end", + ) + axis.set_title( + f"{config.run_label} — {label} top-down on {config.map_label}\n" + f"duration {interval.end - interval.start:.1f}s | samples {len(official_segment):,} | " + f"reference XY median {percentile(errors, 50):.2f}m, " + f"P95 {percentile(errors, 95):.2f}m, max {errors.max():.2f}m" + ) + axis.set_xlabel("Map X / East [m]") + axis.set_ylabel("Map Y / North [m]") + axis.set_xlim(x_min, x_max) + axis.set_ylim(y_min, y_max) + axis.set_aspect("equal", adjustable="box") + axis.grid(True, alpha=0.22) + axis.legend(loc="best", framealpha=0.93) + axis.text( + 0.008, + 0.008, + f"PCD total points: {map_total:,} | interval [{interval.start:.3f}, {interval.end:.3f}]", + transform=axis.transAxes, + fontsize=8.5, + color="#333333", + ) + figure.savefig(output, dpi=config.dpi) + plt.close(figure) + return { + "index": interval.index, + "complete": interval.complete, + "kind": interval.kind, + "start": interval.start, + "end": interval.end, + "duration_s": interval.end - interval.start, + "sample_count": len(official_segment), + "reset_count": len(reset_segment), + "reference_xy_error_m": { + "median": percentile(errors, 50), + "p90": percentile(errors, 90), + "p95": percentile(errors, 95), + "p99": percentile(errors, 99), + "max": float(errors.max()), + }, + "image": output.name, + "image_sha256": sha256(output), + } + + +def main() -> int: + args = arguments() + offset = tuple(args.reference_offset) + if args.map_sample_count <= 0: + raise RuntimeError("--map-sample-count must be positive") + if args.plot_margin_m < 0.0: + raise RuntimeError("--plot-margin-m must be non-negative") + if args.dpi <= 0: + raise RuntimeError("--dpi must be positive") + if args.min_lap_duration_s <= 0.0: + raise RuntimeError("--min-lap-duration-s must be positive") + if args.max_lap_duration_s <= args.min_lap_duration_s: + raise RuntimeError("--max-lap-duration-s must exceed --min-lap-duration-s") + + config = PlotConfig( + run_label=args.run_label, + map_label=args.map_label, + trajectory_label=args.trajectory_label, + reference_label=args.reference_label, + reference_offset=offset, + reference_yaw_deg=args.reference_yaw_deg, + margin_m=args.plot_margin_m, + dpi=args.dpi, + ) + args.output_dir.mkdir(parents=True, exist_ok=True) + + finals, guesses = read_debug_poses( + args.debug_bag, + args.final_pose_topic, + args.initial_guess_topic, + ) + reference = read_reference( + args.reference_bag, + args.reference_topic, + offset, + args.reference_yaw_deg, + ) + statuses, resets = parse_log(args.localization_log) + official, rejected, matched = official_trajectory(finals, guesses, statuses) + map_points, map_total = pcd_xyz_sample( + args.map_path, + args.map_sample_count, + args.map_sample_seed, + ) + + official_array = np.asarray(official, dtype=np.float64) + reference_array = np.asarray(reference, dtype=np.float64) + resets_array = np.asarray(resets, dtype=np.float64).reshape((-1, 4)) + if len(official_array) < 2: + raise RuntimeError("localization trajectory must contain at least two poses") + if len(reference_array) < 2: + raise RuntimeError("reference trajectory must contain at least two poses") + + image = args.output_dir / "full_topdown.png" + errors, visible_map_count = make_plot( + map_points, + map_total, + official_array, + reference_array, + resets_array, + image, + config, + ) + x_all = np.concatenate((official_array[:, 1], reference_array[:, 1])) + y_all = np.concatenate((official_array[:, 2], reference_array[:, 2])) + global_bounds = ( + float(x_all.min() - config.margin_m), + float(x_all.max() + config.margin_m), + float(y_all.min() - config.margin_m), + float(y_all.max() + config.margin_m), + ) + if args.no_lap_split: + intervals = [] + lap_detection = { + "method": "disabled by --no-lap-split", + "confidence": "n/a", + "crossing_times": [], + } + else: + intervals, lap_detection = detect_intervals( + reference_array, + min_lap_duration_s=args.min_lap_duration_s, + max_lap_duration_s=args.max_lap_duration_s, + line_half_width_m=args.start_line_half_width_m, + min_speed_mps=args.min_crossing_speed_mps, + min_partial_duration_s=args.min_partial_duration_s, + ) + lap_items = [] + for interval in intervals: + if interval.complete: + name = f"lap_{interval.index:03d}.png" + elif interval.kind == "partial_run": + name = "lap_001_partial.png" + else: + name = f"partial_{interval.index:03d}.png" + lap_items.append( + make_interval_plot( + map_points, + map_total, + official_array, + reference_array, + resets_array, + interval, + global_bounds, + args.output_dir / name, + config, + ) + ) + manifest = { + "schema_version": 2, + "run": args.run_label, + "image": image.name, + "image_sha256": sha256(image), + "inputs": { + "debug_bag": str(args.debug_bag.resolve()), + "reference_bag": str(args.reference_bag.resolve()), + "localization_log": ( + str(args.localization_log.resolve()) if args.localization_log else None + ), + "map": str(args.map_path.resolve()), + "map_total_points": map_total, + "topics": { + "final_pose": args.final_pose_topic, + "initial_guess": args.initial_guess_topic, + "reference": args.reference_topic, + }, + "reference_to_map": { + "operation": "map_xyz = Rz(yaw) * reference_xyz + offset", + "offset_xyz_m": list(offset), + "yaw_deg": args.reference_yaw_deg, + }, + }, + "parameters": { + "map_sample_count": args.map_sample_count, + "map_sample_seed": args.map_sample_seed, + "plot_margin_m": args.plot_margin_m, + "dpi": args.dpi, + "lap_split_enabled": not args.no_lap_split, + "min_lap_duration_s": args.min_lap_duration_s, + "max_lap_duration_s": args.max_lap_duration_s, + "start_line_half_width_m": args.start_line_half_width_m, + "min_crossing_speed_mps": args.min_crossing_speed_mps, + "min_partial_duration_s": args.min_partial_duration_s, + }, + "counts": { + "debug_final_pose": len(finals), + "debug_initial_guess_pose": len(guesses), + "status_rows": len(statuses), + "status_matched_poses": matched, + "rejected_poses_rendered_from_initial_guess": rejected, + "reference_samples": len(reference), + "absolute_resets": len(resets), + "sampled_map_points_visible": visible_map_count, + "complete_laps": sum(item["complete"] for item in lap_items), + "partial_segments": sum(not item["complete"] for item in lap_items), + }, + "reference_xy_error_m": { + "count": int(len(errors)), + "median": percentile(errors, 50), + "p90": percentile(errors, 90), + "p95": percentile(errors, 95), + "p99": percentile(errors, 99), + "max": float(errors.max()), + }, + "plot_contract": { + "green": ( + "localization trajectory: accepted final pose, or initial guess for a " + "rejected status when the optional log is available" + ), + "red": "injected reference topic after the injected reference-to-map transform", + "yellow_x": "absolute state resets parsed from the optional localization log", + "gray": "deterministic sample of the injected PCD map", + }, + "lap_detection": lap_detection, + "laps": lap_items, + } + manifest_path = args.output_dir / "trajectory_manifest.json" + manifest_path.write_text(json.dumps(manifest, indent=2, sort_keys=True) + "\n") + print( + json.dumps( + { + "image": str(image), + "manifest": str(manifest_path), + **manifest["counts"], + "errors": manifest["reference_xy_error_m"], + }, + indent=2, + ) + ) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/offset_odom.py b/scripts/offset_odom.py new file mode 100755 index 00000000..aaec0bd5 --- /dev/null +++ b/scripts/offset_odom.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 +"""Republish Odometry with an explicit map-frame translation. + +This small bridge is intended for replay datasets whose recorded INS odometry +uses the same ENU axes as a localization map but a different documented height +or origin convention. The translation is explicit at the command line; no +site-specific offset is embedded in the tool. +""" + +from __future__ import annotations + +import argparse +import math +from typing import Sequence + +import rclpy +from nav_msgs.msg import Odometry +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from rclpy.qos import ( + DurabilityPolicy, + HistoryPolicy, + QoSProfile, + ReliabilityPolicy, +) + + +class OdomOffsetBridge(Node): + def __init__( + self, + input_topic: str, + output_topic: str, + offset: tuple[float, float, float], + frame_id: str, + ) -> None: + super().__init__("odom_offset_bridge") + self._offset = offset + self._frame_id = frame_id + self._publisher = self.create_publisher(Odometry, output_topic, 50) + input_qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=200, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + ) + self._subscription = self.create_subscription( + Odometry, input_topic, self._callback, input_qos + ) + self._count = 0 + self.get_logger().info( + f"Republishing {input_topic} -> {output_topic}; " + f"map translation={offset}, frame_id={frame_id!r}" + ) + + def _callback(self, msg: Odometry) -> None: + msg.pose.pose.position.x += self._offset[0] + msg.pose.pose.position.y += self._offset[1] + msg.pose.pose.position.z += self._offset[2] + if self._frame_id: + msg.header.frame_id = self._frame_id + self._publisher.publish(msg) + self._count += 1 + if self._count % 10000 == 0: + self.get_logger().info(f"Published {self._count} translated samples") + + +def parse_args(argv: Sequence[str] | None = None) -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--input-topic", required=True) + parser.add_argument("--output-topic", required=True) + parser.add_argument( + "--offset", + type=float, + nargs=3, + metavar=("X", "Y", "Z"), + required=True, + help="translation added to every odometry position, in map meters", + ) + parser.add_argument( + "--frame-id", + default="map", + help="replacement header.frame_id; pass an empty string to preserve it", + ) + args = parser.parse_args(argv) + if not all(math.isfinite(value) for value in args.offset): + parser.error("--offset values must be finite") + if not args.input_topic.startswith("/") or not args.output_topic.startswith("/"): + parser.error("--input-topic and --output-topic must be absolute ROS topics") + if args.input_topic == args.output_topic: + parser.error("input and output topics must differ") + return args + + +def main(argv: Sequence[str] | None = None) -> int: + args = parse_args(argv) + rclpy.init() + node = OdomOffsetBridge( + args.input_topic, + args.output_topic, + tuple(args.offset), + args.frame_id, + ) + try: + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/run_gicp_replay_audit.sh b/scripts/run_gicp_replay_audit.sh new file mode 100755 index 00000000..fed11b5f --- /dev/null +++ b/scripts/run_gicp_replay_audit.sh @@ -0,0 +1,348 @@ +#!/usr/bin/env bash +# Run a lossless, auditable offline GICP replay. +# +# The runner is dataset-independent. It derives DATASET_ROOT from --map-dir or +# --map when possible and writes to DATASET_ROOT/gicp_result unless --out-root +# is supplied. Multiple --bag arguments are passed to ros2 bag play as inputs. +set -o pipefail +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +usage() { + printf '%s\n' \ + 'Usage: run_gicp_replay_audit.sh [options]' \ + '' \ + 'Required:' \ + ' --map FILE | --map-dir DIR ENU PCD or directory containing map.pcd' \ + ' --bag PATH rosbag2 input; repeat for multiple inputs' \ + ' --run-name NAME new result directory name' \ + ' --overlay SETUP.BASH built GICP++ overlay' \ + ' --duration SECONDS playback duration' \ + '' \ + 'Common options:' \ + ' --out-root DIR defaults to DATASET_ROOT/gicp_result' \ + ' --start-offset SECONDS default 0' \ + ' --rate RATE default 1.0' \ + ' --domain-id ID default 177' \ + ' --pointcloud-topic TOPIC default /luminar_front/points' \ + ' --imu-topic TOPIC default /gps_p1/imu' \ + ' --gt-topic TOPIC default /gps_p1/filtered_odom' \ + ' --reference-topic TOPIC defaults to --gt-topic' \ + ' --primary-queue-size N default 8' \ + ' --qos-overrides YAML optional publisher QoS override' \ + ' --play-topic TOPIC repeat to replace the default topic set' \ + ' --bridge-script FILE optional preprocessing/offset ROS node' \ + ' --bridge-arg VALUE repeat; passed literally to the bridge' +} + +MAP= +MAP_DIR= +OUT_ROOT= +RUN_NAME= +OVERLAY= +START_OFFSET=0 +DURATION= +RATE=1.0 +DOMAIN_ID=177 +STORAGE_ID=mcap +POINTCLOUD_TOPIC=/luminar_front/points +IMU_TOPIC=/gps_p1/imu +GT_TOPIC=/gps_p1/filtered_odom +REFERENCE_TOPIC= +PRIMARY_QUEUE_SIZE=8 +FUTURE_AUX_WAIT_TIMEOUT_S=0.150 +LIDAR_CONCAT_ENABLED=false +REQUIRE_ALL_AUX=false +LIDAR_RELIABLE_QOS=true +QOS_OVERRIDES= +BRIDGE_SCRIPT= +declare -a BAGS=() +declare -a BRIDGE_ARGS=() +declare -a PLAY_TOPICS=() + +while [[ $# -gt 0 ]]; do + case "$1" in + --map) MAP="${2:?missing value for --map}"; shift 2 ;; + --map-dir) MAP_DIR="${2:?missing value for --map-dir}"; shift 2 ;; + --bag) BAGS+=("${2:?missing value for --bag}"); shift 2 ;; + --out-root) OUT_ROOT="${2:?missing value for --out-root}"; shift 2 ;; + --run-name) RUN_NAME="${2:?missing value for --run-name}"; shift 2 ;; + --overlay) OVERLAY="${2:?missing value for --overlay}"; shift 2 ;; + --start-offset) START_OFFSET="${2:?missing value for --start-offset}"; shift 2 ;; + --duration) DURATION="${2:?missing value for --duration}"; shift 2 ;; + --rate) RATE="${2:?missing value for --rate}"; shift 2 ;; + --domain-id) DOMAIN_ID="${2:?missing value for --domain-id}"; shift 2 ;; + --storage-id) STORAGE_ID="${2:?missing value for --storage-id}"; shift 2 ;; + --pointcloud-topic) POINTCLOUD_TOPIC="${2:?missing value}"; shift 2 ;; + --imu-topic) IMU_TOPIC="${2:?missing value}"; shift 2 ;; + --gt-topic) GT_TOPIC="${2:?missing value}"; shift 2 ;; + --reference-topic) REFERENCE_TOPIC="${2:?missing value}"; shift 2 ;; + --primary-queue-size) PRIMARY_QUEUE_SIZE="${2:?missing value}"; shift 2 ;; + --future-aux-wait-timeout) FUTURE_AUX_WAIT_TIMEOUT_S="${2:?missing value}"; shift 2 ;; + --lidar-concat-enabled) LIDAR_CONCAT_ENABLED="${2:?missing value}"; shift 2 ;; + --require-all-aux) REQUIRE_ALL_AUX="${2:?missing value}"; shift 2 ;; + --lidar-reliable-qos) LIDAR_RELIABLE_QOS="${2:?missing value}"; shift 2 ;; + --qos-overrides) QOS_OVERRIDES="${2:?missing value}"; shift 2 ;; + --play-topic) PLAY_TOPICS+=("${2:?missing value}"); shift 2 ;; + --bridge-script) BRIDGE_SCRIPT="${2:?missing value}"; shift 2 ;; + --bridge-arg) BRIDGE_ARGS+=("${2:?missing value}"); shift 2 ;; + -h|--help) usage; exit 0 ;; + *) printf 'Unknown argument: %s\n' "$1" >&2; usage >&2; exit 2 ;; + esac +done + +if [[ -n "$MAP" && -n "$MAP_DIR" ]]; then + printf 'Pass only one of --map or --map-dir\n' >&2 + exit 2 +fi +if [[ -n "$MAP_DIR" ]]; then + MAP="${MAP_DIR%/}/map.pcd" +fi +if [[ -z "$MAP" || -z "$RUN_NAME" || -z "$OVERLAY" || -z "$DURATION" ]]; then + printf '%s\n' '--map/--map-dir, --run-name, --overlay and --duration are required' >&2 + usage >&2 + exit 2 +fi +if [[ ${#BAGS[@]} -eq 0 ]]; then + printf 'At least one --bag is required\n' >&2 + exit 2 +fi + +MAP="$(realpath -e "$MAP")" +OVERLAY="$(realpath -e "$OVERLAY")" +for index in "${!BAGS[@]}"; do + BAGS[$index]="$(realpath -e "${BAGS[$index]}")" +done +if [[ -n "$QOS_OVERRIDES" ]]; then + QOS_OVERRIDES="$(realpath -e "$QOS_OVERRIDES")" +fi +if [[ -n "$BRIDGE_SCRIPT" ]]; then + BRIDGE_SCRIPT="$(realpath -e "$BRIDGE_SCRIPT")" +fi + +if [[ "$MAP" == */maps/* ]]; then + DATASET_ROOT="${MAP%%/maps/*}" +elif [[ "$(basename "$(dirname "$MAP")")" == "maps" ]]; then + DATASET_ROOT="$(dirname "$(dirname "$MAP")")" +else + DATASET_ROOT= +fi +if [[ -z "$OUT_ROOT" ]]; then + if [[ -z "$DATASET_ROOT" ]]; then + printf 'Could not derive DATASET_ROOT from map path; pass --out-root explicitly\n' >&2 + exit 2 + fi + OUT_ROOT="$DATASET_ROOT/gicp_result" +fi +OUT_ROOT="$(realpath -m "$OUT_ROOT")" +RUN_DIR="$OUT_ROOT/$RUN_NAME" + +if [[ -e "$RUN_DIR" ]]; then + printf 'Refusing to overwrite run directory: %s\n' "$RUN_DIR" >&2 + exit 3 +fi +if [[ ! -s "$MAP" ]]; then + printf 'Map is missing or empty: %s\n' "$MAP" >&2 + exit 3 +fi +if [[ "$LIDAR_RELIABLE_QOS" == "true" && -z "$QOS_OVERRIDES" ]]; then + QOS_OVERRIDES="$SCRIPT_DIR/../GICP_plusplus/cfg/lidar_reliable_replay.yaml" +fi +if [[ "$LIDAR_RELIABLE_QOS" == "true" && ! -s "$QOS_OVERRIDES" ]]; then + printf 'Reliable replay QoS file is missing or empty: %s\n' "$QOS_OVERRIDES" >&2 + exit 3 +fi +if [[ -z "$REFERENCE_TOPIC" ]]; then + REFERENCE_TOPIC="$GT_TOPIC" +fi +if [[ ${#PLAY_TOPICS[@]} -eq 0 ]]; then + PLAY_TOPICS=( + "$POINTCLOUD_TOPIC" + /luminar_left/points + /luminar_right/points + "$IMU_TOPIC" + "$GT_TOPIC" + ) + if [[ "$REFERENCE_TOPIC" != "$GT_TOPIC" ]]; then + PLAY_TOPICS+=("$REFERENCE_TOPIC") + fi +fi + +source /opt/ros/jazzy/setup.bash +source "$OVERLAY" +set -u +export ROS_DOMAIN_ID="$DOMAIN_ID" +export ROS_LOG_DIR="$RUN_DIR/ros_logs" +mkdir -p "$RUN_DIR" "$ROS_LOG_DIR" + +bridge_pid= +launch_pid= +record_pid= +reference_record_pid= +resource_pid= + +stop_pid() { + local pid="${1:-}" + if [[ -n "$pid" ]] && kill -0 "$pid" 2>/dev/null; then + kill -INT "$pid" 2>/dev/null || true + for _ in {1..20}; do + kill -0 "$pid" 2>/dev/null || return 0 + sleep 0.25 + done + kill -TERM "$pid" 2>/dev/null || true + fi +} + +stop_launch() { + if [[ -z "$launch_pid" ]] || ! kill -0 "$launch_pid" 2>/dev/null; then + return 0 + fi + local child + while read -r child; do + [[ -n "$child" ]] && kill -INT "$child" 2>/dev/null || true + done < <(pgrep -P "$launch_pid" || true) + for _ in {1..80}; do + pgrep -P "$launch_pid" >/dev/null 2>&1 || break + sleep 0.25 + done + stop_pid "$launch_pid" +} + +cleanup() { + stop_pid "$record_pid" + stop_pid "$reference_record_pid" + stop_launch + stop_pid "$bridge_pid" + stop_pid "$resource_pid" +} +trap cleanup EXIT INT TERM + +if [[ -n "$BRIDGE_SCRIPT" ]]; then + python3 "$BRIDGE_SCRIPT" "${BRIDGE_ARGS[@]}" >"$RUN_DIR/bridge.log" 2>&1 & + bridge_pid=$! +fi + +ros2 launch gicp_plusplus localization_with_tf.launch.py \ + rviz:=false \ + map_path:="$MAP" \ + pointcloud_topic:="$POINTCLOUD_TOPIC" \ + imu_topic:="$IMU_TOPIC" \ + gt_odom_topic:="$GT_TOPIC" \ + lidar_concat_enabled:="$LIDAR_CONCAT_ENABLED" \ + require_all_aux:="$REQUIRE_ALL_AUX" \ + lidar_reliable_qos:="$LIDAR_RELIABLE_QOS" \ + future_aux_wait_timeout_s:="$FUTURE_AUX_WAIT_TIMEOUT_S" \ + primary_queue_size:="$PRIMARY_QUEUE_SIZE" \ + >"$RUN_DIR/localization.log" 2>&1 & +launch_pid=$! + +initialized=0 +for _ in {1..180}; do + if grep -q "DLIO Localization Node Initialized" "$RUN_DIR/localization.log"; then + initialized=1 + break + fi + if ! kill -0 "$launch_pid" 2>/dev/null; then + printf 'Localization launch exited before initialization\n' >&2 + exit 4 + fi + sleep 1 +done +if [[ "$initialized" -ne 1 ]]; then + printf 'Timed out waiting for localization initialization\n' >&2 + exit 5 +fi + +( + while kill -0 "$launch_pid" 2>/dev/null; do + date --iso-8601=seconds + ps -o pid,etime,%cpu,%mem,rss,stat,cmd -C gicp_plusplus_node || true + sleep 5 + done +) >"$RUN_DIR/resource.log" 2>&1 & +resource_pid=$! + +ros2 bag record --storage mcap --output "$RUN_DIR/debug_topics_bag" \ + --regex '(^/gicp/localization/debug(/.*)?$)' \ + >"$RUN_DIR/record.log" 2>&1 & +record_pid=$! + +ros2 bag record --storage mcap --output "$RUN_DIR/reference_topics_bag" \ + "$REFERENCE_TOPIC" >"$RUN_DIR/reference_record.log" 2>&1 & +reference_record_pid=$! +sleep 2 + +declare -a play_args=() +for bag in "${BAGS[@]}"; do + play_args+=(-i "$bag" "$STORAGE_ID") +done +play_args+=( + --rate "$RATE" + --start-offset "$START_OFFSET" + --playback-duration "$DURATION" + --clock-topics "$POINTCLOUD_TOPIC" + --disable-keyboard-controls + --topics +) +play_args+=("${PLAY_TOPICS[@]}") +if [[ "$LIDAR_RELIABLE_QOS" == "true" ]]; then + play_args+=(--qos-profile-overrides-path "$QOS_OVERRIDES") +fi + +play_start_ns="$(date +%s%N)" +ros2 bag play "${play_args[@]}" >"$RUN_DIR/playback.log" 2>&1 +playback_exit=$? +play_end_ns="$(date +%s%N)" + +sleep 3 +launch_alive=0 +if kill -0 "$launch_pid" 2>/dev/null; then + launch_alive=1 +fi + +stop_launch +launch_pid= +sleep 2 +stop_pid "$record_pid" +record_pid= +stop_pid "$reference_record_pid" +reference_record_pid= +stop_pid "$bridge_pid" +bridge_pid= +stop_pid "$resource_pid" +resource_pid= + +play_wall_s="$(awk -v start="$play_start_ns" -v end="$play_end_ns" \ + 'BEGIN { printf "%.6f", (end-start)/1000000000.0 }')" +{ + printf 'playback_exit=%s\n' "$playback_exit" + printf 'localization_alive_after_playback=%s\n' "$launch_alive" + printf 'completed_utc=%s\n' "$(date --utc --iso-8601=seconds)" + printf 'dataset_root=%s\n' "$DATASET_ROOT" + printf 'ros_domain_id=%s\n' "$ROS_DOMAIN_ID" + printf 'map=%s\n' "$MAP" + printf 'map_bytes=%s\n' "$(stat -c %s "$MAP")" + printf 'map_sha256=%s\n' "$(sha256sum "$MAP" | awk '{print $1}')" + printf 'bags=%s\n' "${BAGS[*]}" + printf 'start_offset_s=%s\n' "$START_OFFSET" + printf 'playback_duration_s=%s\n' "$DURATION" + printf 'playback_rate=%s\n' "$RATE" + printf 'playback_wall_s=%s\n' "$play_wall_s" + printf 'lidar_concat_enabled=%s\n' "$LIDAR_CONCAT_ENABLED" + printf 'require_all_aux=%s\n' "$REQUIRE_ALL_AUX" + printf 'lidar_reliable_qos=%s\n' "$LIDAR_RELIABLE_QOS" + printf 'future_aux_wait_timeout_s=%s\n' "$FUTURE_AUX_WAIT_TIMEOUT_S" + printf 'primary_queue_size=%s\n' "$PRIMARY_QUEUE_SIZE" + printf 'pointcloud_topic=%s\n' "$POINTCLOUD_TOPIC" + printf 'imu_topic=%s\n' "$IMU_TOPIC" + printf 'gt_topic=%s\n' "$GT_TOPIC" + printf 'reference_topic=%s\n' "$REFERENCE_TOPIC" +} >"$RUN_DIR/run_status.env" + +python3 "$SCRIPT_DIR/../GICP_plusplus/scripts/analyze_scan_debug_log.py" \ + "$RUN_DIR/localization.log" \ + >"$RUN_DIR/scan_debug_scorecard.md" \ + 2>"$RUN_DIR/scan_debug_scorecard.err" || true + +if [[ "$playback_exit" -ne 0 || "$launch_alive" -ne 1 ]]; then + exit 6 +fi From 6680c93ced75efbb0a169ced82fae55f38086c4b Mon Sep 17 00:00:00 2001 From: FieldDiTian Date: Tue, 28 Jul 2026 04:17:54 -0700 Subject: [PATCH 3/4] Raise compressed-map GICP quality at 1x --- GICP_plusplus/README.md | 20 ++ GICP_plusplus/cfg/front_quality_replay.yaml | 20 ++ GICP_plusplus/cfg/localization.yaml | 11 + .../include/gicp_plusplus/localization.h | 3 + .../gicp_plusplus/small_gicp_backend.hpp | 82 +++++- .../launch/localization_with_tf.launch.py | 23 +- .../scripts/analyze_scan_debug_log.py | 6 +- GICP_plusplus/src/localization.cc | 72 ++++- README.md | 37 ++- scripts/offset_odom.py | 6 + scripts/prepare_gicp_replay_bag.py | 247 ++++++++++++++++++ scripts/run_gicp_replay_audit.sh | 28 +- 12 files changed, 534 insertions(+), 21 deletions(-) create mode 100644 GICP_plusplus/cfg/front_quality_replay.yaml create mode 100755 scripts/prepare_gicp_replay_bag.py diff --git a/GICP_plusplus/README.md b/GICP_plusplus/README.md index 03b30373..ba817e13 100644 --- a/GICP_plusplus/README.md +++ b/GICP_plusplus/README.md @@ -120,6 +120,8 @@ ros2 launch gicp_plusplus localization_with_tf.launch.py \ | `gt_odom_topic` | `/gps_p1/filtered_odom` | Atlas FusionEngine INS odometry, at `gps_antenna_top`. Used when `localization/gt_odom/enable=true` and/or `gt_recovery/enable=true`. Same frame as `base_frame`, so no TF correction is needed. | | `imu_only` | `false` | Disable GICP and propagate pose from IMU only (debug/sanity check). | | `lidar_concat_enabled` | `false` | Opt in to front+left+right online GICP for synchronization/diagnostic A/B tests. Production uses a three-LiDAR offline map with front-only online GICP to meet 10 Hz. | +| `primary_queue_size` | `8` | Bounded front compute queue. Keep 8 for live operation. A lossless offline replay may use a larger bounded queue to absorb rosbag delivery bursts, but must separately prove sub-100 ms scan compute and zero overload drops. | +| `config_path` | empty | Optional run-local YAML loaded after the package default, used for reproducible profiles such as `cfg/front_quality_replay.yaml`. | | `urdf_path` | (auto-found) | Path to the URDF (`av24.urdf`) used for offline extrinsic resolution. The launch resolves it by walking up from the launch dir; `av24.urdf` is also installed into `share/gicp_plusplus`. | | `parent_frame` / `child_frame` | `base_link` / `luminar_front` | `child_frame` overrides `localization/lidar_frame` (the LiDAR link the node resolves extrinsics for); `parent_frame` is declared but currently unused (no static-TF helper is launched — `robot_state_publisher` provides the URDF tree). | | `map_path` | (yaml) | Override the yaml `localization/map_path` from the command line. | @@ -320,6 +322,24 @@ without assuming a particular map's fitness scale. Score a replay with `scripts/analyze_scan_debug_log.py`; it reports accepted fitness and support so optional ratio thresholds can still be evaluated in an explicit A/B. +### Compressed-map quality profile + +`cfg/front_quality_replay.yaml` is the checked-in Laguna compressed-map +profile used through the launch file's `config_path` argument. It leaves the +production motion chain enabled, uses 0.25 m target and 0.30 m source voxels +with a 100 m sensor-frame crop, 32 iterations, and an 80 ms optimizer budget. +Atlas translation seeds only the GICP optimizer; it never modifies +`basePose`, observer state, or published output. Every candidate must still +pass correspondence, physical-jump, and the unchanged 5 m Atlas wrong-basin +gate. + +Use the profile with the topic-reduced replay bag and the repository audit +runner documented in the root workflow. A rate pass requires 1.0x playback, +zero front overload drops, and measured scan-compute latency below the 10 Hz +deadline. The live-car queue remains 8; a lossless offline audit may use a +larger bounded queue only to absorb rosbag delivery bursts, and must report +that queue separately. + ### Multi-LiDAR concatenation 3x Luminar: `luminar_front` primary + `luminar_right`/`luminar_left` merged. diff --git a/GICP_plusplus/cfg/front_quality_replay.yaml b/GICP_plusplus/cfg/front_quality_replay.yaml new file mode 100644 index 00000000..e78002c7 --- /dev/null +++ b/GICP_plusplus/cfg/front_quality_replay.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + # High-information front-only localization profile. Atlas translation is + # used only to place the GICP optimizer in the correct local basin; it is + # never blended into basePose, observer state, or published output. + localization/map_voxel_size: 0.25 + dlio/preprocessing/cropBoxFilter/size: 100.0 + dlio/preprocessing/voxelFilter/use: true + dlio/preprocessing/voxelFilter/res: 0.30 + gicp/maxIterations: 32 + gicp/maxOptimizationTimeMs: 80.0 + + # Keep the deployed Laguna motion-prediction chain. + dlio/deskew: true + localization/ins_prior/enable: true + localization/ins_prior/gicp_position_seed_blend: 1.0 + localization/ins_prior/gicp_position_seed_max_step_m: 20.0 + + # Fail closed on a map match outside the quality-gated Atlas envelope. + localization/gt_odom/max_candidate_position_error_m: 5.0 diff --git a/GICP_plusplus/cfg/localization.yaml b/GICP_plusplus/cfg/localization.yaml index ff5f6864..f68d1b21 100644 --- a/GICP_plusplus/cfg/localization.yaml +++ b/GICP_plusplus/cfg/localization.yaml @@ -527,6 +527,11 @@ # Match the deployed perception-ws Laguna localizer. Fifty iterations # leaves ample convergence margin while bounding the 10 Hz CPU deadline. gicp/maxIterations: 50 + # Optional iterative-optimizer wall-clock budget in milliseconds. Zero + # preserves the historical unbounded behavior; replay/site profiles may + # set a finite budget so a pathological basin fails closed at the INS + # prior instead of blocking the sensor pipeline. + gicp/maxOptimizationTimeMs: 0.0 # Number of neighbors used to compute per-point covariances (typical: 20) gicp/correspondenceRandomness: 10 @@ -717,6 +722,12 @@ # keeping scan-time and IMU-rate output consistent. # Numeric ins_prior params are sanitized at load: # out-of-range values warn and fall back to defaults. + # Atlas translation may optionally seed the GICP optimizer without + # shifting basePose/state/output. The point-cloud result must still pass + # support, jump, and wrong-basin gates. This keeps Atlas in the + # initialization/validation role instead of blending it into localization. + localization/ins_prior/gicp_position_seed_blend: 0.0 + localization/ins_prior/gicp_position_seed_max_step_m: 20.0 localization/ins_prior/require_rtk_fixed: true # consume RTK-quality samples only # Heading-quality gate (REVIEW FIX 2026-07-08), mirroring GLIM gnss_global's # orientation_prior_max_yaw_sigma_deg. require_rtk_fixed above gates on diff --git a/GICP_plusplus/include/gicp_plusplus/localization.h b/GICP_plusplus/include/gicp_plusplus/localization.h index a0cfe3fe..6a05b3f4 100644 --- a/GICP_plusplus/include/gicp_plusplus/localization.h +++ b/GICP_plusplus/include/gicp_plusplus/localization.h @@ -740,6 +740,7 @@ class LocalizationNode : public rclcpp::Node { // GICP parameters int gicp_max_iter_; + double gicp_max_optimization_time_ms_; int gicp_corr_randomness_; double gicp_max_corr_dist_; double gicp_transformation_epsilon_; @@ -787,6 +788,8 @@ class LocalizationNode : public rclcpp::Node { double ins_prior_max_yaw_step_deg_; // hard cap on the per-scan yaw correction double ins_prior_sanity_max_yaw_deg_; // above this, warn and do NOT apply (frame/INS fault) double ins_prior_pos_blend_; // optional position pull toward INS (0 = off) + double ins_prior_gicp_position_seed_blend_; // Atlas translation used only as GICP initial guess + double ins_prior_gicp_position_seed_max_step_m_; // cap on that initial-guess translation bool ins_prior_require_rtk_; // only consume RTK-quality samples double ins_prior_max_yaw_sigma_deg_; // heading-quality gate on sqrt(cov[35]); <=0 disables double last_ins_yaw_diff_deg_ = std::numeric_limits::quiet_NaN(); // diagnostic diff --git a/GICP_plusplus/include/gicp_plusplus/small_gicp_backend.hpp b/GICP_plusplus/include/gicp_plusplus/small_gicp_backend.hpp index dd39afab..fa5c36e0 100644 --- a/GICP_plusplus/include/gicp_plusplus/small_gicp_backend.hpp +++ b/GICP_plusplus/include/gicp_plusplus/small_gicp_backend.hpp @@ -2,6 +2,7 @@ #define GICP_PLUSPLUS_SMALL_GICP_BACKEND_HPP #include +#include #include #include #include @@ -113,6 +114,8 @@ struct PriorAwareLevenbergMarquardtOptimizer { : verbose(false), max_iterations(20), max_inner_iterations(10), + max_time_ms(0.0), + timeout_flag(nullptr), init_lambda(1e-3), lambda_factor(10.0) {} @@ -141,14 +144,50 @@ struct PriorAwareLevenbergMarquardtOptimizer { double lambda = init_lambda; small_gicp::RegistrationResult result(init_T); + if (timeout_flag) { + *timeout_flag = false; + } + const auto start = std::chrono::steady_clock::now(); + const auto deadline_exceeded = [&]() { + if (max_time_ms <= 0.0) { + return false; + } + const double elapsed_ms = + std::chrono::duration( + std::chrono::steady_clock::now() - start) + .count(); + return elapsed_ms >= max_time_ms; + }; + const auto mark_timeout = [&]() { + if (timeout_flag) { + *timeout_flag = true; + } + }; + for (int i = 0; i < max_iterations && !result.converged; ++i) { + if (deadline_exceeded()) { + mark_timeout(); + break; + } auto [H, b, e] = reduction.linearize( target, source, target_tree, rejector, result.T_target_source, factors); + result.iterations = static_cast(i); + result.H = H; + result.b = b; + result.error = e; + if (deadline_exceeded()) { + mark_timeout(); + break; + } general_factor.update_linearized_system( target, source, target_tree, result.T_target_source, &H, &b, &e); bool success = false; for (int j = 0; j < max_inner_iterations; ++j) { + if (deadline_exceeded()) { + mark_timeout(); + break; + } const Eigen::Matrix delta = (H + lambda * Eigen::Matrix::Identity()).ldlt().solve(-b); const Eigen::Isometry3d new_T = result.T_target_source * small_gicp::se3_exp(delta); @@ -177,6 +216,9 @@ struct PriorAwareLevenbergMarquardtOptimizer { lambda *= lambda_factor; } + if (timeout_flag && *timeout_flag) { + break; + } result.iterations = static_cast(i); result.H = H; result.b = b; @@ -203,12 +245,19 @@ struct PriorAwareLevenbergMarquardtOptimizer { // stays pure point residual, so fitness (= error / num_inliers) measures // map agreement, not prior disagreement. The augmented system exists only // inside the LM iterations above. - { + if (deadline_exceeded()) { + mark_timeout(); + } + if (!(timeout_flag && *timeout_flag)) { auto [H_final, b_final, e_final] = reduction.linearize( target, source, target_tree, rejector, result.T_target_source, factors); - result.H = H_final; - result.b = b_final; - result.error = e_final; + if (deadline_exceeded()) { + mark_timeout(); + } else { + result.H = H_final; + result.b = b_final; + result.error = e_final; + } } result.num_inliers = static_cast(std::count_if( @@ -219,6 +268,8 @@ struct PriorAwareLevenbergMarquardtOptimizer { bool verbose; int max_iterations; int max_inner_iterations; + double max_time_ms; + bool* timeout_flag; double init_lambda; double lambda_factor; }; @@ -236,9 +287,11 @@ class SmallGicpBackend { k_correspondences_(20), max_corr_dist_(1.0), max_iterations_(20), + max_optimization_time_ms_(0.0), transformation_epsilon_(1e-3), rotation_epsilon_(0.1 * 3.14159265358979323846 / 180.0), debug_print_(false), + timed_out_(false), has_rotation_prior_(false), final_transformation_(Eigen::Matrix4f::Identity()), final_fitness_(std::numeric_limits::infinity()), @@ -252,6 +305,9 @@ class SmallGicpBackend { void setCorrespondenceRandomness(int k) { k_correspondences_ = std::max(5, k); } void setMaxCorrespondenceDistance(double corr) { max_corr_dist_ = std::max(0.0, corr); } void setMaximumIterations(int iter) { max_iterations_ = std::max(1, iter); } + void setMaximumOptimizationTimeMs(double time_ms) { + max_optimization_time_ms_ = std::max(0.0, time_ms); + } void setTransformationEpsilon(double eps) { transformation_epsilon_ = std::max(0.0, eps); } void setRotationEpsilon(double eps) { rotation_epsilon_ = std::max(0.0, eps); } void setDebugPrint(bool enabled) { debug_print_ = enabled; } @@ -354,6 +410,7 @@ class SmallGicpBackend { final_fitness_ = std::numeric_limits::infinity(); final_error_ = std::numeric_limits::infinity(); num_correspondences = 0; + timed_out_ = false; result_ = small_gicp::RegistrationResult(Eigen::Isometry3d(guess.cast())); if (!target_ || target_->empty() || !target_tree_ || !input_ || input_->empty()) { @@ -397,11 +454,25 @@ class SmallGicpBackend { registration.rejector.max_dist_sq = max_corr_dist_ * max_corr_dist_; registration.optimizer.verbose = debug_print_; registration.optimizer.max_iterations = max_iterations_; + registration.optimizer.max_time_ms = max_optimization_time_ms_; + registration.optimizer.timeout_flag = &timed_out_; registration.general_factor = general_factor; result_ = registration.align( target_proxy, source_proxy, *target_tree_, Eigen::Isometry3d(guess.cast())); + if (timed_out_) { + result_ = small_gicp::RegistrationResult( + Eigen::Isometry3d(guess.cast())); + converged_ = false; + final_transformation_ = guess; + final_error_ = std::numeric_limits::infinity(); + num_correspondences = 0; + final_fitness_ = std::numeric_limits::infinity(); + output.clear(); + return; + } + converged_ = result_.converged; final_transformation_ = result_.T_target_source.matrix().cast(); final_error_ = result_.error; @@ -429,6 +500,7 @@ class SmallGicpBackend { double getFinalError() const { return final_error_; } bool hasConverged() const { return converged_; } + bool hasTimedOut() const { return timed_out_; } const Eigen::Matrix& getFinalHessian() const { return result_.H; } Eigen::Matrix4f getFinalTransformation() const { return final_transformation_; } const small_gicp::RegistrationResult& getRegistrationResult() const { return result_; } @@ -440,9 +512,11 @@ class SmallGicpBackend { int k_correspondences_; double max_corr_dist_; int max_iterations_; + double max_optimization_time_ms_; double transformation_epsilon_; double rotation_epsilon_; bool debug_print_; + bool timed_out_; PointCloudSourceConstPtr input_; PointCloudTargetConstPtr target_; diff --git a/GICP_plusplus/launch/localization_with_tf.launch.py b/GICP_plusplus/launch/localization_with_tf.launch.py index bf91af06..435db845 100644 --- a/GICP_plusplus/launch/localization_with_tf.launch.py +++ b/GICP_plusplus/launch/localization_with_tf.launch.py @@ -50,6 +50,7 @@ def generate_launch_description(): future_aux_wait_timeout_s = LaunchConfiguration( 'future_aux_wait_timeout_s', default='0.150') primary_queue_size = LaunchConfiguration('primary_queue_size', default='8') + config_path = LaunchConfiguration('config_path', default='') urdf_path = LaunchConfiguration( 'urdf_path', default='') @@ -100,8 +101,14 @@ def generate_launch_description(): 'Front-only releases immediately; keep 0.150 s online for concat.') declare_primary_queue_size_arg = DeclareLaunchArgument( 'primary_queue_size', default_value=primary_queue_size, - description='Bounded pending-primary queue. Keep 8 online; a lossless slowed ' - 'offline audit may use a deeper queue.') + description='Bounded pending-primary compute queue. Keep 8 live; a lossless ' + 'offline replay may use a larger bounded queue for rosbag bursts ' + 'while separately auditing scan latency and overload drops.') + declare_config_path_arg = DeclareLaunchArgument( + 'config_path', default_value=config_path, + description='Optional run-local YAML loaded after the package default. ' + 'Use this for reproducible quality profiles without editing ' + 'the installed localization.yaml.') declare_urdf_path_arg = DeclareLaunchArgument( 'urdf_path', default_value=urdf_path, description='Absolute path to the vehicle URDF used by robot_state_publisher ' @@ -158,6 +165,7 @@ def make_robot_state_publisher(context): # GICP Localization Node def make_localization_node(context): map_path_value = LaunchConfiguration('map_path').perform(context).strip() + config_path_value = LaunchConfiguration('config_path').perform(context).strip() child_frame_value = LaunchConfiguration('child_frame').perform(context).strip() # Same av24.urdf the robot_state_publisher uses: hand the localization node # the resolved ABSOLUTE path so lidar_concat resolves aux extrinsics from the @@ -165,6 +173,14 @@ def make_localization_node(context): urdf_file = resolve_urdf_path(context) params = [ localization_yaml_path, + ] + if config_path_value: + config_path_value = os.path.realpath(config_path_value) + if not os.path.isfile(config_path_value): + raise RuntimeError( + f"Run-local GICP config not found at '{config_path_value}'.") + params.append(config_path_value) + params.extend([ {'localization/lidar_frame': child_frame_value}, {'localization/imu_only': LaunchConfiguration('imu_only')}, {'localization/lidar_concat/enabled': @@ -177,7 +193,7 @@ def make_localization_node(context): {'localization/lidar_concat/primary_queue_size': LaunchConfiguration('primary_queue_size')}, {'localization/lidar_concat/urdf_path': urdf_file}, - ] + ]) if map_path_value: params.append({'localization/map_path': map_path_value}) @@ -241,6 +257,7 @@ def make_rviz_node(context): declare_lidar_reliable_qos_arg, declare_future_aux_wait_timeout_arg, declare_primary_queue_size_arg, + declare_config_path_arg, declare_urdf_path_arg, declare_parent_frame_arg, declare_child_frame_arg, diff --git a/GICP_plusplus/scripts/analyze_scan_debug_log.py b/GICP_plusplus/scripts/analyze_scan_debug_log.py index e7845c4b..78e688f9 100644 --- a/GICP_plusplus/scripts/analyze_scan_debug_log.py +++ b/GICP_plusplus/scripts/analyze_scan_debug_log.py @@ -41,14 +41,14 @@ def fmt(v, nd=3): r"SCAN DEBUG \| status=(?P\w+) stamp=(?P[\d.]+)" r".*?guess=\{xyz=\[(?P[-\d.,]+)\] rpy_deg=\[(?P[-\d.,]+)\]\}" r".*?gicp_ms=(?P[-\d.]+|n/a)" - r".*?fitness=(?P[-\d.eE+]+|n/a)" - r"(?:.*?fit_ratio=(?P[-\d.]+|n/a))?" + r".*?fitness=(?P[-\d.eE+]+|n/a|nan|inf)" + r"(?:.*?fit_ratio=(?P[-\d.]+|n/a|nan|inf))?" r"(?:.*?degen=\[r(?P\d+),t(?P
\d+),yaw_veto=(?P\d)(?:,rp_clamp=(?P\d))?,partial=(?P\d)\])?" r"(?:.*?yaw_innov=\[(?P-?[\d.]+|nan)deg,fin=(?P-?[\d.]+|nan)deg\])?" r"(?:.*?yaw_stiff=(?P-?[\d.]+|n/a))?" r"(?:.*?ins_dyaw=(?P-?(?:[\d.]+|nan(?:\(ind\))?))deg)?" r"(?:.*?concat=\[(?P-?\d+)/(?P\d+)(?P[^\]]*)\])?" - r".*?hessian_cond=(?P[-\d.eE+]+|n/a|inf)" + r".*?hessian_cond=(?P[-\d.eE+]+|n/a|nan|inf)" r"(?:.*?gt_err=\[(?P[\d.]+)m,(?P[\d.]+)deg)?" ) diff --git a/GICP_plusplus/src/localization.cc b/GICP_plusplus/src/localization.cc index f10ea465..56fba9ed 100644 --- a/GICP_plusplus/src/localization.cc +++ b/GICP_plusplus/src/localization.cc @@ -1199,6 +1199,7 @@ gicp_plusplus::LocalizationNode::LocalizationNode() : Node("gicp_plusplus_node") this->gicp.setCorrespondenceRandomness(this->gicp_corr_randomness_); this->gicp.setMaxCorrespondenceDistance(this->gicp_max_corr_dist_); this->gicp.setMaximumIterations(this->gicp_max_iter_); + this->gicp.setMaximumOptimizationTimeMs(this->gicp_max_optimization_time_ms_); this->gicp.setTransformationEpsilon(this->gicp_transformation_epsilon_); this->gicp.setRotationEpsilon(this->gicp_rotation_epsilon_); this->gicp.setDebugPrint(this->debug_lm_print_); @@ -1833,6 +1834,10 @@ void gicp_plusplus::LocalizationNode::getParams() { // GICP parameters this->declare_parameter("gicp/maxIterations", 32); + // Optional wall-clock budget for the iterative optimizer. Zero preserves + // the unbounded historical behavior. A timed-out solve fails closed at the + // INS prior instead of blocking LiDAR reception on a pathological basin. + this->declare_parameter("gicp/maxOptimizationTimeMs", 0.0); this->declare_parameter("gicp/correspondenceRandomness", 20); this->declare_parameter("gicp/maxCorrespondenceDistance", 1.0); this->declare_parameter("gicp/transformationEpsilon", 0.0001); @@ -1924,10 +1929,14 @@ void gicp_plusplus::LocalizationNode::getParams() { this->declare_parameter("localization/ins_prior/max_yaw_step_deg", 2.0); this->declare_parameter("localization/ins_prior/sanity_max_yaw_deg", 30.0); this->declare_parameter("localization/ins_prior/pos_blend", 0.0); + this->declare_parameter("localization/ins_prior/gicp_position_seed_blend", 0.0); + this->declare_parameter("localization/ins_prior/gicp_position_seed_max_step_m", 20.0); this->declare_parameter("localization/ins_prior/require_rtk_fixed", true); this->declare_parameter("localization/ins_prior/max_yaw_sigma_deg", 3.0); this->get_parameter("gicp/maxIterations", this->gicp_max_iter_); + this->get_parameter("gicp/maxOptimizationTimeMs", + this->gicp_max_optimization_time_ms_); this->get_parameter("gicp/correspondenceRandomness", this->gicp_corr_randomness_); this->get_parameter("gicp/maxCorrespondenceDistance", this->gicp_max_corr_dist_); this->get_parameter("gicp/transformationEpsilon", this->gicp_transformation_epsilon_); @@ -1967,6 +1976,10 @@ void gicp_plusplus::LocalizationNode::getParams() { this->get_parameter("localization/ins_prior/max_yaw_step_deg", this->ins_prior_max_yaw_step_deg_); this->get_parameter("localization/ins_prior/sanity_max_yaw_deg", this->ins_prior_sanity_max_yaw_deg_); this->get_parameter("localization/ins_prior/pos_blend", this->ins_prior_pos_blend_); + this->get_parameter("localization/ins_prior/gicp_position_seed_blend", + this->ins_prior_gicp_position_seed_blend_); + this->get_parameter("localization/ins_prior/gicp_position_seed_max_step_m", + this->ins_prior_gicp_position_seed_max_step_m_); this->get_parameter("localization/ins_prior/require_rtk_fixed", this->ins_prior_require_rtk_); this->get_parameter("localization/ins_prior/max_yaw_sigma_deg", this->ins_prior_max_yaw_sigma_deg_); // [REVIEW FIX 2026-07-08 P3] Sanitize: defaults are safe, but bad YAML values @@ -1989,13 +2002,21 @@ void gicp_plusplus::LocalizationNode::getParams() { sanitize("max_yaw_step_deg", this->ins_prior_max_yaw_step_deg_, 0.0, 90.0, 2.0); sanitize("sanity_max_yaw_deg", this->ins_prior_sanity_max_yaw_deg_, 1e-3, 180.0, 30.0); sanitize("pos_blend", this->ins_prior_pos_blend_, 0.0, 1.0, 0.0); + sanitize("gicp_position_seed_blend", + this->ins_prior_gicp_position_seed_blend_, 0.0, 1.0, 0.0); + sanitize("gicp_position_seed_max_step_m", + this->ins_prior_gicp_position_seed_max_step_m_, 0.0, 1000.0, 20.0); } RCLCPP_INFO(this->get_logger(), - "INS prior: %s (yaw_blend=%.2f, max_step=%.1fdeg, sanity=%.1fdeg, pos_blend=%.2f, rtk_only=%s, max_yaw_sigma=%.1fdeg) — " + "INS prior: %s (yaw_blend=%.2f, max_step=%.1fdeg, sanity=%.1fdeg, " + "pos_blend=%.2f, gicp_pos_seed=[blend=%.2f,max=%.1fm], " + "rtk_only=%s, max_yaw_sigma=%.1fdeg) — " "IMU=/gps_p1/imu for propagation/deskew, filtered_odom for stable heading", this->ins_prior_enable_ ? "ENABLED" : "disabled", this->ins_prior_yaw_blend_, this->ins_prior_max_yaw_step_deg_, this->ins_prior_sanity_max_yaw_deg_, this->ins_prior_pos_blend_, + this->ins_prior_gicp_position_seed_blend_, + this->ins_prior_gicp_position_seed_max_step_m_, this->ins_prior_require_rtk_ ? "yes" : "no", this->ins_prior_max_yaw_sigma_deg_); if (this->gicp_dof_mode_ != "6dof" && this->gicp_dof_mode_ != "4dof" && this->gicp_dof_mode_ != "3dof") { RCLCPP_WARN(this->get_logger(), "gicp/dof/mode '%s' unknown; falling back to 6dof", @@ -4815,6 +4836,44 @@ void gicp_plusplus::LocalizationNode::performLocalization() { initial_guess = this->T_prior * T_base_lidar; } Eigen::Matrix4f guess_pose_map = this->T_prior; + bool gicp_position_seed_applied = false; + double gicp_position_seed_step_m = 0.0; + + // Optional Atlas translation INITIAL GUESS for GICP. This deliberately + // does not modify basePose, observer state, T_prior or the published pose: + // point-cloud registration must still produce a supported candidate and + // pass the independent wrong-basin gate. It only places the optimizer in + // the correct local basin when dead-reckoned position has drifted. + if (this->ins_prior_gicp_position_seed_blend_ > 0.0 && + this->gt_odom_enabled_ && this->gt_odom_received_.load()) { + const double seed_stamp = + (this->t_prior_stamp_ > 0.0) ? this->t_prior_stamp_ : this->scan_stamp.seconds(); + GtSample ins_seed; + if (this->getGtPoseAt(seed_stamp, ins_seed) && + (!this->ins_prior_require_rtk_ || this->gtSampleIsRtkFixed(ins_seed))) { + Eigen::Vector3f ins_p; + Eigen::Quaternionf ins_q; + if (this->composeGtPoseInBase(ins_seed, ins_p, ins_q)) { + Eigen::Vector3f seed_delta = + static_cast(this->ins_prior_gicp_position_seed_blend_) * + (ins_p - this->T_prior.block<3, 1>(0, 3)); + const double raw_step_m = static_cast(seed_delta.norm()); + if (this->ins_prior_gicp_position_seed_max_step_m_ > 0.0 && + raw_step_m > this->ins_prior_gicp_position_seed_max_step_m_) { + seed_delta *= static_cast( + this->ins_prior_gicp_position_seed_max_step_m_ / raw_step_m); + } + if (seed_delta.allFinite()) { + initial_guess.block<3, 1>(0, 3) += seed_delta; + gicp_position_seed_step_m = static_cast(seed_delta.norm()); + gicp_position_seed_applied = gicp_position_seed_step_m > 0.0; + } + } + } + } + guess_pose_map = this->scan_in_world_frame_ + ? (initial_guess * this->T_prior) + : (initial_guess * T_lidar_base); double guess_from_last_trans = 0.0; double guess_from_last_rot_deg = 0.0; @@ -4862,6 +4921,7 @@ void gicp_plusplus::LocalizationNode::performLocalization() { double elapsed_ms = std::chrono::duration_cast(end - start).count() / 1000.0; double fitness_score = this->gicp.getFitnessScore(); + const bool gicp_timed_out = this->gicp.hasTimedOut(); bool converged_precheck = this->gicp.hasConverged(); double fitness_score_final = fitness_score; if (!converged_precheck) { @@ -5425,10 +5485,13 @@ void gicp_plusplus::LocalizationNode::performLocalization() { << " raw=" << this->last_raw_point_count_ << " pre=" << this->last_preprocessed_point_count_ << " guess={" << poseSummary(guess_pose_map) << "}" + << " pos_seed=[" << (gicp_position_seed_applied ? 1 : 0) + << ",step=" << scalarSummary(gicp_position_seed_step_m) << "m]" << " guess_from_last=[" << scalarSummary(guess_from_last_trans) << "m," << scalarSummary(guess_from_last_rot_deg) << "deg]" << " gicp_ms=" << scalarSummary(elapsed_ms, 2) << " converged=" << (converged ? "true" : "false") + << " timed_out=" << (gicp_timed_out ? 1 : 0) << " fitness=" << scalarSummary(fitness_score, 6) << " fit_ratio=" << scalarSummary(fitness_ratio, 3) << " degen=[r" << degen.degen_rot_axes << ",t" << degen.degen_trans_axes @@ -5578,6 +5641,12 @@ void gicp_plusplus::LocalizationNode::performLocalization() { if (!candidate_pose_valid) { RCLCPP_WARN(this->get_logger(), "%s", build_scan_debug_log("invalid_solution").c_str()); + } else if (gicp_timed_out) { + RCLCPP_WARN( + this->get_logger(), + "GICP REJECTED (optimization wall-clock budget %.1fms exceeded): %s", + this->gicp_max_optimization_time_ms_, + build_scan_debug_log("rejected_timeout").c_str()); } else if (!effectively_converged) { RCLCPP_WARN(this->get_logger(), "%s", build_scan_debug_log("failed_to_converge").c_str()); } else if (gicp_rejected_gt_sanity) { @@ -5772,6 +5841,7 @@ void gicp_plusplus::LocalizationNode::performLocalization() { // reality, fitness gets worse, and the optimizer never recovers. ++this->consecutive_failures_; const char* reason = !candidate_pose_valid ? "invalid solution" + : gicp_timed_out ? "optimization timed out" : !effectively_converged ? "failed to converge" : gicp_rejected_gt_sanity ? "RTK candidate sanity rejected (wrong basin)" : gicp_rejected_support ? "insufficient correspondence support" diff --git a/README.md b/README.md index b82209a4..bd5b0ffe 100644 --- a/README.md +++ b/README.md @@ -290,26 +290,49 @@ If you ever switch sensors and the deskew looks wrong, use the one-shot diagnost every source range, datum, transform, and filter count. 6. **Localize** online against that PCD with `gicp_localization`/`GICP_plusplus`, using the adapter's ENU `/gps_p1/*` streams as IMU + seed. Because the exported map is genuinely ENU, Atlas seeds/GT are frame-correct directly — and `localization/utm_transform_path` must stay **EMPTY** (it exists only for legacy world-frame maps and would double-transform an ENU map). -7. **Audit the compressed map at real time** with the repository runner. It +7. **Prepare a deterministic real-time replay input.** Topic filtering at + `ros2 bag play` time still makes the player scan unrelated messages in large + camera/multi-LiDAR bags. Build one compressed MCAP containing only the + online-localization input contract before the audit: + ```bash + python3 scripts/prepare_gicp_replay_bag.py \ + --bag /path/to/DATASET_ROOT///filtered/all \ + --bag /path/to/DATASET_ROOT///navigation_bag \ + --out /path/to/DATASET_ROOT/prep_bag/_front_atlas_gicp + ``` + The helper refuses cross-dataset inputs and outputs, retains only the + `/luminar_front/points`, `/gps_p1/imu`, and `/gps_p1/filtered_odom` + streams with their full message counts, and records input/config hashes + plus `ros2 bag info`. This step + changes only the offline I/O envelope; live-car localization still consumes + those three topics directly. + +8. **Audit the compressed map at real time** with the repository runner. It derives `DATASET_ROOT` from `--map-dir`, refuses to overwrite an existing result, and writes the debug/reference bags, logs, resource samples, machine-readable run status and scan scorecard under that dataset's - `gicp_result/`: + `gicp_result/intermediate/`. Promote a run to `gicp_result/` only after + manual log, bag, status, and metric audit passes: ```bash scripts/run_gicp_replay_audit.sh \ --map-dir /path/to/DATASET_ROOT/maps/ \ - --bag /path/to/lidar-bag \ - --bag /path/to/navigation-bag \ + --bag /path/to/DATASET_ROOT/prep_bag/_front_atlas_gicp \ --run-name _compressed_full_1x \ --overlay /path/to/gicp/install/setup.bash \ + --config-path GICP_plusplus/cfg/front_quality_replay.yaml \ --start-offset 0 \ --duration \ --rate 1.0 \ - --primary-queue-size 8 + --primary-queue-size 32 ``` The offline audit uses RELIABLE LiDAR publication/subscription on both sides so a large PointCloud2 cannot disappear in DDS without accounting. - Live sensors keep the default BEST_EFFORT profile and the same queue depth. + Its 50,000-message rosbag read-ahead queue keeps storage/decompression + latency out of the 10 Hz delivery schedule. A bounded 32-frame offline + compute queue absorbs rosbag delivery bursts without hiding registration + cost: the audit must independently report GICP P95/max below 100 ms and + zero overload drops. Live sensors keep BEST_EFFORT and the default + 8-frame queue. The online localization contract remains front LiDAR only; the map itself is built from all configured LiDARs. @@ -319,7 +342,7 @@ If you ever switch sensors and the deskew looks wrong, use the one-shot diagnost `--bridge-arg` for its explicit input topic, output topic, XYZ offset and frame. The runner never embeds a site-specific transform. -8. **Render the audited result from above.** The plotting tool reads the +9. **Render the audited result from above.** The plotting tool reads the runner's two output bags and map directly, writes a full-run image plus complete-lap images, and records the exact input hashes and lap boundaries in `trajectory_manifest.json`: diff --git a/scripts/offset_odom.py b/scripts/offset_odom.py index aaec0bd5..107377d8 100755 --- a/scripts/offset_odom.py +++ b/scripts/offset_odom.py @@ -104,6 +104,12 @@ def main(argv: Sequence[str] | None = None) -> int: rclpy.spin(node) except (KeyboardInterrupt, ExternalShutdownException): pass + except Exception: + # SIGINT can invalidate the rclpy context while the executor is + # rebuilding its wait set, which Jazzy reports as RCLError rather than + # ExternalShutdownException. Suppress only that shutdown race. + if rclpy.ok(): + raise finally: node.destroy_node() if rclpy.ok(): diff --git a/scripts/prepare_gicp_replay_bag.py b/scripts/prepare_gicp_replay_bag.py new file mode 100755 index 00000000..4fdd254c --- /dev/null +++ b/scripts/prepare_gicp_replay_bag.py @@ -0,0 +1,247 @@ +#!/usr/bin/env python3 +"""Build a topic-reduced rosbag for deterministic real-time GICP audits. + +ros2 bag play still has to scan every message in each input bag even when +--topics is used. Merging only the localization contract into one compressed +MCAP removes unrelated camera/aux-LiDAR traffic from the audit I/O path. +""" + +from __future__ import annotations + +import argparse +import hashlib +import json +from pathlib import Path +import shutil +import subprocess +import sys +from typing import Iterable + + +DEFAULT_TOPICS = ( + "/luminar_front/points", + "/gps_p1/imu", + "/gps_p1/filtered_odom", +) + + +def sha256_file(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def dataset_root_from_path(path: Path) -> Path | None: + """Return .../rosbags/ when the path carries that contract.""" + parts = path.resolve().parts + for index, part in enumerate(parts[:-1]): + if part == "rosbags" and index + 1 < len(parts): + return Path(*parts[: index + 2]) + return None + + +def is_relative_to(path: Path, parent: Path) -> bool: + try: + path.relative_to(parent) + return True + except ValueError: + return False + + +def metadata_path(bag: Path) -> Path | None: + candidate = bag / "metadata.yaml" if bag.is_dir() else bag.parent / "metadata.yaml" + return candidate if candidate.is_file() else None + + +def source_record(bag: Path) -> dict[str, object]: + metadata = metadata_path(bag) + record: dict[str, object] = { + "path": str(bag), + "kind": "directory" if bag.is_dir() else "file", + } + if metadata is not None: + record["metadata_yaml"] = str(metadata) + record["metadata_sha256"] = sha256_file(metadata) + if bag.is_file(): + record["bytes"] = bag.stat().st_size + return record + + +def unique(values: Iterable[str]) -> list[str]: + result: list[str] = [] + seen: set[str] = set() + for value in values: + if value not in seen: + seen.add(value) + result.append(value) + return result + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Merge only the front-LiDAR/Atlas localization contract into one " + "compressed MCAP under DATASET_ROOT/prep_bag." + ) + ) + parser.add_argument( + "--bag", + action="append", + required=True, + help="Input rosbag2 directory/file; repeat for multiple sources.", + ) + output_group = parser.add_mutually_exclusive_group() + output_group.add_argument( + "--out", + help="Explicit output bag path, normally DATASET_ROOT/prep_bag/.", + ) + output_group.add_argument( + "--prep-root", + help="Explicit DATASET_ROOT/prep_bag; requires --name.", + ) + parser.add_argument( + "--name", + help="Output directory name. Required unless --out supplies the full path.", + ) + parser.add_argument( + "--topic", + action="append", + help="Topic to retain; repeat to replace the default front/IMU/odom set.", + ) + parser.add_argument("--storage-id", default="mcap") + parser.add_argument("--storage-preset", default="zstd_fast") + parser.add_argument( + "--dry-run", + action="store_true", + help="Validate scope and write the conversion config without converting.", + ) + return parser.parse_args() + + +def main() -> int: + args = parse_args() + bags = [Path(value).expanduser().resolve(strict=True) for value in args.bag] + + if args.out: + output = Path(args.out).expanduser().resolve(strict=False) + prep_root = output.parent + else: + if not args.name: + raise SystemExit("--name is required unless --out supplies the full path") + if "/" in args.name or args.name in {".", ".."}: + raise SystemExit("--name must be one directory component") + if args.prep_root: + prep_root = Path(args.prep_root).expanduser().resolve(strict=False) + else: + inferred_roots = [ + root for bag in bags if (root := dataset_root_from_path(bag)) is not None + ] + if not inferred_roots: + raise SystemExit( + "Could not derive DATASET_ROOT from --bag; pass --prep-root or --out" + ) + if any(root != inferred_roots[0] for root in inferred_roots[1:]): + raise SystemExit("Input bags resolve to more than one DATASET_ROOT") + prep_root = inferred_roots[0] / "prep_bag" + output = prep_root / args.name + + dataset_root = dataset_root_from_path(output) + if dataset_root is None: + raise SystemExit( + "Output does not carry a .../rosbags/ DATASET_ROOT contract" + ) + required_prep_root = dataset_root / "prep_bag" + if not is_relative_to(output, required_prep_root): + raise SystemExit( + f"Output must remain under this dataset's prep_bag: {required_prep_root}" + ) + + for bag in bags: + source_root = dataset_root_from_path(bag) + if source_root is not None and source_root != dataset_root: + raise SystemExit( + f"Cross-dataset input refused: {bag} belongs to {source_root}, " + f"output belongs to {dataset_root}" + ) + + if output.exists(): + raise SystemExit(f"Refusing to overwrite output bag: {output}") + topics = unique(args.topic or DEFAULT_TOPICS) + if not topics or any(not topic.startswith("/") for topic in topics): + raise SystemExit("Every retained topic must be an absolute ROS topic") + + prep_root.mkdir(parents=True, exist_ok=True) + config_dir = prep_root / "configs" + config_dir.mkdir(parents=True, exist_ok=True) + config_path = config_dir / f"{output.name}.convert.yaml" + if config_path.exists(): + raise SystemExit(f"Refusing to overwrite conversion config: {config_path}") + + config = { + "output_bags": [ + { + "uri": str(output), + "storage_id": args.storage_id, + "storage_preset_profile": args.storage_preset, + "topics": topics, + } + ] + } + config_path.write_text(json.dumps(config, indent=2) + "\n", encoding="utf-8") + + command = ["ros2", "bag", "convert"] + for bag in bags: + command.extend(("-i", str(bag))) + command.extend(("-o", str(config_path))) + + print(f"dataset_root={dataset_root}") + print(f"output={output}") + print(f"config={config_path}") + print("topics=" + ",".join(topics)) + print("command=" + " ".join(command)) + if args.dry_run: + return 0 + + if shutil.which("ros2") is None: + raise SystemExit("ros2 not found; source /opt/ros/jazzy/setup.bash first") + completed = subprocess.run(command, check=False) + if completed.returncode != 0: + return completed.returncode + if not output.is_dir() or not (output / "metadata.yaml").is_file(): + raise SystemExit("ros2 bag convert returned success without a readable output bag") + + bag_info = subprocess.run( + ["ros2", "bag", "info", str(output)], + check=True, + capture_output=True, + text=True, + ).stdout + (output / "bag_info.txt").write_text(bag_info, encoding="utf-8") + manifest = { + "schema": 1, + "dataset_root": str(dataset_root), + "output": str(output), + "output_metadata_sha256": sha256_file(output / "metadata.yaml"), + "conversion_config": str(config_path), + "conversion_config_sha256": sha256_file(config_path), + "storage_id": args.storage_id, + "storage_preset_profile": args.storage_preset, + "topics": topics, + "sources": [source_record(bag) for bag in bags], + "command": command, + } + (output / "preparation_manifest.json").write_text( + json.dumps(manifest, indent=2) + "\n", encoding="utf-8" + ) + print(f"metadata_sha256={manifest['output_metadata_sha256']}") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except (OSError, subprocess.SubprocessError) as error: + print(f"ERROR: {error}", file=sys.stderr) + raise SystemExit(1) diff --git a/scripts/run_gicp_replay_audit.sh b/scripts/run_gicp_replay_audit.sh index fed11b5f..83c465b6 100755 --- a/scripts/run_gicp_replay_audit.sh +++ b/scripts/run_gicp_replay_audit.sh @@ -3,7 +3,8 @@ # # The runner is dataset-independent. It derives DATASET_ROOT from --map-dir or # --map when possible and writes to DATASET_ROOT/gicp_result unless --out-root -# is supplied. Multiple --bag arguments are passed to ros2 bag play as inputs. +# is supplied. Unaudited runs default to DATASET_ROOT/gicp_result/intermediate. +# Multiple --bag arguments are passed to ros2 bag play as inputs. set -o pipefail SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" @@ -19,7 +20,7 @@ usage() { ' --duration SECONDS playback duration' \ '' \ 'Common options:' \ - ' --out-root DIR defaults to DATASET_ROOT/gicp_result' \ + ' --out-root DIR defaults to DATASET_ROOT/gicp_result/intermediate' \ ' --start-offset SECONDS default 0' \ ' --rate RATE default 1.0' \ ' --domain-id ID default 177' \ @@ -28,6 +29,8 @@ usage() { ' --gt-topic TOPIC default /gps_p1/filtered_odom' \ ' --reference-topic TOPIC defaults to --gt-topic' \ ' --primary-queue-size N default 8' \ + ' --read-ahead-queue-size N rosbag playback prefetch; default 50000' \ + ' --config-path YAML run-local overrides loaded after package defaults' \ ' --qos-overrides YAML optional publisher QoS override' \ ' --play-topic TOPIC repeat to replace the default topic set' \ ' --bridge-script FILE optional preprocessing/offset ROS node' \ @@ -49,6 +52,8 @@ IMU_TOPIC=/gps_p1/imu GT_TOPIC=/gps_p1/filtered_odom REFERENCE_TOPIC= PRIMARY_QUEUE_SIZE=8 +READ_AHEAD_QUEUE_SIZE=50000 +CONFIG_PATH= FUTURE_AUX_WAIT_TIMEOUT_S=0.150 LIDAR_CONCAT_ENABLED=false REQUIRE_ALL_AUX=false @@ -77,6 +82,8 @@ while [[ $# -gt 0 ]]; do --gt-topic) GT_TOPIC="${2:?missing value}"; shift 2 ;; --reference-topic) REFERENCE_TOPIC="${2:?missing value}"; shift 2 ;; --primary-queue-size) PRIMARY_QUEUE_SIZE="${2:?missing value}"; shift 2 ;; + --read-ahead-queue-size) READ_AHEAD_QUEUE_SIZE="${2:?missing value}"; shift 2 ;; + --config-path) CONFIG_PATH="${2:?missing value}"; shift 2 ;; --future-aux-wait-timeout) FUTURE_AUX_WAIT_TIMEOUT_S="${2:?missing value}"; shift 2 ;; --lidar-concat-enabled) LIDAR_CONCAT_ENABLED="${2:?missing value}"; shift 2 ;; --require-all-aux) REQUIRE_ALL_AUX="${2:?missing value}"; shift 2 ;; @@ -106,6 +113,11 @@ if [[ ${#BAGS[@]} -eq 0 ]]; then printf 'At least one --bag is required\n' >&2 exit 2 fi +if [[ ! "$PRIMARY_QUEUE_SIZE" =~ ^[1-9][0-9]*$ || + ! "$READ_AHEAD_QUEUE_SIZE" =~ ^[1-9][0-9]*$ ]]; then + printf 'Queue sizes must be positive integers\n' >&2 + exit 2 +fi MAP="$(realpath -e "$MAP")" OVERLAY="$(realpath -e "$OVERLAY")" @@ -118,6 +130,9 @@ fi if [[ -n "$BRIDGE_SCRIPT" ]]; then BRIDGE_SCRIPT="$(realpath -e "$BRIDGE_SCRIPT")" fi +if [[ -n "$CONFIG_PATH" ]]; then + CONFIG_PATH="$(realpath -e "$CONFIG_PATH")" +fi if [[ "$MAP" == */maps/* ]]; then DATASET_ROOT="${MAP%%/maps/*}" @@ -131,7 +146,7 @@ if [[ -z "$OUT_ROOT" ]]; then printf 'Could not derive DATASET_ROOT from map path; pass --out-root explicitly\n' >&2 exit 2 fi - OUT_ROOT="$DATASET_ROOT/gicp_result" + OUT_ROOT="$DATASET_ROOT/gicp_result/intermediate" fi OUT_ROOT="$(realpath -m "$OUT_ROOT")" RUN_DIR="$OUT_ROOT/$RUN_NAME" @@ -232,6 +247,7 @@ ros2 launch gicp_plusplus localization_with_tf.launch.py \ lidar_reliable_qos:="$LIDAR_RELIABLE_QOS" \ future_aux_wait_timeout_s:="$FUTURE_AUX_WAIT_TIMEOUT_S" \ primary_queue_size:="$PRIMARY_QUEUE_SIZE" \ + config_path:="$CONFIG_PATH" \ >"$RUN_DIR/localization.log" 2>&1 & launch_pid=$! @@ -276,6 +292,7 @@ for bag in "${BAGS[@]}"; do play_args+=(-i "$bag" "$STORAGE_ID") done play_args+=( + --read-ahead-queue-size "$READ_AHEAD_QUEUE_SIZE" --rate "$RATE" --start-offset "$START_OFFSET" --playback-duration "$DURATION" @@ -332,6 +349,11 @@ play_wall_s="$(awk -v start="$play_start_ns" -v end="$play_end_ns" \ printf 'lidar_reliable_qos=%s\n' "$LIDAR_RELIABLE_QOS" printf 'future_aux_wait_timeout_s=%s\n' "$FUTURE_AUX_WAIT_TIMEOUT_S" printf 'primary_queue_size=%s\n' "$PRIMARY_QUEUE_SIZE" + printf 'read_ahead_queue_size=%s\n' "$READ_AHEAD_QUEUE_SIZE" + printf 'config_path=%s\n' "$CONFIG_PATH" + if [[ -n "$CONFIG_PATH" ]]; then + printf 'config_sha256=%s\n' "$(sha256sum "$CONFIG_PATH" | awk '{print $1}')" + fi printf 'pointcloud_topic=%s\n' "$POINTCLOUD_TOPIC" printf 'imu_topic=%s\n' "$IMU_TOPIC" printf 'gt_topic=%s\n' "$GT_TOPIC" From 4c86beea3c63c25d2396e97425d3882eace138d5 Mon Sep 17 00:00:00 2001 From: FieldDiTian Date: Wed, 29 Jul 2026 21:06:41 -0700 Subject: [PATCH 4/4] Address consolidated PR 16 replay review --- GICP_plusplus/CMakeLists.txt | 5 + GICP_plusplus/README.md | 14 +- .../front_no_atlas_translation_replay.yaml | 38 ++ GICP_plusplus/cfg/front_quality_replay.yaml | 25 +- GICP_plusplus/cfg/localization.yaml | 120 ++---- .../include/gicp_plusplus/imu_range.hpp | 73 ++++ .../include/gicp_plusplus/localization.h | 13 +- .../include/gicp_plusplus/rtk_gate.hpp | 16 +- .../gicp_plusplus/small_gicp_backend.hpp | 83 +++- .../launch/localization_with_tf.launch.py | 10 +- .../scripts/analyze_scan_debug_log.py | 122 +++++- GICP_plusplus/src/localization.cc | 395 ++++++++++++++---- GICP_plusplus/test/imu_range_test.cpp | 66 +++ GICP_plusplus/test/rtk_gate_test.cpp | 9 +- .../mapping/global_mapping_pose_graph.cpp | 50 ++- GLIM/glim_ext/config/config_gnss_global.json | 4 +- .../include/glim_ext/gnss_global_module.hpp | 2 +- GLIM/glim_ros2/src/glim_pcap_rosbag.cpp | 8 +- GLIM/glim_ros2/src/glim_rosbag.cpp | 60 ++- README.md | 12 + gicp_localization/cfg/localization.yaml | 5 +- scripts/build_consistent_pcd.py | 88 +++- scripts/prepare_gicp_replay_bag.py | 10 +- scripts/run_gicp_replay_audit.sh | 220 +++++++++- 24 files changed, 1235 insertions(+), 213 deletions(-) create mode 100644 GICP_plusplus/cfg/front_no_atlas_translation_replay.yaml create mode 100644 GICP_plusplus/include/gicp_plusplus/imu_range.hpp create mode 100644 GICP_plusplus/test/imu_range_test.cpp diff --git a/GICP_plusplus/CMakeLists.txt b/GICP_plusplus/CMakeLists.txt index dbd53c77..7cb4d166 100644 --- a/GICP_plusplus/CMakeLists.txt +++ b/GICP_plusplus/CMakeLists.txt @@ -113,6 +113,11 @@ if(BUILD_TESTING) test/rtk_gate_test.cpp ) target_include_directories(rtk_gate_test PRIVATE include) + ament_add_gtest( + imu_range_test + test/imu_range_test.cpp + ) + target_include_directories(imu_range_test PRIVATE include) endif() ament_package() diff --git a/GICP_plusplus/README.md b/GICP_plusplus/README.md index ba817e13..ed5a5b5b 100644 --- a/GICP_plusplus/README.md +++ b/GICP_plusplus/README.md @@ -121,7 +121,7 @@ ros2 launch gicp_plusplus localization_with_tf.launch.py \ | `imu_only` | `false` | Disable GICP and propagate pose from IMU only (debug/sanity check). | | `lidar_concat_enabled` | `false` | Opt in to front+left+right online GICP for synchronization/diagnostic A/B tests. Production uses a three-LiDAR offline map with front-only online GICP to meet 10 Hz. | | `primary_queue_size` | `8` | Bounded front compute queue. Keep 8 for live operation. A lossless offline replay may use a larger bounded queue to absorb rosbag delivery bursts, but must separately prove sub-100 ms scan compute and zero overload drops. | -| `config_path` | empty | Optional run-local YAML loaded after the package default, used for reproducible profiles such as `cfg/front_quality_replay.yaml`. | +| `config_path` | empty | Optional run-local YAML loaded after the package default. Parameter files are logged in precedence order. Use `cfg/front_quality_replay.yaml` for the GNSS-aided Laguna profile or `cfg/front_no_atlas_translation_replay.yaml` for the per-scan zero-Atlas-translation A/B. | | `urdf_path` | (auto-found) | Path to the URDF (`av24.urdf`) used for offline extrinsic resolution. The launch resolves it by walking up from the launch dir; `av24.urdf` is also installed into `share/gicp_plusplus`. | | `parent_frame` / `child_frame` | `base_link` / `luminar_front` | `child_frame` overrides `localization/lidar_frame` (the LiDAR link the node resolves extrinsics for); `parent_frame` is declared but currently unused (no static-TF helper is launched — `robot_state_publisher` provides the URDF tree). | | `map_path` | (yaml) | Override the yaml `localization/map_path` from the command line. | @@ -327,12 +327,22 @@ optional ratio thresholds can still be evaluated in an explicit A/B. `cfg/front_quality_replay.yaml` is the checked-in Laguna compressed-map profile used through the launch file's `config_path` argument. It leaves the production motion chain enabled, uses 0.25 m target and 0.30 m source voxels -with a 100 m sensor-frame crop, 32 iterations, and an 80 ms optimizer budget. +with a 100 m sensor-frame crop, 32 iterations, and an 80 ms cooperative +scan-registration budget. The budget includes source KD-tree/covariance +preparation and passes only its remaining time to the iterative optimizer. Atlas translation seeds only the GICP optimizer; it never modifies `basePose`, observer state, or published output. Every candidate must still pass correspondence, physical-jump, and the unchanged 5 m Atlas wrong-basin gate. +`cfg/front_no_atlas_translation_replay.yaml` is the registration-side A/B: it +sets both the per-scan Atlas translation seed blend and Atlas +candidate-position gate to zero. Package defaults may still use Atlas for +initialization, heading, and recovery, so this profile is not independent +truth. The audit runner requires an explicit `gnss_aided` or `independent` +evidence label; independent evidence must use a reference topic distinct from +the runtime GT topic. + Use the profile with the topic-reduced replay bag and the repository audit runner documented in the root workflow. A rate pass requires 1.0x playback, zero front overload drops, and measured scan-compute latency below the 10 Hz diff --git a/GICP_plusplus/cfg/front_no_atlas_translation_replay.yaml b/GICP_plusplus/cfg/front_no_atlas_translation_replay.yaml new file mode 100644 index 00000000..b0264ad0 --- /dev/null +++ b/GICP_plusplus/cfg/front_no_atlas_translation_replay.yaml @@ -0,0 +1,38 @@ +/**: + ros__parameters: + # Laguna front-only A/B profile with per-scan Atlas translation removed + # from the optimizer seed and candidate gate. This is NOT independent + # truth: package defaults still permit Atlas initialization, heading, and + # recovery unless a caller overrides those contracts explicitly. + localization/map_voxel_size: 0.25 + dlio/preprocessing/cropBoxFilter/size: 100.0 + dlio/preprocessing/voxelFilter/use: true + dlio/preprocessing/voxelFilter/res: 0.30 + gicp/maxIterations: 32 + gicp/maxOptimizationTimeMsCooperative: 80.0 + gicp/correspondenceRandomness: 10 + gicp/maxCorrespondenceDistance: 1.0 + gicp/transformationEpsilon: 0.01 + gicp/rotationEpsilon: 0.004363323 + gicp/fitnessRejectThreshold: 1000000000.0 + gicp/nonConvergedFitnessOkMaxTransM: 0.0 + gicp/nonConvergedFitnessOkMaxRotDeg: 0.0 + gicp/minCorrespondences: 0 + gicp/minCorrespondenceRatio: 0.3 + gicp/fitnessBaseline/enable: false + gicp/fitnessRatioRejectThreshold: 0.0 + + dlio/deskew: true + localization/ins_prior/enable: true + localization/ins_prior/gicp_position_seed_blend: 0.0 + localization/ins_prior/gicp_position_seed_max_step_m: 20.0 + localization/gt_odom/max_candidate_position_error_m: 0.0 + + localization/lidar_concat/enabled: false + localization/lidar_concat/reliable_qos: false + localization/lidar_concat/time_threshold: 0.05 + localization/lidar_concat/aux_time_offsets: [0.0, 0.0] + localization/lidar_concat/float64_time_is_epoch_ns: false + odom/geo/max_pos_correction: 2.0 + odom/geo/max_vel_correction: 5.0 + odom/geo/max_state_speed: 100.0 diff --git a/GICP_plusplus/cfg/front_quality_replay.yaml b/GICP_plusplus/cfg/front_quality_replay.yaml index e78002c7..8fe8f0b3 100644 --- a/GICP_plusplus/cfg/front_quality_replay.yaml +++ b/GICP_plusplus/cfg/front_quality_replay.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - # High-information front-only localization profile. Atlas translation is + # Laguna high-information front-only localization profile. Atlas translation is # used only to place the GICP optimizer in the correct local basin; it is # never blended into basePose, observer state, or published output. localization/map_voxel_size: 0.25 @@ -8,7 +8,18 @@ dlio/preprocessing/voxelFilter/use: true dlio/preprocessing/voxelFilter/res: 0.30 gicp/maxIterations: 32 - gicp/maxOptimizationTimeMs: 80.0 + gicp/maxOptimizationTimeMsCooperative: 80.0 + gicp/correspondenceRandomness: 10 + gicp/maxCorrespondenceDistance: 1.0 + gicp/transformationEpsilon: 0.01 + gicp/rotationEpsilon: 0.004363323 + gicp/fitnessRejectThreshold: 1000000000.0 + gicp/nonConvergedFitnessOkMaxTransM: 0.0 + gicp/nonConvergedFitnessOkMaxRotDeg: 0.0 + gicp/minCorrespondences: 0 + gicp/minCorrespondenceRatio: 0.3 + gicp/fitnessBaseline/enable: false + gicp/fitnessRatioRejectThreshold: 0.0 # Keep the deployed Laguna motion-prediction chain. dlio/deskew: true @@ -18,3 +29,13 @@ # Fail closed on a map match outside the quality-gated Atlas envelope. localization/gt_odom/max_candidate_position_error_m: 5.0 + + # Laguna online contract and observer safety limits. + localization/lidar_concat/enabled: false + localization/lidar_concat/reliable_qos: false + localization/lidar_concat/time_threshold: 0.05 + localization/lidar_concat/aux_time_offsets: [0.0, 0.0] + localization/lidar_concat/float64_time_is_epoch_ns: false + odom/geo/max_pos_correction: 2.0 + odom/geo/max_vel_correction: 5.0 + odom/geo/max_state_speed: 100.0 diff --git a/GICP_plusplus/cfg/localization.yaml b/GICP_plusplus/cfg/localization.yaml index f68d1b21..a4fbac6d 100644 --- a/GICP_plusplus/cfg/localization.yaml +++ b/GICP_plusplus/cfg/localization.yaml @@ -6,6 +6,9 @@ # Localization Parameters # Path to pre-built PCD map file localization/map_path: "dlio_maps/may_4_putnam.pcd" # REQUIRED: Set this to your map file path (e.g., "/path/to/map.pcd") + # Opt-in deployment gate: require a complete ENU manifest and an explicitly + # configured expected_enu_origin. Legacy maps remain warn-only by default. + localization/require_map_manifest: false # UTM output: path to GLIM's T_world_utm.txt for the map being used. # Leave empty to disable UTM-frame publishing. @@ -120,13 +123,9 @@ localization/gt_odom/enable: true localization/gt_odom/buffer_size: 200 # ring buffer depth (~2s @ 100Hz GT) localization/gt_odom/max_dt: 0.1 # seconds; reject lookups farther than this from scan stamp - # On-car wrong-basin safety envelope. Atlas position is NOT blended into - # every accepted GICP pose: it only rejects a candidate outside this broad - # radius after the RTK covariance/time gates pass. Five consecutive - # rejects invoke the existing recovery path. This prevents the observed - # 20-200 m parallel-structure slides from poisoning observer velocity. - # Set 0 for a deliberately GNSS-independent mapping-quality experiment. - localization/gt_odom/max_candidate_position_error_m: 5.0 + # Optional GNSS wrong-basin envelope. Keep disabled in the shared, + # site-independent profile; deployment overlays may enable it. + localization/gt_odom/max_candidate_position_error_m: 0.0 # RTK quality gate, applied PER CONSUMER at consumption time — NOT a # buffer filter. Every incoming gt_odom sample enters the buffer; the @@ -156,6 +155,7 @@ # (calibration / INS prior / cross-check) should accept every sample. # Buffering, snap recovery, and odom-init are unaffected either way. localization/rtk_gate/enable: true + localization/rtk_gate/allow_zero_covariance: false localization/rtk_gate/max_pose_var_xy: 0.25 # m^2; ~0.5 m horizontal std localization/rtk_gate/max_pose_var_z: 1.0 # m^2; ~1.0 m vertical std (GPS Z is naturally worse) @@ -163,10 +163,9 @@ # scans in a row, snap current_pose / lidarPose / state.{p,q,v} to the # time-matched GT sample so GICP can re-acquire from a known-good state. # The GT pose+twist is transformed from msg->child_frame_id into base_frame - # via TF (cached on first GT message). IMU biases are preserved. The Laguna - # deployment profile enables this explicitly; deployments without an Atlas - # INS reference should disable it. Setting enable=true forces - # gt_odom/enable=true. + # via TF (cached on first GT message). IMU biases are preserved. Disabled by + # default — production deployments without a GT topic keep the existing + # dead-reckoning behavior. Setting enable=true forces gt_odom/enable=true. localization/gt_recovery/enable: true # P2#3: raised 1 -> 5. With min=1 the snap fired on EVERY rejected frame # (1,860 snaps in run 12), masking dead-reckoning quality in all replay @@ -219,20 +218,13 @@ # sensor frame, rebases per-point timestamps onto the primary clock, and appends # the bytes to the primary PointCloud2. Downstream steps see a single cloud in # primary frame with one coherent timebase. - # Deployment contract: the OFFLINE GLIM map uses all three LiDARs, while - # ONLINE GICP follows perception-ws and consumes the front LiDAR only. A - # three-cloud GICP frame triples preprocessing/deskew work and cannot meet - # the 10 Hz live deadline on the current CPU. Keep concat as an explicit - # synchronization/diagnostic A/B option via launch - # lidar_concat_enabled:=true; never slow a deployment bag to hide overload. - localization/lidar_concat/enabled: false - # Live default is false (BEST_EFFORT SensorDataQoS). Set true only when an - # offline rosbag publisher is also overridden to RELIABLE; this prevents - # large three-cloud replay bursts from being silently dropped by DDS. + localization/lidar_concat/enabled: true + # QoS is independent of site geometry. Live sensors normally use + # BEST_EFFORT; lossless replay overlays may opt into RELIABLE. localization/lidar_concat/reliable_qos: false localization/lidar_concat/aux_topics: ["/luminar_right/points", "/luminar_left/points"] localization/lidar_concat/aux_frames: ["luminar_right", "luminar_left"] - localization/lidar_concat/time_threshold: 0.05 # seconds; NON-LUMINAR/relative-time header matching gate. + localization/lidar_concat/time_threshold: 0.1 # seconds; NON-LUMINAR fallback matching + tie-break only. # Luminar acceptance is gated by the absolute point-time # threshold below, never by header proximity (headers carry # 66-92 ms acquisition phase on AV-24 with coherent point times). @@ -240,12 +232,10 @@ # max(|min-min|,|max-max|) must be <= this. Header distance is only a # tie-break between candidates with equal range error. localization/lidar_concat/luminar_point_time_threshold_s: 0.010 - # Async Luminar front worker (front-only production and concat diagnostic - # paths). The front callback only validates + enqueues, so a long GICP - # iteration cannot block DDS reception. With concat enabled, the worker - # releases fronts in order when every aux is matched/final (watermark), or - # at this arrival-time deadline, merging whatever matched. Front-only - # releases immediately. Aux state can NEVER drop a front. + # Async front/aux synchronizer (Luminar production path). The front + # callback only validates + enqueues; a worker releases fronts in order + # when every aux is matched or final (watermark), or at this arrival-time + # deadline — merging whatever matched. Aux state can NEVER drop a front. localization/lidar_concat/future_aux_wait_timeout_s: 0.150 # HARD bound on the pending-front queue (compute-overload policy). If the # GICP pipeline is slower than the front input rate, the OLDEST queued @@ -260,17 +250,10 @@ # header-delta extrema, while the geometric point-time regression measured # |offset| < 11 ms on both runs. Keep zero until a geometric point-time # measurement establishes a value; never copy header phase in here. - # Keep an explicitly typed vector for ROS 2 parameter parsing. An empty - # YAML sequence has no element type and Jazzy rejects it before node - # startup; zeros preserve the intended "no measured correction" behavior. - localization/lidar_concat/aux_time_offsets: [0.0, 0.0] - # Luminar point-time carrier contract. Laguna's decoder declares FLOAT64 - # and stores ordinary seconds-since-sweep-start (0..~0.1), so keep false: - # aux scans are matched by the safe future-header fallback and their - # relative times are rebased onto the primary header before deskew. - # Set true only for a driver known to mislabel raw uint64 epoch-ns bits as - # FLOAT64. UINT8[8] raw epoch-ns is detected independently. + # Omit aux_time_offsets here so the generic empty-vector default is used. + # Site profiles may provide an explicitly typed vector. localization/lidar_concat/float64_time_is_epoch_ns: false + localization/lidar_concat/float64_time_fail_on_mismatch: true localization/lidar_concat/buffer_size: 200 # per-aux ring buffer depth (P4#3: raised 20 -> 200 # for GLIM parity; 20 = only 2 s of aux history at # 10 Hz, so brief stalls degraded frames to fewer @@ -452,14 +435,9 @@ # Optional hard clamps on the per-update correction magnitude (0 = disabled). # The dt cap above already bounds the gain; enable these only if you also want # an absolute ceiling on how far one scan can move the state. - # Long-run Laguna failure audit: a wrong-basin candidate created a 25 m - # residual and the unbounded Kv path injected >120 m/s into the observer, - # turning subsequent local searches into second-scale full-map misses. - # These limits are well above healthy per-scan corrections but prevent a - # bad candidate from converting directly into an unstable prediction. - odom/geo/max_pos_correction: 2.0 # m — clamp per-update position correction - odom/geo/max_vel_correction: 5.0 # m/s — clamp per-update velocity correction - odom/geo/max_state_speed: 100.0 # m/s — above Laguna race speed, finite safety cap + odom/geo/max_pos_correction: 0.0 # m — clamp per-update position correction + odom/geo/max_vel_correction: 0.0 # m/s — clamp per-update velocity correction + odom/geo/max_state_speed: 0.0 # m/s — optional physical speed cap # P1 yaw-safety fix #3: ORIENTATION got the clamp position/velocity always # had. The observer pulls dt_eff*Kq (~45%) of the orientation error per # update; unclamped, one bad accepted scan injects tens of degrees of @@ -524,35 +502,32 @@ localization/jump/yaw_total_max_deg: 15.0 # GICP Registration Parameters - # Match the deployed perception-ws Laguna localizer. Fifty iterations - # leaves ample convergence margin while bounding the 10 Hz CPU deadline. - gicp/maxIterations: 50 + # Maximum number of iterations (increased for better convergence) + gicp/maxIterations: 128 # Optional iterative-optimizer wall-clock budget in milliseconds. Zero # preserves the historical unbounded behavior; replay/site profiles may # set a finite budget so a pathological basin fails closed at the INS # prior instead of blocking the sensor pipeline. - gicp/maxOptimizationTimeMs: 0.0 + gicp/maxOptimizationTimeMsCooperative: 0.0 # Number of neighbors used to compute per-point covariances (typical: 20) - gicp/correspondenceRandomness: 10 + gicp/correspondenceRandomness: 20 # Maximum correspondence distance (meters) # Points further apart than this won't be considered correspondences # Tightened from 5.0 to cut spurious matches against the dense full map. - gicp/maxCorrespondenceDistance: 1.0 + gicp/maxCorrespondenceDistance: 4.0 # Convergence criteria — tightened now that downsampling is re-enabled - gicp/transformationEpsilon: 0.01 - gicp/rotationEpsilon: 0.004363323 - - # Laguna/perception-ws parity: do not gate a registration on the absolute - # fitness magnitude. That value changes with map density, scene and speed: - # on the compressed 32.54M map, candidates with 99% correspondence support - # and only 0.9–1.1 m TTL error reached fitness 6–8 in the first fast turn. - # The old 1.0 gate rejected those correct poses and created a cascade. - # Keep a finite high ceiling because this value also bounds the optional - # non-converged fallback; NaN/Inf still fail closed. - gicp/fitnessRejectThreshold: 1000000000.0 + gicp/transformationEpsilon: 0.004 + gicp/rotationEpsilon: 0.004 + + # GICP result gating: reject poor registrations and fall back to IMU dead-reckoning. + # Good alignments on the full map hit fitness ~0.01–0.05 (p90 0.04, p99 0.39). + # Keep this at 1.0, not tighter: during the first ~4 scans the initial pose is still + # settling, so fitness spikes to 1.2–1.4 on correct geometry; a 0.3 threshold rejects + # those and the node gets stuck at initial pose with no GICP updates to recover from. + gicp/fitnessRejectThreshold: 1.0 # Reject GICP if fitness score exceeds this value # PR#6: bounds on the NON-CONVERGED low-fitness fallback ("effectively # converged"). The fallback exists for max-iterations-at-speed scans whose # correction is small; unbounded, it also admitted wrong-basin results @@ -562,10 +537,8 @@ # classified failed_to_converge instead. <=0 disables a bound. # PR validation: nonconv accepts 1145->397; accepts with INS err >=50 m # 99->3; p90 INS err 21.6->12.7 m. - # perception-ws lets support and physical gates judge a final candidate - # even when small_gicp exhausts its iteration budget. - gicp/nonConvergedFitnessOkMaxTransM: 0.0 - gicp/nonConvergedFitnessOkMaxRotDeg: 0.0 + gicp/nonConvergedFitnessOkMaxTransM: 3.0 + gicp/nonConvergedFitnessOkMaxRotDeg: 5.0 # Minimum correspondence-support gate (REVIEW FIX 2026-07-08). Mean # fitness over a handful of inliers can look excellent while the scan @@ -576,10 +549,8 @@ # it (status: rejected_support; SCAN DEBUG: support=[count,ratio,reeval]). # <=0 disables either bound. Re-baseline against a replay scorecard if the # scan preprocessing (voxel size, crop) changes materially. - # perception-ws uses a normalized support gate, which remains meaningful - # when scan point count changes with crop/voxel settings. - gicp/minCorrespondences: 0 - gicp/minCorrespondenceRatio: 0.3 + gicp/minCorrespondences: 500 + gicp/minCorrespondenceRatio: 0.2 gicp/rejectLargeJumps: true # Hard-reject results that exceed the jump thresholds below # ========== P1 gating rework (docs/action_plan_turn_error_20260704.md) ========== @@ -610,10 +581,7 @@ # (fitness_ratio > yawGate/fitnessRatio) keeps the IMU yaw instead. # This is the wrong-basin ENTRY signature the jump gate can't see # (bad accepts had jump medians of only 1.87 m / 1.38 deg). - # The deployed perception-ws path has no rolling fitness-ratio rejection. - # Support, finite-pose, physical jump/yaw and GT sanity/recovery guards - # remain active. - gicp/fitnessBaseline/enable: false + gicp/fitnessBaseline/enable: true gicp/fitnessBaseline/window: 201 # accepted-frame samples in the rolling median (~20 s @ 10 Hz) gicp/fitnessBaseline/minSamples: 50 # rolling median takes over after this many accepted frames # Warm-up seed (review fix): baseline used until minSamples accepted frames @@ -623,7 +591,7 @@ # (scripts/analyze_scan_debug_log.py; run-12 sparse cross-run map: 0.28). # RE-MEASURE after the dense-map rebuild. 0 = off (absolute-only warm-up). gicp/fitnessBaseline/seedBaseline: 0.28 - gicp/fitnessRatioRejectThreshold: 0.0 # disabled for perception-ws parity + gicp/fitnessRatioRejectThreshold: 2.0 # <=0 disables the wrong-basin ratio reject gicp/degeneracy/partialUpdate: true # false = restore legacy binary hessian gate below # Full 6D coupled solution remapping (default). The re-centered hessian's # rotation coordinates are scaled by couplingLengthM (the typical diff --git a/GICP_plusplus/include/gicp_plusplus/imu_range.hpp b/GICP_plusplus/include/gicp_plusplus/imu_range.hpp new file mode 100644 index 00000000..b2d04e1b --- /dev/null +++ b/GICP_plusplus/include/gicp_plusplus/imu_range.hpp @@ -0,0 +1,73 @@ +#ifndef GICP_PLUSPLUS_IMU_RANGE_HPP +#define GICP_PLUSPLUS_IMU_RANGE_HPP + +#include +#include +#include + +namespace gicp_plusplus { + +// Select an oldest->newest IMU slice from a container stored newest->oldest. +// The result contains the nearest real sample at/before start_time (or one up +// to older_tolerance_s after it), every interior sample, and the first sample +// at/after end_time. A strictly monotone buffer and at least two output samples +// are required. +template +bool selectBracketedImuRange( + const Container& newest_to_oldest, double start_time, double end_time, + double older_tolerance_s, + std::vector& out) { + out.clear(); + if (newest_to_oldest.empty() || + !std::isfinite(start_time) || !std::isfinite(end_time) || + !std::isfinite(older_tolerance_s) || older_tolerance_s < 0.0 || + start_time > end_time) { + return false; + } + + const double newest = newest_to_oldest.front().stamp; + const double oldest = newest_to_oldest.back().stamp; + if (!std::isfinite(newest) || !std::isfinite(oldest) || + newest < end_time || oldest - start_time > older_tolerance_s) { + return false; + } + + auto start_it = newest_to_oldest.rend(); + double previous_stamp = -std::numeric_limits::infinity(); + for (auto it = newest_to_oldest.rbegin(); + it != newest_to_oldest.rend(); ++it) { + if (!std::isfinite(it->stamp) || it->stamp <= previous_stamp) { + return false; + } + previous_stamp = it->stamp; + if (it->stamp <= start_time) { + start_it = it; + } else { + break; + } + } + if (start_it == newest_to_oldest.rend()) { + // Startup phase: the oldest retained IMU may land just after the requested + // time. The precheck above limits this extrapolation to older_tolerance_s. + start_it = newest_to_oldest.rbegin(); + } + + previous_stamp = -std::numeric_limits::infinity(); + for (auto it = start_it; it != newest_to_oldest.rend(); ++it) { + if (!std::isfinite(it->stamp) || it->stamp <= previous_stamp) { + out.clear(); + return false; + } + previous_stamp = it->stamp; + out.push_back(*it); + if (it->stamp >= end_time && out.size() >= 2) { + return true; + } + } + out.clear(); + return false; +} + +} // namespace gicp_plusplus + +#endif // GICP_PLUSPLUS_IMU_RANGE_HPP diff --git a/GICP_plusplus/include/gicp_plusplus/localization.h b/GICP_plusplus/include/gicp_plusplus/localization.h index 6a05b3f4..f97fc142 100644 --- a/GICP_plusplus/include/gicp_plusplus/localization.h +++ b/GICP_plusplus/include/gicp_plusplus/localization.h @@ -5,6 +5,7 @@ #include "dlio/dlio.h" #include "gicp_plusplus/small_gicp_backend.hpp" #include "gicp_plusplus/luminar_sweep_matching.hpp" +#include "gicp_plusplus/imu_range.hpp" #include "gicp_plusplus/rtk_gate.hpp" // ROS @@ -131,7 +132,7 @@ class LocalizationNode : public rclcpp::Node { Eigen::Vector3f& v_ang_body_out) const; // GT-driven pose recovery. Returns true when the snap fired (guards passed and // a time-matched GT sample with finite extrinsic was applied to the state). - bool maybeSnapPoseToGT(const char* reason); + bool maybeSnapPoseToGT(const char* reason, bool force_absolute); // [P3 FIX 2026-07-14] Optional world-frame linear velocity seed. When null // (RViz /initialpose, param pose) velocity is zeroed as before; the GT // odom-init path passes the message's own twist so a mid-run seed does not @@ -262,6 +263,10 @@ class LocalizationNode : public rclcpp::Node { std::deque gt_odom_buffer_; std::mutex gt_odom_mtx_; std::atomic gt_odom_received_{false}; + std::string gt_expected_frame_id_; + std::string gt_expected_child_frame_id_; + std::atomic gt_dropped_invalid_{0}; + std::atomic gt_dropped_frame_{0}; // RTK quality gate (P1-native), applied PER CONSUMER — not a buffer // filter. Every gt_odom sample is buffered; gtSampleIsRtkFixed (finite, @@ -272,6 +277,7 @@ class LocalizationNode : public rclcpp::Node { // separate status topic is involved. Replaces the old BESTGNSSPOS-enum // gate (removed when the NovAtel path was retired). bool rtk_gate_enabled_; + bool rtk_gate_allow_zero_covariance_; double rtk_gate_max_pose_var_xy_; // m^2; reject if cov[0] or cov[7] > this double rtk_gate_max_pose_var_z_; // m^2; reject if cov[14] > this // Counter for rate-limited rejection logging. @@ -344,6 +350,8 @@ class LocalizationNode : public rclcpp::Node { // as FLOAT64; those require an explicit opt-in so ordinary doubles are // never reinterpreted as multi-billion-second timestamps. bool concat_float64_time_is_epoch_ns_ = false; + bool concat_float64_time_fail_on_mismatch_ = true; + std::atomic concat_float64_contract_checked_{false}; // ---- Async Luminar front worker / aux synchronizer ---- // Contract: every valid front cloud is released exactly once, in order, @@ -466,6 +474,7 @@ class LocalizationNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr dbg_pose_markers_pub; rclcpp::Publisher::SharedPtr dbg_fitness_pub; rclcpp::Publisher::SharedPtr dbg_gicp_elapsed_ms_pub; + rclcpp::Publisher::SharedPtr dbg_scan_total_ms_pub; rclcpp::Publisher::SharedPtr dbg_corr_norm_pub; rclcpp::Publisher::SharedPtr dbg_scan_dt_pub; rclcpp::Publisher::SharedPtr dbg_imu_age_pub; @@ -511,6 +520,7 @@ class LocalizationNode : public rclcpp::Node { pcl::PointCloud::Ptr original_scan; rclcpp::Time scan_stamp; double prev_scan_stamp; + std::chrono::steady_clock::time_point scan_pipeline_start_; // [REVIEW FIX 2026-07-08] The timestamp basePose actually corresponds to. // basePose is set from the accepted candidate / T_prior, which is the pose // at the MEDIAN POINT TIME of the scan (frames[median_pt_index]) -- NOT the @@ -721,6 +731,7 @@ class LocalizationNode : public rclcpp::Node { // Parameters std::string map_path_; + bool require_map_manifest_ = false; double map_roll_deg_; double map_pitch_deg_; double map_yaw_deg_; diff --git a/GICP_plusplus/include/gicp_plusplus/rtk_gate.hpp b/GICP_plusplus/include/gicp_plusplus/rtk_gate.hpp index b6c2918c..295b42b0 100644 --- a/GICP_plusplus/include/gicp_plusplus/rtk_gate.hpp +++ b/GICP_plusplus/include/gicp_plusplus/rtk_gate.hpp @@ -17,15 +17,19 @@ namespace gicp_plusplus { // cross-check. This matches the adapter's stricter // /gps_p1/filtered_odom_rtk_fixed gate (finite, nonnegative, thresholded). // NaN fails closed via the isfinite test. -inline bool rtkCovarianceComponentOk(double var, double max_var) { - return std::isfinite(var) && var >= 0.0 && var <= max_var; +inline bool rtkCovarianceComponentOk(double var, double max_var, + bool allow_zero_covariance = false) { + return std::isfinite(var) && + (allow_zero_covariance ? var >= 0.0 : var > 0.0) && + var <= max_var; } inline bool rtkPositionCovarianceOk(double cov_xx, double cov_yy, double cov_zz, - double max_var_xy, double max_var_z) { - return rtkCovarianceComponentOk(cov_xx, max_var_xy) && - rtkCovarianceComponentOk(cov_yy, max_var_xy) && - rtkCovarianceComponentOk(cov_zz, max_var_z); + double max_var_xy, double max_var_z, + bool allow_zero_covariance = false) { + return rtkCovarianceComponentOk(cov_xx, max_var_xy, allow_zero_covariance) && + rtkCovarianceComponentOk(cov_yy, max_var_xy, allow_zero_covariance) && + rtkCovarianceComponentOk(cov_zz, max_var_z, allow_zero_covariance); } // Conservative combine for an INTERPOLATED sample's position variance diff --git a/GICP_plusplus/include/gicp_plusplus/small_gicp_backend.hpp b/GICP_plusplus/include/gicp_plusplus/small_gicp_backend.hpp index fa5c36e0..4b722848 100644 --- a/GICP_plusplus/include/gicp_plusplus/small_gicp_backend.hpp +++ b/GICP_plusplus/include/gicp_plusplus/small_gicp_backend.hpp @@ -329,12 +329,7 @@ class SmallGicpBackend { void setInputSource(const PointCloudSourceConstPtr& cloud) { input_ = cloud; source_covs_.clear(); - if (input_ && !input_->empty()) { - source_tree_ = std::make_shared>( - input_, small_gicp::KdTreeBuilderOMP(num_threads_)); - } else { - source_tree_.reset(); - } + source_tree_.reset(); } bool calculateTargetCovariances() { @@ -405,6 +400,12 @@ class SmallGicpBackend { } void align(PointCloudSource& output, const Eigen::Matrix4f& guess) { + const auto cooperative_budget_start = std::chrono::steady_clock::now(); + source_tree_ms_ = 0.0; + source_covariance_ms_ = 0.0; + target_covariance_ms_ = 0.0; + optimizer_ms_ = 0.0; + timeout_stage_ = "none"; converged_ = false; final_transformation_ = guess; final_fitness_ = std::numeric_limits::infinity(); @@ -417,21 +418,66 @@ class SmallGicpBackend { output.clear(); return; } + const auto elapsed_budget_ms = [&]() { + return std::chrono::duration( + std::chrono::steady_clock::now() - cooperative_budget_start) + .count(); + }; + const auto budget_exhausted = [&]() { + return max_optimization_time_ms_ > 0.0 && + elapsed_budget_ms() >= max_optimization_time_ms_; + }; + const auto fail_timeout = [&](const char* stage) { + timed_out_ = true; + timeout_stage_ = stage; + converged_ = false; + final_transformation_ = guess; + final_error_ = std::numeric_limits::infinity(); + final_fitness_ = std::numeric_limits::infinity(); + num_correspondences = 0; + output.clear(); + }; small_gicp::PointCloudProxy source_proxy(*input_, source_covs_); small_gicp::PointCloudProxy target_proxy(*target_, target_covs_); if (!source_tree_) { + const auto stage_start = std::chrono::steady_clock::now(); source_tree_ = std::make_shared>( input_, small_gicp::KdTreeBuilderOMP(num_threads_)); + source_tree_ms_ = std::chrono::duration( + std::chrono::steady_clock::now() - stage_start) + .count(); + } + if (budget_exhausted()) { + fail_timeout("source_tree"); + return; } if (source_covs_.size() != input_->size()) { + const auto stage_start = std::chrono::steady_clock::now(); small_gicp::estimate_covariances_omp( source_proxy, *source_tree_, k_correspondences_, num_threads_); + source_covariance_ms_ = + std::chrono::duration( + std::chrono::steady_clock::now() - stage_start) + .count(); + } + if (budget_exhausted()) { + fail_timeout("source_covariance"); + return; } if (target_covs_.size() != target_->size()) { + const auto stage_start = std::chrono::steady_clock::now(); small_gicp::estimate_covariances_omp( target_proxy, *target_tree_, k_correspondences_, num_threads_); + target_covariance_ms_ = + std::chrono::duration( + std::chrono::steady_clock::now() - stage_start) + .count(); + } + if (budget_exhausted()) { + fail_timeout("target_covariance"); + return; } GroundVehicleGeneralFactor general_factor; @@ -454,14 +500,27 @@ class SmallGicpBackend { registration.rejector.max_dist_sq = max_corr_dist_ * max_corr_dist_; registration.optimizer.verbose = debug_print_; registration.optimizer.max_iterations = max_iterations_; - registration.optimizer.max_time_ms = max_optimization_time_ms_; + registration.optimizer.max_time_ms = + max_optimization_time_ms_ > 0.0 + ? max_optimization_time_ms_ - elapsed_budget_ms() + : 0.0; + if (max_optimization_time_ms_ > 0.0 && + registration.optimizer.max_time_ms <= 0.0) { + fail_timeout("pre_optimizer"); + return; + } registration.optimizer.timeout_flag = &timed_out_; registration.general_factor = general_factor; + const auto optimizer_start = std::chrono::steady_clock::now(); result_ = registration.align( target_proxy, source_proxy, *target_tree_, Eigen::Isometry3d(guess.cast())); + optimizer_ms_ = std::chrono::duration( + std::chrono::steady_clock::now() - optimizer_start) + .count(); if (timed_out_) { + timeout_stage_ = "optimizer"; result_ = small_gicp::RegistrationResult( Eigen::Isometry3d(guess.cast())); converged_ = false; @@ -501,6 +560,11 @@ class SmallGicpBackend { double getFinalError() const { return final_error_; } bool hasConverged() const { return converged_; } bool hasTimedOut() const { return timed_out_; } + double getSourceTreeMs() const { return source_tree_ms_; } + double getSourceCovarianceMs() const { return source_covariance_ms_; } + double getTargetCovarianceMs() const { return target_covariance_ms_; } + double getOptimizerMs() const { return optimizer_ms_; } + const char* getTimeoutStage() const { return timeout_stage_; } const Eigen::Matrix& getFinalHessian() const { return result_.H; } Eigen::Matrix4f getFinalTransformation() const { return final_transformation_; } const small_gicp::RegistrationResult& getRegistrationResult() const { return result_; } @@ -517,6 +581,11 @@ class SmallGicpBackend { double rotation_epsilon_; bool debug_print_; bool timed_out_; + double source_tree_ms_ = 0.0; + double source_covariance_ms_ = 0.0; + double target_covariance_ms_ = 0.0; + double optimizer_ms_ = 0.0; + const char* timeout_stage_ = "none"; PointCloudSourceConstPtr input_; PointCloudTargetConstPtr target_; diff --git a/GICP_plusplus/launch/localization_with_tf.launch.py b/GICP_plusplus/launch/localization_with_tf.launch.py index 435db845..22b4356e 100644 --- a/GICP_plusplus/launch/localization_with_tf.launch.py +++ b/GICP_plusplus/launch/localization_with_tf.launch.py @@ -13,7 +13,7 @@ import yaml from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node @@ -213,7 +213,13 @@ def make_localization_node(context): ('map', 'gicp/localization/map'), ], ) - return [node] + active_files = [str(localization_yaml_path.perform(context))] + if config_path_value: + active_files.append(config_path_value) + return [ + LogInfo(msg='GICP parameter files (in precedence order): ' + ' -> '.join(active_files)), + node, + ] rviz_config_path = PathJoinSubstitution([current_pkg, 'launch', 'localization.rviz']) diff --git a/GICP_plusplus/scripts/analyze_scan_debug_log.py b/GICP_plusplus/scripts/analyze_scan_debug_log.py index 78e688f9..c380fd37 100644 --- a/GICP_plusplus/scripts/analyze_scan_debug_log.py +++ b/GICP_plusplus/scripts/analyze_scan_debug_log.py @@ -20,10 +20,12 @@ """ import argparse +import json import math import re import sys from collections import Counter +from pathlib import Path def pct(sorted_vals, p): @@ -59,10 +61,61 @@ def main(): ap.add_argument("--period", type=float, default=0.1, help="scan period s (watchpoint)") ap.add_argument("--gt-bad", type=float, default=20.0, help="bad-accept gt_err threshold m") ap.add_argument("--gt-bad-rot", type=float, default=10.0, help="bad-accept gt_rot threshold deg") + ap.add_argument("--json-out", type=Path, help="write a machine-readable audit summary") + ap.add_argument( + "--mode", + choices=("gnss_aided", "independent"), + default="gnss_aided", + help="label whether Atlas GNSS seeded/gated localization", + ) + ap.add_argument( + "--min-accept-rate", + type=float, + default=0.0, + help="fail when accepted/total is below this fraction (0 disables)", + ) + ap.add_argument( + "--max-rejection-streak", + type=int, + default=0, + help="fail when the longest rejection streak exceeds this value (0 disables)", + ) + ap.add_argument( + "--require-zero-drops", + action="store_true", + help="fail when front synchronization drops or timestamp resets are reported", + ) args = ap.parse_args() rows = [] + front_overload_dropped = 0 + front_epoch_dropped = 0 + timestamp_resets = 0 + gt_invalid_dropped = 0 + gt_frame_dropped = 0 for line in open(args.log, errors="ignore"): + if "EPOCH RESET (" in line: + timestamp_resets += 1 + for pattern, target in ( + (r"(?:front_)?overload_dropped=(\d+)", "front_overload_dropped"), + (r"(?:front_)?epoch_dropped=(\d+)", "front_epoch_dropped"), + (r"timestamp_resets=(\d+)", "timestamp_resets"), + (r"gt_dropped_invalid=(\d+)", "gt_invalid_dropped"), + (r"gt_dropped_frame=(\d+)", "gt_frame_dropped"), + ): + counter_match = re.search(pattern, line) + if counter_match: + value = int(counter_match.group(1)) + if target == "front_overload_dropped": + front_overload_dropped = max(front_overload_dropped, value) + elif target == "front_epoch_dropped": + front_epoch_dropped = max(front_epoch_dropped, value) + elif target == "timestamp_resets": + timestamp_resets = max(timestamp_resets, value) + elif target == "gt_invalid_dropped": + gt_invalid_dropped = max(gt_invalid_dropped, value) + else: + gt_frame_dropped = max(gt_frame_dropped, value) m = ROW.search(line) if not m: continue @@ -98,7 +151,7 @@ def main(): sys.exit("no SCAN DEBUG rows found") n = len(rows) - print(f"# SCAN DEBUG scorecard — {args.log}\nframes: {n}") + print(f"# SCAN DEBUG scorecard — {args.log}\nmode: {args.mode}\nframes: {n}") # --- status / acceptance / streaks --- counts = Counter(r["st"] for r in rows) @@ -120,7 +173,8 @@ def main(): if cur: streaks.append(cur) long_s = [s for s in streaks if s >= 10] - print(f" rejection streaks: n={len(streaks)} max={max(streaks) if streaks else 0} " + max_streak = max(streaks) if streaks else 0 + print(f" rejection streaks: n={len(streaks)} max={max_streak} " f">=10: {len(long_s)} (frames in them: {sum(long_s)}) [plan gate: max < 20]") # --- gicp_ms --- @@ -253,6 +307,68 @@ def main(): else: print("\n## lidar_concat coverage: no per-frame concat fields (pre-P4 log)") + concat_coverage = {} + if cc: + concat_coverage = { + f"{merged}/{cc[0]['ct']}": count + for merged, count in sorted(Counter(r["cn"] for r in cc).items()) + } + summary = { + "mode": args.mode, + "frames": n, + "accepted": acc, + "acceptance_rate": acc / n, + "max_rejection_streak": max_streak, + "status_counts": dict(sorted(counts.items())), + "front_overload_dropped": front_overload_dropped, + "front_epoch_dropped": front_epoch_dropped, + "timestamp_resets": timestamp_resets, + "gt_dropped_invalid": gt_invalid_dropped, + "gt_dropped_frame": gt_frame_dropped, + "concat_coverage": concat_coverage, + "thresholds": { + "min_accept_rate": args.min_accept_rate, + "max_rejection_streak": args.max_rejection_streak, + "require_zero_drops": args.require_zero_drops, + }, + } + + failures = [] + if args.min_accept_rate and summary["acceptance_rate"] < args.min_accept_rate: + failures.append( + f"acceptance_rate={summary['acceptance_rate']:.6f} < {args.min_accept_rate:.6f}" + ) + if args.max_rejection_streak and max_streak > args.max_rejection_streak: + failures.append( + f"max_rejection_streak={max_streak} > {args.max_rejection_streak}" + ) + if args.require_zero_drops: + for key in ( + "front_overload_dropped", + "front_epoch_dropped", + "timestamp_resets", + ): + if summary[key]: + failures.append(f"{key}={summary[key]} (expected 0)") + summary["passed"] = not failures + summary["failures"] = failures + + print("\n## Audit gate") + print(f" mode: {args.mode}") + print(f" result: {'PASS' if not failures else 'FAIL'}") + for failure in failures: + print(f" - {failure}") + + if args.json_out: + args.json_out.parent.mkdir(parents=True, exist_ok=True) + args.json_out.write_text( + json.dumps(summary, indent=2, sort_keys=True) + "\n", + encoding="utf-8", + ) + if failures: + return 2 + return 0 + if __name__ == "__main__": - main() + sys.exit(main()) diff --git a/GICP_plusplus/src/localization.cc b/GICP_plusplus/src/localization.cc index 56fba9ed..99049ac1 100644 --- a/GICP_plusplus/src/localization.cc +++ b/GICP_plusplus/src/localization.cc @@ -712,6 +712,85 @@ bool luminarCloudUsesRelativeFloat64( datatype, count, float64_time_is_epoch_ns); } +// Sanity-check the explicit FLOAT64 carrier contract against the first cloud. +// A raw epoch-ns carrier is plausible only when the uint64 interpretation lies +// close to the header epoch and spans at most a sweep. Ordinary doubles are +// plausible only as finite, small scan-relative seconds. This catches the +// otherwise silent configuration inversion where both interpretations still +// produce finite numbers. +bool luminarFloat64TimeContractMatches( + const sensor_msgs::msg::PointCloud2& msg, bool expect_raw_epoch_ns, + std::string& detail) { + int time_off = -1; + uint8_t datatype = 0; + int count = 0; + if (!findTimeField(msg, time_off, datatype, count) || + datatype != sensor_msgs::msg::PointField::FLOAT64 || count != 1) { + detail = "cloud does not advertise a FLOAT64[1] time field"; + return true; + } + if (msg.is_bigendian || time_off < 0 || msg.point_step == 0 || + static_cast(time_off) + sizeof(uint64_t) > msg.point_step) { + detail = "malformed or big-endian FLOAT64 time field"; + return false; + } + const size_t point_count = static_cast(msg.width) * msg.height; + const size_t required_bytes = + point_count * static_cast(msg.point_step); + if (point_count == 0 || msg.data.size() < required_bytes) { + detail = "empty or truncated FLOAT64 time payload"; + return false; + } + + double min_double = std::numeric_limits::infinity(); + double max_double = -std::numeric_limits::infinity(); + uint64_t min_raw = std::numeric_limits::max(); + uint64_t max_raw = 0; + bool doubles_finite = true; + for (size_t i = 0; i < point_count; ++i) { + const uint8_t* tp = + msg.data.data() + i * msg.point_step + static_cast(time_off); + double as_double = 0.0; + uint64_t as_raw = 0; + std::memcpy(&as_double, tp, sizeof(as_double)); + std::memcpy(&as_raw, tp, sizeof(as_raw)); + doubles_finite = doubles_finite && std::isfinite(as_double); + min_double = std::min(min_double, as_double); + max_double = std::max(max_double, as_double); + if (as_raw != 0) { + min_raw = std::min(min_raw, as_raw); + max_raw = std::max(max_raw, as_raw); + } + } + + const uint64_t header_ns = + static_cast(msg.header.stamp.sec) * 1000000000ULL + + static_cast(msg.header.stamp.nanosec); + const bool raw_nonempty = min_raw != std::numeric_limits::max(); + const uint64_t raw_mid = raw_nonempty ? min_raw + (max_raw - min_raw) / 2 : 0; + const uint64_t raw_header_error = + raw_mid > header_ns ? raw_mid - header_ns : header_ns - raw_mid; + const bool raw_plausible = + raw_nonempty && header_ns > 1000000000000000ULL && + min_raw > 1000000000000000ULL && max_raw >= min_raw && + max_raw - min_raw <= 2000000000ULL && + raw_header_error <= 5000000000ULL; + const bool relative_plausible = + doubles_finite && min_double >= -1.0 && max_double <= 10.0 && + max_double >= min_double && max_double - min_double <= 2.0; + + std::ostringstream oss; + oss << std::setprecision(9) + << "double_range=[" << min_double << "," << max_double << "] " + << "raw_range=[" << min_raw << "," << max_raw << "] " + << "header_ns=" << header_ns + << " relative_plausible=" << relative_plausible + << " raw_epoch_plausible=" << raw_plausible; + detail = oss.str(); + return expect_raw_epoch_ns ? raw_plausible + : (relative_plausible && !raw_plausible); +} + inline bool luminarRawTimestampNsFromBytes( const uint8_t* tp, uint8_t datatype, int count, size_t bytes_avail, bool float64_time_is_epoch_ns, uint64_t& out) { @@ -1425,6 +1504,8 @@ gicp_plusplus::LocalizationNode::LocalizationNode() : Node("gicp_plusplus_node") this->dbg_fitness_pub = this->create_publisher("gicp/localization/debug/fitness", 10); this->dbg_gicp_elapsed_ms_pub = this->create_publisher("gicp/localization/debug/gicp_elapsed_ms", 10); + this->dbg_scan_total_ms_pub = + this->create_publisher("gicp/localization/debug/scan_total_ms", 10); this->dbg_corr_norm_pub = this->create_publisher("gicp/localization/debug/corr_norm", 10); this->dbg_scan_dt_pub = this->create_publisher("gicp/localization/debug/scan_dt", 10); this->dbg_imu_age_pub = this->create_publisher("gicp/localization/debug/imu_age", 10); @@ -1690,6 +1771,7 @@ void gicp_plusplus::LocalizationNode::getParams() { // Map parameters this->declare_parameter("localization/map_path", ""); + this->declare_parameter("localization/require_map_manifest", false); this->declare_parameter("localization/utm_transform_path", ""); // [P3 FIX 2026-07-10] optional ENU-datum enforcement against the map manifest this->declare_parameter("localization/expected_enu_origin", ""); @@ -1708,6 +1790,8 @@ void gicp_plusplus::LocalizationNode::getParams() { this->declare_parameter("localization/map_rotation/yaw_deg", 0.0); this->get_parameter("localization/map_path", this->map_path_); + this->get_parameter( + "localization/require_map_manifest", this->require_map_manifest_); std::string utm_transform_path; this->get_parameter("localization/utm_transform_path", utm_transform_path); @@ -1741,6 +1825,10 @@ void gicp_plusplus::LocalizationNode::getParams() { this->declare_parameter("localization/gt_odom/enable", true); // [P3 FIX 2026-07-10] yaml-aligned this->declare_parameter("localization/gt_odom/buffer_size", 200); this->declare_parameter("localization/gt_odom/max_dt", 0.1); + this->declare_parameter( + "localization/gt_odom/expected_frame_id", this->map_frame); + this->declare_parameter( + "localization/gt_odom/expected_child_frame_id", this->base_frame); // Runtime GNSS/INS guard, not a scoring shortcut: an RTK-quality candidate // outside this radius is rejected before it can inject a false correction // into the geometric observer. Disabled in the C++ fallback; the Laguna @@ -1757,6 +1845,11 @@ void gicp_plusplus::LocalizationNode::getParams() { this->get_parameter("localization/gt_odom/enable", gt_enable); this->get_parameter("localization/gt_odom/buffer_size", gt_buf); this->get_parameter("localization/gt_odom/max_dt", gt_max_dt); + this->get_parameter( + "localization/gt_odom/expected_frame_id", this->gt_expected_frame_id_); + this->get_parameter( + "localization/gt_odom/expected_child_frame_id", + this->gt_expected_child_frame_id_); this->get_parameter( "localization/gt_odom/max_candidate_position_error_m", this->gt_max_candidate_pos_error_m_); @@ -1793,10 +1886,13 @@ void gicp_plusplus::LocalizationNode::getParams() { // RTK-fixed and float-mode covariances measured on AV-24 ~ 5e-5 m^2); // z threshold 1.0 m^2 (~1 m std, since GPS Z is naturally worse). this->declare_parameter("localization/rtk_gate/enable", true); + this->declare_parameter("localization/rtk_gate/allow_zero_covariance", false); this->declare_parameter("localization/rtk_gate/max_pose_var_xy", 0.25); this->declare_parameter("localization/rtk_gate/max_pose_var_z", 1.0); this->get_parameter("localization/rtk_gate/enable", this->rtk_gate_enabled_); + this->get_parameter("localization/rtk_gate/allow_zero_covariance", + this->rtk_gate_allow_zero_covariance_); this->get_parameter("localization/rtk_gate/max_pose_var_xy", this->rtk_gate_max_pose_var_xy_); this->get_parameter("localization/rtk_gate/max_pose_var_z", @@ -1837,7 +1933,8 @@ void gicp_plusplus::LocalizationNode::getParams() { // Optional wall-clock budget for the iterative optimizer. Zero preserves // the unbounded historical behavior. A timed-out solve fails closed at the // INS prior instead of blocking LiDAR reception on a pathological basin. - this->declare_parameter("gicp/maxOptimizationTimeMs", 0.0); + this->declare_parameter("gicp/maxOptimizationTimeMsCooperative", 0.0); + this->declare_parameter("gicp/maxOptimizationTimeMs", -1.0); this->declare_parameter("gicp/correspondenceRandomness", 20); this->declare_parameter("gicp/maxCorrespondenceDistance", 1.0); this->declare_parameter("gicp/transformationEpsilon", 0.0001); @@ -1935,8 +2032,22 @@ void gicp_plusplus::LocalizationNode::getParams() { this->declare_parameter("localization/ins_prior/max_yaw_sigma_deg", 3.0); this->get_parameter("gicp/maxIterations", this->gicp_max_iter_); - this->get_parameter("gicp/maxOptimizationTimeMs", + this->get_parameter("gicp/maxOptimizationTimeMsCooperative", this->gicp_max_optimization_time_ms_); + double legacy_max_optimization_time_ms = -1.0; + this->get_parameter( + "gicp/maxOptimizationTimeMs", legacy_max_optimization_time_ms); + if (legacy_max_optimization_time_ms >= 0.0) { + RCLCPP_WARN( + this->get_logger(), + "gicp/maxOptimizationTimeMs is deprecated; use " + "gicp/maxOptimizationTimeMsCooperative. The budget is cooperative " + "and includes source KD-tree/covariance preparation."); + if (this->gicp_max_optimization_time_ms_ <= 0.0) { + this->gicp_max_optimization_time_ms_ = + legacy_max_optimization_time_ms; + } + } this->get_parameter("gicp/correspondenceRandomness", this->gicp_corr_randomness_); this->get_parameter("gicp/maxCorrespondenceDistance", this->gicp_max_corr_dist_); this->get_parameter("gicp/transformationEpsilon", this->gicp_transformation_epsilon_); @@ -2093,6 +2204,8 @@ void gicp_plusplus::LocalizationNode::getParams() { // regardless of this setting. this->declare_parameter( "localization/lidar_concat/float64_time_is_epoch_ns", false); + this->declare_parameter( + "localization/lidar_concat/float64_time_fail_on_mismatch", true); // Luminar acceptance gate: absolute point-time endpoint-range error // max(|min-min|,|max-max|) <= this. Header time is only a tie-break; the // 0.1 s header threshold stays solely for non-Luminar fallback matching. @@ -2137,6 +2250,9 @@ void gicp_plusplus::LocalizationNode::getParams() { this->get_parameter( "localization/lidar_concat/float64_time_is_epoch_ns", this->concat_float64_time_is_epoch_ns_); + this->get_parameter( + "localization/lidar_concat/float64_time_fail_on_mismatch", + this->concat_float64_time_fail_on_mismatch_); // Fail LOUD on invalid offsets (GLIM config-loader policy): a NaN/inf or // extreme value would flow into point-range matching and the int64 ns // conversion in shiftCloudTimestamps (UB / corrupted absolute timestamps). @@ -2505,16 +2621,25 @@ bool gicp_plusplus::LocalizationNode::loadMap() { // a non-ENU frame is FATAL; a configured localization/expected_enu_origin // that mismatches the manifest is FATAL; a missing manifest (legacy map) // or unspecified origin warns and proceeds. + bool manifest_points_present = false; + size_t manifest_points = 0; { const std::string manifest_path = this->map_path_ + ".manifest.yaml"; std::ifstream mf(manifest_path); if (!mf.is_open()) { + if (this->require_map_manifest_) { + RCLCPP_FATAL( + this->get_logger(), + "Required map manifest is missing at '%s'.", + manifest_path.c_str()); + return false; + } RCLCPP_WARN(this->get_logger(), "No map manifest at '%s' — cannot verify the map's frame/ENU datum " "(legacy export?). Re-export with scripts/export_glim_dump_to_pcd.py.", manifest_path.c_str()); } else { - std::string line, mf_frame, mf_origin; + std::string line, mf_frame, mf_origin, mf_points; while (std::getline(mf, line)) { auto value_of = [&](const char* key) -> std::string { const std::string k(key); @@ -2528,6 +2653,35 @@ bool gicp_plusplus::LocalizationNode::loadMap() { }; if (mf_frame.empty()) { const auto v = value_of("frame:"); if (!v.empty()) mf_frame = v; } if (mf_origin.empty()) { const auto v = value_of("enu_origin:"); if (!v.empty()) mf_origin = v; } + if (mf_points.empty()) { const auto v = value_of("points:"); if (!v.empty()) mf_points = v; } + } + if (!mf_points.empty()) { + try { + size_t parsed = 0; + const auto value = std::stoull(mf_points, &parsed); + if (parsed != mf_points.size() || value == 0) { + throw std::invalid_argument("points must be a positive integer"); + } + manifest_points = static_cast(value); + manifest_points_present = true; + } catch (const std::exception& e) { + RCLCPP_FATAL( + this->get_logger(), + "Map manifest points field is invalid ('%s'): %s", + mf_points.c_str(), e.what()); + return false; + } + } + if (this->require_map_manifest_ && + (mf_frame.empty() || mf_origin.empty() || + mf_origin.rfind("UNSPECIFIED", 0) == 0 || + !manifest_points_present)) { + RCLCPP_FATAL( + this->get_logger(), + "Required map manifest is partial: frame='%s' origin='%s' " + "points='%s'. A complete ENU provenance record is mandatory.", + mf_frame.c_str(), mf_origin.c_str(), mf_points.c_str()); + return false; } if (!mf_frame.empty() && mf_frame != "enu") { RCLCPP_FATAL(this->get_logger(), @@ -2552,8 +2706,21 @@ bool gicp_plusplus::LocalizationNode::loadMap() { } std::string expected_origin; this->get_parameter("localization/expected_enu_origin", expected_origin); + if (this->require_map_manifest_ && expected_origin.empty()) { + RCLCPP_FATAL( + this->get_logger(), + "localization/require_map_manifest=true also requires " + "localization/expected_enu_origin; refusing an unverified datum."); + return false; + } if (!expected_origin.empty()) { if (mf_origin.empty() || mf_origin.rfind("UNSPECIFIED", 0) == 0) { + if (this->require_map_manifest_) { + RCLCPP_FATAL( + this->get_logger(), + "Required map manifest has no usable ENU datum."); + return false; + } RCLCPP_WARN(this->get_logger(), "localization/expected_enu_origin is set but the map manifest carries no " "datum — origin compatibility CANNOT be verified."); @@ -2614,6 +2781,15 @@ bool gicp_plusplus::LocalizationNode::loadMap() { RCLCPP_ERROR(this->get_logger(), "Loaded map is empty!"); return false; } + if (manifest_points_present && + manifest_points != this->map_cloud->points.size()) { + RCLCPP_FATAL( + this->get_logger(), + "Map/manifest pair is inconsistent: manifest points=%zu but loaded " + "PCD points=%zu. Refusing a partially replaced artifact pair.", + manifest_points, this->map_cloud->points.size()); + return false; + } // Optional static map rotation to correct coordinate-frame differences from source map files. if (std::abs(this->map_roll_deg_) > 1e-6 || @@ -3114,6 +3290,41 @@ void gicp_plusplus::LocalizationNode::enqueuePrimary( return; } + if (!this->concat_float64_contract_checked_.exchange(true)) { + std::string contract_detail; + if (!luminarFloat64TimeContractMatches( + *pc, this->concat_float64_time_is_epoch_ns_, contract_detail)) { + if (this->concat_float64_time_fail_on_mismatch_) { + { + std::lock_guard lk(this->sync_mtx_); + ++this->front_received_; + ++this->front_invalid_; + this->sync_fatal_.store(true); + } + RCLCPP_FATAL( + this->get_logger(), + "Luminar FLOAT64 point-time carrier contradicts " + "localization/lidar_concat/float64_time_is_epoch_ns=%s: %s", + this->concat_float64_time_is_epoch_ns_ ? "true" : "false", + contract_detail.c_str()); + rclcpp::shutdown(); + return; + } + RCLCPP_ERROR( + this->get_logger(), + "Luminar FLOAT64 point-time carrier mismatch ignored by explicit " + "escape hatch: %s", + contract_detail.c_str()); + } else { + RCLCPP_INFO( + this->get_logger(), + "Luminar FLOAT64 point-time carrier validated (%s): %s", + this->concat_float64_time_is_epoch_ns_ ? "raw epoch ns" + : "relative seconds", + contract_detail.c_str()); + } + } + PendingPrimaryCloud pending; pending.msg = pc; // Decode ONCE. An invalid range (unsupported time field) means point-time @@ -3370,6 +3581,7 @@ void gicp_plusplus::LocalizationNode::syncWorkerLoop() { void gicp_plusplus::LocalizationNode::processScan( const sensor_msgs::msg::PointCloud2::ConstSharedPtr& pc_in, uint64_t sync_epoch) { + this->scan_pipeline_start_ = std::chrono::steady_clock::now(); // The synchronizer worker intentionally unlocks sync_mtx_ before entering // the expensive scan pipeline. Serialize that handoff with epoch reset: if a @@ -5354,6 +5566,10 @@ void gicp_plusplus::LocalizationNode::performLocalization() { } if (this->debug_pub_enabled_) { + const double scan_total_ms = + std::chrono::duration( + std::chrono::steady_clock::now() - this->scan_pipeline_start_) + .count(); auto publish_float = [](const rclcpp::Publisher::SharedPtr& pub, double value) { std_msgs::msg::Float64 msg; msg.data = value; @@ -5362,6 +5578,7 @@ void gicp_plusplus::LocalizationNode::performLocalization() { publish_float(this->dbg_fitness_pub, fitness_score); publish_float(this->dbg_gicp_elapsed_ms_pub, elapsed_ms); + publish_float(this->dbg_scan_total_ms_pub, scan_total_ms); publish_float(this->dbg_corr_norm_pub, guess_to_solution_trans); publish_float(this->dbg_scan_dt_pub, scan_dt); publish_float(this->dbg_imu_age_pub, imu_buffer_span); @@ -5490,6 +5707,17 @@ void gicp_plusplus::LocalizationNode::performLocalization() { << " guess_from_last=[" << scalarSummary(guess_from_last_trans) << "m," << scalarSummary(guess_from_last_rot_deg) << "deg]" << " gicp_ms=" << scalarSummary(elapsed_ms, 2) + << " gicp_stage_ms=[tree=" + << scalarSummary(this->gicp.getSourceTreeMs(), 2) + << ",cov=" << scalarSummary(this->gicp.getSourceCovarianceMs(), 2) + << ",target_cov=" + << scalarSummary(this->gicp.getTargetCovarianceMs(), 2) + << ",optimizer=" << scalarSummary(this->gicp.getOptimizerMs(), 2) + << ",timeout=" << this->gicp.getTimeoutStage() << "]" + << " scan_total_ms=" << scalarSummary( + std::chrono::duration( + std::chrono::steady_clock::now() - + this->scan_pipeline_start_).count(), 2) << " converged=" << (converged ? "true" : "false") << " timed_out=" << (gicp_timed_out ? 1 : 0) << " fitness=" << scalarSummary(fitness_score, 6) @@ -5512,6 +5740,8 @@ void gicp_plusplus::LocalizationNode::performLocalization() { << " ins_dyaw=" << scalarSummary(this->last_ins_yaw_diff_deg_, 2) << "deg" << " imu_buffer_span=" << scalarSummary(imu_buffer_span) << "s" << " scan_to_latest_imu_lag=" << scalarSummary(scan_to_latest_imu_lag) << "s" + << " gt_dropped_invalid=" << this->gt_dropped_invalid_.load() + << " gt_dropped_frame=" << this->gt_dropped_frame_.load() << " concat=[" << this->concat_last_merged_aux_ << "/" << this->aux_lidars_.size(); for (size_t i = 0; i < this->concat_last_aux_dt_.size(); ++i) { oss << ",dt" << i << "=" << scalarSummary(this->concat_last_aux_dt_[i], 3) @@ -5889,7 +6119,10 @@ void gicp_plusplus::LocalizationNode::performLocalization() { // configured threshold, snap state.{pose,velocity} to the time-matched GT // sample (transformed into base_frame). The snap overrides the dead-reckoned // pose and resets the counter; logs its own warn line. - this->maybeSnapPoseToGT(reason); + const bool force_absolute_snap = + gicp_rejected_gt_sanity || gicp_rejected_fitness_ratio || + gicp_rejected_yaw || gicp_rejected_jump; + this->maybeSnapPoseToGT(reason, force_absolute_snap); } } @@ -5994,8 +6227,61 @@ void gicp_plusplus::LocalizationNode::publishPose() { } void gicp_plusplus::LocalizationNode::callbackGtOdom(const nav_msgs::msg::Odometry::ConstSharedPtr msg) { + const double stamp = + msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; + const auto& p = msg->pose.pose.position; + const auto& q = msg->pose.pose.orientation; + const auto& linear = msg->twist.twist.linear; + const auto& angular = msg->twist.twist.angular; + const double q_norm_sq = + q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z; + const bool pose_covariance_finite = std::all_of( + msg->pose.covariance.begin(), msg->pose.covariance.end(), + [](double value) { return std::isfinite(value); }); + const bool twist_covariance_finite = std::all_of( + msg->twist.covariance.begin(), msg->twist.covariance.end(), + [](double value) { return std::isfinite(value); }); + const bool fields_finite = + std::isfinite(stamp) && stamp > 0.0 && + std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z) && + std::isfinite(q.w) && std::isfinite(q.x) && + std::isfinite(q.y) && std::isfinite(q.z) && + std::isfinite(q_norm_sq) && q_norm_sq > 1e-12 && + std::isfinite(linear.x) && std::isfinite(linear.y) && + std::isfinite(linear.z) && std::isfinite(angular.x) && + std::isfinite(angular.y) && std::isfinite(angular.z) && + pose_covariance_finite && twist_covariance_finite; + if (!fields_finite) { + const auto count = ++this->gt_dropped_invalid_; + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "Dropping invalid GT odom before quaternion normalization " + "(stamp=%.9f q_norm_sq=%.6g, gt_dropped_invalid=%lu)", + stamp, q_norm_sq, static_cast(count)); + return; + } + + const bool frame_ok = + !msg->header.frame_id.empty() && !msg->child_frame_id.empty() && + (this->gt_expected_frame_id_.empty() || + msg->header.frame_id == this->gt_expected_frame_id_) && + (this->gt_expected_child_frame_id_.empty() || + msg->child_frame_id == this->gt_expected_child_frame_id_); + if (!frame_ok) { + const auto count = ++this->gt_dropped_frame_; + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "Dropping GT odom with unexpected frames '%s' -> '%s'; expected " + "'%s' -> '%s' (gt_dropped_frame=%lu)", + msg->header.frame_id.c_str(), msg->child_frame_id.c_str(), + this->gt_expected_frame_id_.c_str(), + this->gt_expected_child_frame_id_.c_str(), + static_cast(count)); + return; + } + GtSample s; - s.stamp = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; + s.stamp = stamp; s.p = Eigen::Vector3f(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z); s.q = Eigen::Quaternionf(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z); @@ -6189,7 +6475,8 @@ bool gicp_plusplus::LocalizationNode::gtSampleIsRtkFixed(const GtSample& s) cons // cross-check. Parity with the adapter's filtered_odom_rtk_fixed gate. return rtkPositionCovarianceOk(s.cov_pos_xx, s.cov_pos_yy, s.cov_pos_zz, this->rtk_gate_max_pose_var_xy_, - this->rtk_gate_max_pose_var_z_); + this->rtk_gate_max_pose_var_z_, + this->rtk_gate_allow_zero_covariance_); } bool gicp_plusplus::LocalizationNode::getGtPoseAt(double stamp, GtSample& out) { @@ -6525,7 +6812,8 @@ bool gicp_plusplus::LocalizationNode::tryRtkCalibrationStep( return true; } -bool gicp_plusplus::LocalizationNode::maybeSnapPoseToGT(const char* reason) { +bool gicp_plusplus::LocalizationNode::maybeSnapPoseToGT( + const char* reason, bool force_absolute) { // DIAGNOSTIC: prove helper is being called. Remove once snap behavior verified. // [P3 FIX 2026-07-10] Demoted from unconditional INFO ("prove helper is // being called" diagnostic) — it fired on EVERY non-accepted scan, ~10 @@ -6685,9 +6973,12 @@ bool gicp_plusplus::LocalizationNode::maybeSnapPoseToGT(const char* reason) { Eigen::Matrix4f T_gt_scan = Eigen::Matrix4f::Identity(); T_gt_scan.block<3, 3>(0, 0) = q_new.toRotationMatrix(); T_gt_scan.block<3, 1>(0, 3) = p_new; - const bool snap_delta_ok = matrixFinite(T_est_scan); + const bool snap_estimate_finite = matrixFinite(T_est_scan); + const bool apply_delta = !force_absolute && snap_estimate_finite; const Eigen::Matrix4f T_corr = - snap_delta_ok ? Eigen::Matrix4f(T_gt_scan * T_est_scan.inverse()) : T_gt_scan; + snap_estimate_finite + ? Eigen::Matrix4f(T_gt_scan * T_est_scan.inverse()) + : T_gt_scan; this->current_pose = T_gt_scan; // scan-chain pose stays a scan-time quantity Eigen::Vector3f v_base_world; @@ -6706,13 +6997,14 @@ bool gicp_plusplus::LocalizationNode::maybeSnapPoseToGT(const char* reason) { Eigen::Quaternionf q_state_new; Eigen::Vector3f p_state_new; - if (snap_delta_ok) { + if (apply_delta) { Eigen::Quaternionf q_corr(Eigen::Matrix3f(T_corr.block<3, 3>(0, 0))); q_corr.normalize(); q_state_new = (q_corr * this->state.q).normalized(); p_state_new = T_corr.block<3, 3>(0, 0) * this->state.p + T_corr.block<3, 1>(0, 3); } else { - // Degenerate pre-snap estimate: absolute overwrite (legacy behavior). + // Catastrophic/wrong-lock branch or degenerate pre-snap estimate: + // overwrite the observer absolutely instead of preserving a bad basin. q_state_new = q_new; p_state_new = p_new; } @@ -6733,11 +7025,14 @@ bool gicp_plusplus::LocalizationNode::maybeSnapPoseToGT(const char* reason) { ++this->geo.update_seq; // discard any in-flight propagateState computations } RCLCPP_INFO(this->get_logger(), - "GT recovery: delta-form snap — correction |t|=%.2f m |rot|=%.2f deg applied to the " + "GT recovery: %s snap — correction |t|=%.2f m |rot|=%.2f deg applied to the " "LIVE observer state%s", + apply_delta ? "delta-form" : "absolute", T_corr.block<3, 1>(0, 3).norm(), rotationDistanceDeg(Eigen::Matrix4f::Identity(), T_corr), - snap_delta_ok ? "" : " (ABSOLUTE fallback: pre-snap estimate non-finite)"); + apply_delta ? "" : + (force_absolute ? " (forced by catastrophic/wrong-lock rejection)" + : " (pre-snap estimate non-finite)")); { // [P2 FIX 2026-07-09] Seed writes under the owner lock (pose -> seed: // maybeSnapPoseToGT runs inside performLocalization's pose_mutex scope). @@ -6748,6 +7043,7 @@ bool gicp_plusplus::LocalizationNode::maybeSnapPoseToGT(const char* reason) { // (median point time) — matching the accept/reject stamps and the // estimate the delta was formed against. this->base_pose_stamp_ = snap_stamp; + this->t_prior_stamp_ = snap_stamp; this->prev_vel = v_base_world; } @@ -7224,77 +7520,20 @@ bool gicp_plusplus::LocalizationNode::imuMeasFromTimeRange( std::lock_guard lock(this->mtx_imu); - out.clear(); - const bool empty = this->imu_buffer.empty(); - const bool invalid_range = - !std::isfinite(start_time) || !std::isfinite(end_time) || - start_time > end_time; - const bool missing_newer = - !empty && this->imu_buffer.front().stamp < end_time; - const double missing_older_s = - empty ? std::numeric_limits::infinity() - : this->imu_buffer.back().stamp - start_time; - const bool missing_older = - !empty && missing_older_s > 0.002; - if (empty || invalid_range || missing_newer || missing_older) { - // Need a monotone window with one real IMU sample on or before start_time - // and one on or after end_time. imu_buffer is newest -> oldest. + constexpr double kOlderToleranceSec = 0.002; + if (!selectBracketedImuRange( + this->imu_buffer, start_time, end_time, kOlderToleranceSec, out)) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 2000, "IMU range unavailable: request=[%.6f,%.6f] buffer=[oldest=%.6f," - "newest=%.6f,size=%zu] empty=%d invalid=%d missing_older=%d " - "missing_newer=%d", + "newest=%.6f,size=%zu]", start_time, end_time, - empty ? -1.0 : this->imu_buffer.back().stamp, - empty ? -1.0 : this->imu_buffer.front().stamp, - this->imu_buffer.size(), empty, invalid_range, missing_older, - missing_newer); + this->imu_buffer.empty() ? -1.0 : this->imu_buffer.back().stamp, + this->imu_buffer.empty() ? -1.0 : this->imu_buffer.front().stamp, + this->imu_buffer.size()); return false; } - - // Walk oldest -> newest while the lock is held. Keep only the newest sample - // at/before start_time, then every sample through the first one at/after - // end_time. The former reverse_iterator-range construction mixed a forward - // iterator boundary with reverse_iterator base semantics and could produce - // an empty slice even though the circular buffer visibly bracketed the - // requested interval. With real FLOAT64 Luminar point times that disabled - // IMU prediction/deskew on every frame and eventually caused high-speed - // GICP loss. - bool have_start_bracket = false; - for (auto it = this->imu_buffer.rbegin(); it != this->imu_buffer.rend(); ++it) { - if (!have_start_bracket) { - if (it->stamp <= start_time || out.empty()) { - // There can be many older samples. Retain only the nearest one so the - // returned slice starts at the interpolation bracket, not at the - // circular buffer's oldest entry. The out.empty() case admits at most - // 2 ms of start-side extrapolation (guarded above), covering the - // sub-millisecond seed-vs-first-IMU phase seen at replay startup. - out.clear(); - out.push_back(*it); - } else if (!out.empty()) { - have_start_bracket = true; - out.push_back(*it); - if (it->stamp >= end_time) { - return out.size() >= 2; - } - } - continue; - } - - out.push_back(*it); - if (it->stamp >= end_time) { - return out.size() >= 2; - } - } - - out.clear(); - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 2000, - "IMU range traversal failed despite bracket guard: request=[%.6f,%.6f] " - "buffer=[oldest=%.6f,newest=%.6f,size=%zu]", - start_time, end_time, this->imu_buffer.back().stamp, - this->imu_buffer.front().stamp, this->imu_buffer.size()); - return false; + return true; } std::vector> diff --git a/GICP_plusplus/test/imu_range_test.cpp b/GICP_plusplus/test/imu_range_test.cpp new file mode 100644 index 00000000..52847001 --- /dev/null +++ b/GICP_plusplus/test/imu_range_test.cpp @@ -0,0 +1,66 @@ +#include + +#include +#include +#include + +#include "gicp_plusplus/imu_range.hpp" + +namespace { + +struct Sample { + double stamp; + int id; +}; + +using Buffer = std::deque; + +TEST(ImuRange, ReturnsNearestBracketsInForwardOrder) { + const Buffer buffer{{4.0, 4}, {3.0, 3}, {2.0, 2}, {1.0, 1}}; + std::vector out; + ASSERT_TRUE(gicp_plusplus::selectBracketedImuRange( + buffer, 1.5, 3.2, 0.002, out)); + ASSERT_EQ(out.size(), 4U); + EXPECT_EQ(out.front().id, 1); + EXPECT_EQ(out.back().id, 4); +} + +TEST(ImuRange, ExactBoundaryStillReturnsTwoSamples) { + const Buffer buffer{{3.0, 3}, {2.0, 2}, {1.0, 1}}; + std::vector out; + ASSERT_TRUE(gicp_plusplus::selectBracketedImuRange( + buffer, 2.0, 2.0, 0.002, out)); + ASSERT_EQ(out.size(), 2U); + EXPECT_EQ(out[0].id, 2); + EXPECT_EQ(out[1].id, 3); +} + +TEST(ImuRange, AllowsBoundedStartSideExtrapolation) { + const Buffer buffer{{2.0, 2}, {1.001, 1}}; + std::vector out; + EXPECT_TRUE(gicp_plusplus::selectBracketedImuRange( + buffer, 1.0, 2.0, 0.002, out)); +} + +TEST(ImuRange, RejectsMissingOlderOrNewerBracket) { + std::vector out; + EXPECT_FALSE(gicp_plusplus::selectBracketedImuRange( + Buffer{{2.0, 2}, {1.003, 1}}, 1.0, 2.0, 0.002, out)); + EXPECT_FALSE(gicp_plusplus::selectBracketedImuRange( + Buffer{{1.9, 2}, {1.0, 1}}, 1.0, 2.0, 0.002, out)); +} + +TEST(ImuRange, RejectsEmptyInvalidAndNonMonotoneInputs) { + std::vector out; + EXPECT_FALSE(gicp_plusplus::selectBracketedImuRange( + Buffer{}, 1.0, 2.0, 0.002, out)); + EXPECT_FALSE(gicp_plusplus::selectBracketedImuRange( + Buffer{{2.0, 2}, {1.0, 1}}, 2.0, 1.0, 0.002, out)); + EXPECT_FALSE(gicp_plusplus::selectBracketedImuRange( + Buffer{{2.0, 2}, {2.0, 1}}, 1.0, 2.0, 0.002, out)); + EXPECT_FALSE(gicp_plusplus::selectBracketedImuRange( + Buffer{{2.0, 2}, {1.0, 1}}, + std::numeric_limits::quiet_NaN(), 2.0, 0.002, out)); +} + +} // namespace diff --git a/GICP_plusplus/test/rtk_gate_test.cpp b/GICP_plusplus/test/rtk_gate_test.cpp index ef0ee52f..c6fd8bf3 100644 --- a/GICP_plusplus/test/rtk_gate_test.cpp +++ b/GICP_plusplus/test/rtk_gate_test.cpp @@ -53,8 +53,13 @@ TEST(RtkGate, ThresholdBoundaryIsInclusive) { EXPECT_FALSE(gicp_plusplus::rtkCovarianceComponentOk(std::nextafter(kMaxXY, 1.0), kMaxXY)); } -TEST(RtkGate, ZeroCovariancePasses) { - EXPECT_TRUE(gicp_plusplus::rtkPositionCovarianceOk(0.0, 0.0, 0.0, kMaxXY, kMaxZ)); +TEST(RtkGate, ZeroCovarianceFailsClosedByDefault) { + EXPECT_FALSE(gicp_plusplus::rtkPositionCovarianceOk(0.0, 0.0, 0.0, kMaxXY, kMaxZ)); +} + +TEST(RtkGate, ZeroCovarianceRequiresExplicitCompatibilityEscape) { + EXPECT_TRUE(gicp_plusplus::rtkPositionCovarianceOk( + 0.0, 0.0, 0.0, kMaxXY, kMaxZ, true)); } TEST(RtkGate, PerAxisThresholdsApply) { diff --git a/GLIM/glim/src/glim/mapping/global_mapping_pose_graph.cpp b/GLIM/glim/src/glim/mapping/global_mapping_pose_graph.cpp index fc912933..efa67789 100644 --- a/GLIM/glim/src/glim/mapping/global_mapping_pose_graph.cpp +++ b/GLIM/glim/src/glim/mapping/global_mapping_pose_graph.cpp @@ -362,6 +362,7 @@ void GlobalMappingPoseGraph::save(const std::string& path) { ofs << boost::format("%.9f %.6f %.6f %.6f %.6f %.6f %.6f %.6f") % stamp % trans.x() % trans.y() % trans.z() % quat.x() % quat.y() % quat.z() % quat.w() << std::endl; }; + bool dense_restore_failed = false; for (int i = 0; i < submaps.size(); i++) { for (const auto& frame : submaps[i]->odom_frames) { write_tum_frame(odom_lidar_ofs, frame->stamp, frame->T_world_lidar); @@ -382,7 +383,19 @@ void GlobalMappingPoseGraph::save(const std::string& path) { const std::string output_submap_dir = (boost::format("%s/%06d") % path % i).str(); submaps[i]->save(output_submap_dir); - restore_offloaded_points(i, output_submap_dir); + try { + restore_offloaded_points(i, output_submap_dir); + } catch (const std::exception& e) { + dense_restore_failed = true; + logger->error( + "failed to restore dense points for submap {}: {}. " + "Continuing so the partial dump remains inspectable.", + i, e.what()); + } + } + if (dense_restore_failed) { + throw std::runtime_error( + "one or more dense submap payloads failed to restore; partial dump retained"); } } @@ -806,7 +819,40 @@ void GlobalMappingPoseGraph::restore_offloaded_points(size_t index, const std::s throw std::runtime_error("dense point offload payload is missing for submap " + std::to_string(index) + ": " + source_dir.string()); } - logger->debug("restored {} dense compact point files for submap {} from {}", restored_files, index, source_dir.string()); + const boost::filesystem::path points_path = + destination_dir / "points_compact.bin"; + if (!boost::filesystem::is_regular_file(points_path)) { + throw std::runtime_error( + "restored dense payload has no points_compact.bin for submap " + + std::to_string(index)); + } + const auto points_bytes = boost::filesystem::file_size(points_path); + if (points_bytes == 0 || points_bytes % (3 * sizeof(float)) != 0) { + throw std::runtime_error( + "restored points_compact.bin has invalid byte size for submap " + + std::to_string(index) + ": " + std::to_string(points_bytes)); + } + const auto dense_point_count = points_bytes / (3 * sizeof(float)); + const boost::filesystem::path metadata_path = destination_dir / "data.txt"; + std::ofstream metadata(metadata_path.string(), std::ios::app); + if (!metadata) { + throw std::runtime_error( + "cannot append dense-payload authority marker to " + + metadata_path.string()); + } + metadata << "points_compact_authoritative: true\n"; + metadata << "points_compact_count: " << dense_point_count << "\n"; + metadata.flush(); + if (!metadata) { + throw std::runtime_error( + "failed writing dense-payload authority marker to " + + metadata_path.string()); + } + + logger->debug( + "restored {} dense compact point files ({} authoritative points) for " + "submap {} from {}", + restored_files, dense_point_count, index, source_dir.string()); offloaded_point_dirs[index] = destination_dir.string(); // The dense payload is now durable in the dump. Reclaim only the temporary diff --git a/GLIM/glim_ext/config/config_gnss_global.json b/GLIM/glim_ext/config/config_gnss_global.json index 062748ba..56fe07ef 100644 --- a/GLIM/glim_ext/config/config_gnss_global.json +++ b/GLIM/glim_ext/config/config_gnss_global.json @@ -22,7 +22,9 @@ // must pass fit_max_rms before the transform can latch. "fit_min_samples": 20, "fit_validation_samples": 10, - "fit_max_rms": 0.25, + // Site-independent compatibility default. Generated deployment profiles + // may tighten this after measuring their GNSS/trajectory noise. + "fit_max_rms": 2.0, // Atlas attitude (dual-antenna HEADING) prior on each submap. Position // anchoring (prior_inf_scale) pins WHERE a submap is, but not its yaw -- // the Atlas dual-antenna heading is a drift-free reference that LiDAR+IMU diff --git a/GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_global_module.hpp b/GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_global_module.hpp index 140d9f2d..191a8d67 100644 --- a/GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_global_module.hpp +++ b/GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_global_module.hpp @@ -157,7 +157,7 @@ class GNSSGlobal : public ExtensionModuleBase { std::max(1, config.param("gnss", "fit_validation_samples", 10)); // Maximum training AND held-out prediction RMS (m) accepted before the // one-shot transform can latch. <= 0 disables both gates. - fit_max_rms = config.param("gnss", "fit_max_rms", 0.25); + fit_max_rms = config.param("gnss", "fit_max_rms", 2.0); if (!std::isfinite(fit_max_rms)) { throw std::invalid_argument("gnss.fit_max_rms must be finite"); } diff --git a/GLIM/glim_ros2/src/glim_pcap_rosbag.cpp b/GLIM/glim_ros2/src/glim_pcap_rosbag.cpp index b57265ad..072fb820 100644 --- a/GLIM/glim_ros2/src/glim_pcap_rosbag.cpp +++ b/GLIM/glim_ros2/src/glim_pcap_rosbag.cpp @@ -932,7 +932,13 @@ int main(int argc, char** argv) { } glim->wait(auto_quit); - glim->save(dump_path); + try { + glim->save(dump_path); + } catch (const std::exception& e) { + hard_error = true; + spdlog::critical( + "GLIM dump save failed after retaining all recoverable submaps: {}", e.what()); + } const size_t num_submaps = glim->num_submaps(); if (num_submaps == 0) { spdlog::critical( diff --git a/GLIM/glim_ros2/src/glim_rosbag.cpp b/GLIM/glim_ros2/src/glim_rosbag.cpp index 46a139a4..75b5cf66 100644 --- a/GLIM/glim_ros2/src/glim_rosbag.cpp +++ b/GLIM/glim_ros2/src/glim_rosbag.cpp @@ -226,6 +226,14 @@ int main(int argc, char** argv) { double start_offset = 0.0; glim->declare_parameter("start_offset", start_offset); glim->get_parameter("start_offset", start_offset); + if (start_offset > 0.0 && bag_filenames.size() > 1) { + spdlog::critical( + "start_offset={} is ambiguous across {} input bag paths. Merge the " + "inputs into one bag or run without start_offset; refusing to seek only " + "the first path.", + start_offset, bag_filenames.size()); + return 1; + } double playback_duration = 0.0; glim->declare_parameter("playback_duration", playback_duration); @@ -315,6 +323,8 @@ int main(int argc, char** argv) { std::vector aux_ordinal_next; std::vector indexed_primary_bag_times_s; std::vector> indexed_aux_bag_times_s; + bool seek_verification_pending = false; + double seek_expected_first_primary_s = -1.0; struct PendingPrimaryScan { sensor_msgs::msg::PointCloud2::SharedPtr msg; double enqueue_bag_time_s = 0.0; @@ -599,6 +609,17 @@ int main(int argc, char** argv) { const double seek_time_s = seek_time / 1e9; primary_ordinal_next = std::distance(indexed_primary_bag_times_s.begin(), std::lower_bound(indexed_primary_bag_times_s.begin(), indexed_primary_bag_times_s.end(), seek_time_s)); + if (primary_ordinal_next < indexed_primary_bag_times_s.size()) { + seek_expected_first_primary_s = + indexed_primary_bag_times_s[primary_ordinal_next]; + seek_verification_pending = true; + } else { + spdlog::warn( + "two-pass join seek has no indexed primary at/after {:.6f}; " + "disabling the plan and using streaming matching", + seek_time_s); + two_pass_active = false; + } for (size_t i = 0; i < aux_ordinal_next.size(); ++i) { aux_ordinal_next[i] = std::distance(indexed_aux_bag_times_s[i].begin(), std::lower_bound(indexed_aux_bag_times_s[i].begin(), indexed_aux_bag_times_s[i].end(), seek_time_s)); @@ -734,6 +755,37 @@ int main(int argc, char** argv) { glim->imu_callback(imu_msg); } } else if (msg->topic_name == points_topic) { + if (seek_verification_pending) { + seek_verification_pending = false; + constexpr double kSeekVerificationToleranceSec = 1e-6; + const double seek_error_s = + std::abs(latest_bag_time_s - seek_expected_first_primary_s); + if (seek_error_s > kSeekVerificationToleranceSec) { + spdlog::error( + "two-pass seek verification failed: first streamed primary " + "bag_time={:.9f}, indexed={:.9f}, error={:.3f} ms. Disabling " + "the plan and falling back to streaming matching.", + latest_bag_time_s, seek_expected_first_primary_s, + seek_error_s * 1e3); + for (size_t i = 0; i < planned_aux_store.size(); ++i) { + for (auto& entry : planned_aux_store[i]) { + aux_sensors[i].buffer.push_back(std::move(entry.second.cloud)); + } + while (aux_sensors[i].buffer.size() > aux_sensors[i].buffer_size) { + aux_sensors[i].buffer.pop_front(); + } + planned_aux_store[i].clear(); + } + merge_plan.clear(); + planned_aux_ordinals.clear(); + two_pass_active = false; + } else { + spdlog::info( + "two-pass seek verification passed: first primary {:.9f} " + "matches the indexed stream", + latest_bag_time_s); + } + } if (topic_type != "sensor_msgs/msg/PointCloud2") { g_bag_hard_error = true; spdlog::error("topic_type mismatch: {} != sensor_msgs/msg/PointCloud2 (topic={})", topic_type, msg->topic_name); @@ -1238,7 +1290,13 @@ int main(int argc, char** argv) { } glim->wait(auto_quit); - glim->save(dump_path); + try { + glim->save(dump_path); + } catch (const std::exception& e) { + g_bag_hard_error = true; + spdlog::critical( + "GLIM dump save failed after retaining all recoverable submaps: {}", e.what()); + } const size_t num_submaps = glim->num_submaps(); if (num_submaps == 0) { diff --git a/README.md b/README.md index bd5b0ffe..9bcb5f38 100644 --- a/README.md +++ b/README.md @@ -319,12 +319,24 @@ If you ever switch sensors and the deskew looks wrong, use the one-shot diagnost --bag /path/to/DATASET_ROOT/prep_bag/_front_atlas_gicp \ --run-name _compressed_full_1x \ --overlay /path/to/gicp/install/setup.bash \ + --mode gnss_aided \ + --reference-is-gt-ack \ --config-path GICP_plusplus/cfg/front_quality_replay.yaml \ --start-offset 0 \ --duration \ --rate 1.0 \ --primary-queue-size 32 ``` + `gnss_aided` explicitly labels that Atlas participates in localization. + When the same Atlas odometry is also the score reference, the acknowledgement + flag is mandatory because that evidence is not independent truth. + `--mode independent` instead requires a `--reference-topic` distinct from + the runtime `--gt-topic`; a YAML profile alone cannot make the same aided + stream independent truth. The optional + `GICP_plusplus/cfg/front_no_atlas_translation_replay.yaml` removes + per-scan Atlas translation seeding/gating for a registration A/B, but does + not relabel its evidence as independent. Acceptance, rejection-streak, + debug-coverage, and zero-drop gates are explicit runner flags. The offline audit uses RELIABLE LiDAR publication/subscription on both sides so a large PointCloud2 cannot disappear in DDS without accounting. Its 50,000-message rosbag read-ahead queue keeps storage/decompression diff --git a/gicp_localization/cfg/localization.yaml b/gicp_localization/cfg/localization.yaml index 50213c72..3c9b9520 100644 --- a/gicp_localization/cfg/localization.yaml +++ b/gicp_localization/cfg/localization.yaml @@ -214,10 +214,7 @@ # (false yaw pressure). Read the value off the per-aux offset diagnostic # ("header offset vs primary ... mean=+X ms" -> enter -X/1000 here); # runs 19/20 measured 80-90 ms offsets. - # Keep an explicitly typed vector for ROS 2 parameter parsing. An empty - # YAML sequence has no element type and Jazzy rejects it before node - # startup; zeros preserve the intended "no measured correction" behavior. - localization/lidar_concat/aux_time_offsets: [0.0, 0.0] + localization/lidar_concat/aux_time_offsets: [] localization/lidar_concat/buffer_size: 200 # per-aux ring buffer depth (P4#3: raised 20 -> 200 # for GLIM parity; 20 = only 2 s of aux history at # 10 Hz, so brief stalls degraded frames to fewer diff --git a/scripts/build_consistent_pcd.py b/scripts/build_consistent_pcd.py index 5c1e4bb4..8efe5f68 100755 --- a/scripts/build_consistent_pcd.py +++ b/scripts/build_consistent_pcd.py @@ -58,7 +58,9 @@ def matrix_from_manifest(manifest: dict, key: str) -> np.ndarray: return matrix -def read_manifest(pcd_path: Path) -> tuple[Path, dict]: +def read_manifest( + pcd_path: Path, allow_missing_origin: bool = False +) -> tuple[Path, dict]: path = pcd_path.with_suffix(pcd_path.suffix + ".manifest.yaml") if not path.is_file(): raise FileNotFoundError(f"required map provenance manifest not found: {path}") @@ -68,8 +70,24 @@ def read_manifest(pcd_path: Path) -> tuple[Path, dict]: raise ValueError(f"{path}: expected a YAML mapping") if manifest.get("frame") != "enu": raise ValueError(f"{path}: input map frame must be 'enu'") - if not manifest.get("enu_origin"): + matrix_from_manifest(manifest, "T_world_utm") + matrix_from_manifest(manifest, "T_output_enu_input_enu") + origin = str(manifest.get("enu_origin", "")).split("#", 1)[0].strip() + if not origin or origin.startswith("UNSPECIFIED"): + if allow_missing_origin: + return path, manifest raise ValueError(f"{path}: input map must declare enu_origin") + parts = [part for part in re.split(r"[\s,]+", origin) if part] + if len(parts) != 3: + raise ValueError(f"{path}: enu_origin must be lat,lon,alt, got {origin!r}") + try: + lat, lon, alt = (float(part) for part in parts) + except ValueError as exc: + raise ValueError(f"{path}: non-numeric enu_origin {origin!r}") from exc + if not all(math.isfinite(value) for value in (lat, lon, alt)): + raise ValueError(f"{path}: non-finite enu_origin {origin!r}") + if not (-90.0 <= lat <= 90.0 and -180.0 <= lon <= 180.0): + raise ValueError(f"{path}: enu_origin latitude/longitude out of range") return path, manifest @@ -286,6 +304,12 @@ def main() -> int: parser.add_argument("--voxel-size", type=float, default=0.15) parser.add_argument("--min-sessions", type=int, default=2) parser.add_argument("--corridor-trajectory", type=Path, required=True) + parser.add_argument( + "--corridor-source-index", + type=int, + help="0-based input_pcd index whose GLIM trajectory/transform produced " + "--corridor-trajectory; mandatory for multi-source builds.", + ) parser.add_argument( "--corridor-index-range", type=str, @@ -306,10 +330,21 @@ def main() -> int: action="store_true", help="Replace an existing output PCD/manifest.", ) + parser.add_argument( + "--allow-missing-origin", + action="store_true", + help="compatibility escape for legacy UNSPECIFIED origins; unsafe for deployment maps", + ) args = parser.parse_args() if len(args.input_pcd) < 2: parser.error("at least two input PCDs are required for a consistency map") + if args.corridor_source_index is None: + parser.error( + "--corridor-source-index is mandatory when multiple source maps are used" + ) + if not 0 <= args.corridor_source_index < len(args.input_pcd): + parser.error("--corridor-source-index is outside the input_pcd list") if not math.isfinite(args.voxel_size) or args.voxel_size <= 0.0: parser.error("--voxel-size must be finite and > 0") if args.min_sessions < 2 or args.min_sessions > len(args.input_pcd): @@ -323,11 +358,25 @@ def main() -> int: except ValueError as exc: parser.error(f"--corridor-index-range invalid: {exc}") - manifest_pairs = [read_manifest(path) for path in args.input_pcd] - reference_origin = str(manifest_pairs[0][1]["enu_origin"]).split("#", 1)[0].strip() - coverage_manifest_pairs = [read_manifest(path) for path in args.coverage_pcd] + all_input_paths = [path.resolve() for path in args.input_pcd + args.coverage_pcd] + if len(set(all_input_paths)) != len(all_input_paths): + parser.error("duplicate input/coverage PCD paths are not allowed") + if args.output_pcd.resolve() in set(all_input_paths): + parser.error("output PCD must not collide with an input or coverage PCD") + + manifest_pairs = [ + read_manifest(path, args.allow_missing_origin) + for path in args.input_pcd + ] + reference_origin = str( + manifest_pairs[0][1].get("enu_origin", "UNSPECIFIED") + ).split("#", 1)[0].strip() + coverage_manifest_pairs = [ + read_manifest(path, args.allow_missing_origin) + for path in args.coverage_pcd + ] for manifest_path, manifest in manifest_pairs[1:] + coverage_manifest_pairs: - origin = str(manifest["enu_origin"]).split("#", 1)[0].strip() + origin = str(manifest.get("enu_origin", "UNSPECIFIED")).split("#", 1)[0].strip() if origin != reference_origin: raise SystemExit( f"ENU datum mismatch: {manifest_path} has {origin!r}, " @@ -368,7 +417,7 @@ def main() -> int: centerline = transformed_centerline( args.corridor_trajectory, corridor_range, - manifest_pairs[0][1], + manifest_pairs[args.corridor_source_index][1], args.centerline_stride, ) tree = cKDTree(centerline[:, :2]) @@ -462,6 +511,13 @@ def main() -> int: "coverage_voxels_added": coverage_added_counts, "corridor_radius_m": float(args.corridor_radius), "corridor_trajectory": str(args.corridor_trajectory.resolve()), + "corridor_source_index": int(args.corridor_source_index), + "corridor_source_map": str( + args.input_pcd[args.corridor_source_index].resolve() + ), + "corridor_source_manifest": str( + manifest_pairs[args.corridor_source_index][0].resolve() + ), "corridor_index_range": args.corridor_index_range, "corridor_centerline_stride": int(args.centerline_stride), "algorithm": ( @@ -472,10 +528,24 @@ def main() -> int: } # Keep the exact upstream transforms, so the driven centerline and map # frame remain independently auditable. - manifest["T_world_utm"] = manifest_pairs[0][1]["T_world_utm"] - manifest["T_output_enu_input_enu"] = manifest_pairs[0][1][ + corridor_manifest = manifest_pairs[args.corridor_source_index][1] + manifest["T_world_utm"] = corridor_manifest["T_world_utm"] + manifest["T_output_enu_input_enu"] = corridor_manifest[ "T_output_enu_input_enu" ] + manifest["source_transforms"] = [ + { + "source_index": index, + "pcd": str(pcd.resolve()), + "T_world_utm": source_manifest["T_world_utm"], + "T_output_enu_input_enu": source_manifest[ + "T_output_enu_input_enu" + ], + } + for index, (pcd, (_, source_manifest)) in enumerate( + zip(args.input_pcd, manifest_pairs) + ) + ] manifest["output_formula"] = ( "consistent_voxels + unique_voxels_kept_outside_corridor " "+ globally-new coverage_voxels_added" diff --git a/scripts/prepare_gicp_replay_bag.py b/scripts/prepare_gicp_replay_bag.py index 4fdd254c..841c0f78 100755 --- a/scripts/prepare_gicp_replay_bag.py +++ b/scripts/prepare_gicp_replay_bag.py @@ -123,6 +123,8 @@ def parse_args() -> argparse.Namespace: def main() -> int: args = parse_args() bags = [Path(value).expanduser().resolve(strict=True) for value in args.bag] + if len(set(bags)) != len(bags): + raise SystemExit("Duplicate --bag inputs are not allowed") if args.out: output = Path(args.out).expanduser().resolve(strict=False) @@ -165,6 +167,10 @@ def main() -> int: f"Cross-dataset input refused: {bag} belongs to {source_root}, " f"output belongs to {dataset_root}" ) + if output == bag or is_relative_to(output, bag) or is_relative_to(bag, output): + raise SystemExit( + f"Input/output path collision refused: input={bag} output={output}" + ) if output.exists(): raise SystemExit(f"Refusing to overwrite output bag: {output}") @@ -173,7 +179,9 @@ def main() -> int: raise SystemExit("Every retained topic must be an absolute ROS topic") prep_root.mkdir(parents=True, exist_ok=True) - config_dir = prep_root / "configs" + # Dry-run is intentionally isolated so it cannot occupy the production + # conversion-config pathname and block the subsequent real conversion. + config_dir = prep_root / ("dry_run_configs" if args.dry_run else "configs") config_dir.mkdir(parents=True, exist_ok=True) config_path = config_dir / f"{output.name}.convert.yaml" if config_path.exists(): diff --git a/scripts/run_gicp_replay_audit.sh b/scripts/run_gicp_replay_audit.sh index 83c465b6..1402f6d2 100755 --- a/scripts/run_gicp_replay_audit.sh +++ b/scripts/run_gicp_replay_audit.sh @@ -28,6 +28,12 @@ usage() { ' --imu-topic TOPIC default /gps_p1/imu' \ ' --gt-topic TOPIC default /gps_p1/filtered_odom' \ ' --reference-topic TOPIC defaults to --gt-topic' \ + ' --mode MODE evidence label: gnss_aided | independent' \ + ' --reference-is-gt-ack acknowledge aided scoring is not independent truth' \ + ' --min-accept-rate FRACTION scorecard gate; default 0 (disabled)' \ + ' --max-rejection-streak N scorecard gate; default 0 (disabled)' \ + ' --min-debug-coverage FRACTION fail if debug frames/input scans is lower; default 0.80' \ + ' --require-zero-drops fail on front drops or timestamp resets' \ ' --primary-queue-size N default 8' \ ' --read-ahead-queue-size N rosbag playback prefetch; default 50000' \ ' --config-path YAML run-local overrides loaded after package defaults' \ @@ -51,6 +57,12 @@ POINTCLOUD_TOPIC=/luminar_front/points IMU_TOPIC=/gps_p1/imu GT_TOPIC=/gps_p1/filtered_odom REFERENCE_TOPIC= +MODE= +REFERENCE_IS_GT_ACK=false +MIN_ACCEPT_RATE=0 +MAX_REJECTION_STREAK=0 +MIN_DEBUG_COVERAGE=0.80 +REQUIRE_ZERO_DROPS=false PRIMARY_QUEUE_SIZE=8 READ_AHEAD_QUEUE_SIZE=50000 CONFIG_PATH= @@ -81,6 +93,12 @@ while [[ $# -gt 0 ]]; do --imu-topic) IMU_TOPIC="${2:?missing value}"; shift 2 ;; --gt-topic) GT_TOPIC="${2:?missing value}"; shift 2 ;; --reference-topic) REFERENCE_TOPIC="${2:?missing value}"; shift 2 ;; + --mode) MODE="${2:?missing value}"; shift 2 ;; + --reference-is-gt-ack) REFERENCE_IS_GT_ACK=true; shift ;; + --min-accept-rate) MIN_ACCEPT_RATE="${2:?missing value}"; shift 2 ;; + --max-rejection-streak) MAX_REJECTION_STREAK="${2:?missing value}"; shift 2 ;; + --min-debug-coverage) MIN_DEBUG_COVERAGE="${2:?missing value}"; shift 2 ;; + --require-zero-drops) REQUIRE_ZERO_DROPS=true; shift ;; --primary-queue-size) PRIMARY_QUEUE_SIZE="${2:?missing value}"; shift 2 ;; --read-ahead-queue-size) READ_AHEAD_QUEUE_SIZE="${2:?missing value}"; shift 2 ;; --config-path) CONFIG_PATH="${2:?missing value}"; shift 2 ;; @@ -113,25 +131,46 @@ if [[ ${#BAGS[@]} -eq 0 ]]; then printf 'At least one --bag is required\n' >&2 exit 2 fi +if [[ "$MODE" != "gnss_aided" && "$MODE" != "independent" ]]; then + printf '%s\n' '--mode must be explicitly set to gnss_aided or independent' >&2 + exit 2 +fi if [[ ! "$PRIMARY_QUEUE_SIZE" =~ ^[1-9][0-9]*$ || - ! "$READ_AHEAD_QUEUE_SIZE" =~ ^[1-9][0-9]*$ ]]; then + ! "$READ_AHEAD_QUEUE_SIZE" =~ ^[1-9][0-9]*$ || + ! "$MAX_REJECTION_STREAK" =~ ^[0-9]+$ ]]; then printf 'Queue sizes must be positive integers\n' >&2 exit 2 fi +if ! awk -v a="$MIN_ACCEPT_RATE" -v c="$MIN_DEBUG_COVERAGE" \ + 'BEGIN { exit !(a >= 0 && a <= 1 && c >= 0 && c <= 1) }'; then + printf 'Acceptance and coverage thresholds must be fractions in [0,1]\n' >&2 + exit 2 +fi + +resolve_existing() { + local label="$1" + local path="$2" + local resolved + if ! resolved="$(realpath -e -- "$path" 2>/dev/null)"; then + printf '%s does not exist: %s\n' "$label" "$path" >&2 + return 1 + fi + printf '%s\n' "$resolved" +} -MAP="$(realpath -e "$MAP")" -OVERLAY="$(realpath -e "$OVERLAY")" +MAP="$(resolve_existing Map "$MAP")" || exit 3 +OVERLAY="$(resolve_existing Overlay "$OVERLAY")" || exit 3 for index in "${!BAGS[@]}"; do - BAGS[$index]="$(realpath -e "${BAGS[$index]}")" + BAGS[$index]="$(resolve_existing Bag "${BAGS[$index]}")" || exit 3 done if [[ -n "$QOS_OVERRIDES" ]]; then - QOS_OVERRIDES="$(realpath -e "$QOS_OVERRIDES")" + QOS_OVERRIDES="$(resolve_existing 'QoS overrides' "$QOS_OVERRIDES")" || exit 3 fi if [[ -n "$BRIDGE_SCRIPT" ]]; then - BRIDGE_SCRIPT="$(realpath -e "$BRIDGE_SCRIPT")" + BRIDGE_SCRIPT="$(resolve_existing 'Bridge script' "$BRIDGE_SCRIPT")" || exit 3 fi if [[ -n "$CONFIG_PATH" ]]; then - CONFIG_PATH="$(realpath -e "$CONFIG_PATH")" + CONFIG_PATH="$(resolve_existing 'Config path' "$CONFIG_PATH")" || exit 3 fi if [[ "$MAP" == */maps/* ]]; then @@ -169,6 +208,20 @@ fi if [[ -z "$REFERENCE_TOPIC" ]]; then REFERENCE_TOPIC="$GT_TOPIC" fi +if [[ "$MODE" == "gnss_aided" && "$REFERENCE_TOPIC" == "$GT_TOPIC" && + "$REFERENCE_IS_GT_ACK" != "true" ]]; then + printf '%s\n' \ + 'The GNSS-aided run uses the same topic for seeding/gating and scoring.' \ + 'Pass --reference-is-gt-ack to label and acknowledge this non-independent evidence,' \ + 'or pass a genuinely independent --reference-topic.' >&2 + exit 2 +fi +if [[ "$MODE" == "independent" && "$REFERENCE_TOPIC" == "$GT_TOPIC" ]]; then + printf '%s\n' \ + 'Independent evidence requires --reference-topic to differ from the runtime --gt-topic.' \ + 'A parameter profile alone cannot turn the same aided stream into independent truth.' >&2 + exit 2 +fi if [[ ${#PLAY_TOPICS[@]} -eq 0 ]]; then PLAY_TOPICS=( "$POINTCLOUD_TOPIC" @@ -194,6 +247,7 @@ launch_pid= record_pid= reference_record_pid= resource_pid= +playback_pid= stop_pid() { local pid="${1:-}" @@ -223,6 +277,7 @@ stop_launch() { } cleanup() { + stop_pid "$playback_pid" stop_pid "$record_pid" stop_pid "$reference_record_pid" stop_launch @@ -285,13 +340,37 @@ record_pid=$! ros2 bag record --storage mcap --output "$RUN_DIR/reference_topics_bag" \ "$REFERENCE_TOPIC" >"$RUN_DIR/reference_record.log" 2>&1 & reference_record_pid=$! -sleep 2 + +wait_for_subscription() { + local topic="$1" + local label="$2" + local count + for _ in {1..60}; do + count="$(ros2 topic info "$topic" 2>/dev/null | + awk '/Subscription count:/ {print $3; exit}')" + if [[ "$count" =~ ^[1-9][0-9]*$ ]]; then + return 0 + fi + if ! kill -0 "$record_pid" 2>/dev/null || + ! kill -0 "$reference_record_pid" 2>/dev/null; then + printf 'Recorder exited while waiting for %s subscription\n' "$label" >&2 + return 1 + fi + sleep 0.25 + done + printf 'Timed out waiting for recorder subscription: %s (%s)\n' "$label" "$topic" >&2 + return 1 +} + +wait_for_subscription /gicp/localization/debug/fitness 'debug evidence' || exit 5 +wait_for_subscription "$REFERENCE_TOPIC" 'reference evidence' || exit 5 declare -a play_args=() for bag in "${BAGS[@]}"; do play_args+=(-i "$bag" "$STORAGE_ID") done play_args+=( + --start-paused --read-ahead-queue-size "$READ_AHEAD_QUEUE_SIZE" --rate "$RATE" --start-offset "$START_OFFSET" @@ -301,13 +380,40 @@ play_args+=( --topics ) play_args+=("${PLAY_TOPICS[@]}") -if [[ "$LIDAR_RELIABLE_QOS" == "true" ]]; then +if [[ -n "$QOS_OVERRIDES" ]]; then play_args+=(--qos-profile-overrides-path "$QOS_OVERRIDES") fi play_start_ns="$(date +%s%N)" -ros2 bag play "${play_args[@]}" >"$RUN_DIR/playback.log" 2>&1 -playback_exit=$? +ros2 bag play "${play_args[@]}" >"$RUN_DIR/playback.log" 2>&1 & +playback_pid=$! +resume_ready=0 +for _ in {1..120}; do + if ros2 service list 2>/dev/null | grep -qx '/rosbag2_player/resume'; then + resume_ready=1 + break + fi + if ! kill -0 "$playback_pid" 2>/dev/null; then + break + fi + sleep 0.25 +done +if [[ "$resume_ready" -ne 1 ]]; then + printf 'rosbag player exited or never exposed the resume service\n' >&2 + playback_exit=7 + stop_pid "$playback_pid" +else + if ! ros2 service call /rosbag2_player/resume rosbag2_interfaces/srv/Resume '{}' \ + >"$RUN_DIR/resume.log" 2>&1; then + printf 'Failed to resume paused rosbag playback\n' >&2 + playback_exit=8 + stop_pid "$playback_pid" + else + wait "$playback_pid" + playback_exit=$? + fi +fi +playback_pid= play_end_ns="$(date +%s%N)" sleep 3 @@ -354,17 +460,103 @@ play_wall_s="$(awk -v start="$play_start_ns" -v end="$play_end_ns" \ if [[ -n "$CONFIG_PATH" ]]; then printf 'config_sha256=%s\n' "$(sha256sum "$CONFIG_PATH" | awk '{print $1}')" fi + printf 'qos_overrides=%s\n' "$QOS_OVERRIDES" + printf 'play_topics=%s\n' "${PLAY_TOPICS[*]}" + printf 'bridge_script=%s\n' "$BRIDGE_SCRIPT" + printf 'bridge_args=%s\n' "${BRIDGE_ARGS[*]}" printf 'pointcloud_topic=%s\n' "$POINTCLOUD_TOPIC" printf 'imu_topic=%s\n' "$IMU_TOPIC" printf 'gt_topic=%s\n' "$GT_TOPIC" printf 'reference_topic=%s\n' "$REFERENCE_TOPIC" + printf 'mode=%s\n' "$MODE" + printf 'reference_is_gt_ack=%s\n' "$REFERENCE_IS_GT_ACK" + printf 'min_accept_rate=%s\n' "$MIN_ACCEPT_RATE" + printf 'max_rejection_streak=%s\n' "$MAX_REJECTION_STREAK" + printf 'min_debug_coverage=%s\n' "$MIN_DEBUG_COVERAGE" + printf 'require_zero_drops=%s\n' "$REQUIRE_ZERO_DROPS" } >"$RUN_DIR/run_status.env" +ros2 bag info "$RUN_DIR/debug_topics_bag" \ + >"$RUN_DIR/debug_topics_bag.info" 2>"$RUN_DIR/debug_topics_bag.info.err" +debug_info_exit=$? +ros2 bag info "$RUN_DIR/reference_topics_bag" \ + >"$RUN_DIR/reference_topics_bag.info" 2>"$RUN_DIR/reference_topics_bag.info.err" +reference_info_exit=$? +bag_message_count() { + awk '/^Messages:/ {print $2; exit}' "$1" +} +debug_messages="$(bag_message_count "$RUN_DIR/debug_topics_bag.info")" +reference_messages="$(bag_message_count "$RUN_DIR/reference_topics_bag.info")" +debug_messages="${debug_messages:-0}" +reference_messages="${reference_messages:-0}" + +declare -a analyzer_args=( + "$RUN_DIR/localization.log" + --json-out "$RUN_DIR/scan_debug_scorecard.json" + --mode "$MODE" + --min-accept-rate "$MIN_ACCEPT_RATE" + --max-rejection-streak "$MAX_REJECTION_STREAK" +) +if [[ "$REQUIRE_ZERO_DROPS" == "true" ]]; then + analyzer_args+=(--require-zero-drops) +fi python3 "$SCRIPT_DIR/../GICP_plusplus/scripts/analyze_scan_debug_log.py" \ - "$RUN_DIR/localization.log" \ + "${analyzer_args[@]}" \ >"$RUN_DIR/scan_debug_scorecard.md" \ - 2>"$RUN_DIR/scan_debug_scorecard.err" || true + 2>"$RUN_DIR/scan_debug_scorecard.err" +analyzer_exit=$? + +debug_frames=0 +if [[ -s "$RUN_DIR/scan_debug_scorecard.json" ]]; then + debug_frames="$(python3 -c \ + 'import json,sys; print(json.load(open(sys.argv[1]))["frames"])' \ + "$RUN_DIR/scan_debug_scorecard.json")" +fi +expected_input_frames=0 +for index in "${!BAGS[@]}"; do + bag="${BAGS[$index]}" + bag_info="$RUN_DIR/input_$(printf '%02d' "$index").info" + ros2 bag info "$bag" >"$bag_info" 2>"$bag_info.err" || continue + bag_duration="$(sed -n 's/^Duration:[[:space:]]*\([0-9.]*\)s.*/\1/p' "$bag_info" | head -1)" + topic_count="$(awk -v topic="$POINTCLOUD_TOPIC" \ + 'index($0, "Topic: " topic " ") { + if (match($0, /Count: [0-9]+/)) { + value=substr($0, RSTART+7, RLENGTH-7); print value; exit + } + }' "$bag_info")" + if [[ -n "$bag_duration" && -n "$topic_count" ]]; then + estimate="$(awk -v count="$topic_count" -v total="$bag_duration" \ + -v start="$START_OFFSET" -v duration="$DURATION" \ + 'BEGIN { + available=total-start; if (available < 0) available=0; + window=(duration < available ? duration : available); + estimated=(total > 0 ? count*window/total : 0); + printf "%d", estimated + }')" + expected_input_frames=$((expected_input_frames + estimate)) + fi +done +debug_coverage="$(awk -v actual="$debug_frames" -v expected="$expected_input_frames" \ + 'BEGIN { + coverage=(expected > 0 ? actual/expected : 0); + printf "%.6f", coverage + }')" +{ + printf 'debug_bag_info_exit=%s\n' "$debug_info_exit" + printf 'reference_bag_info_exit=%s\n' "$reference_info_exit" + printf 'debug_messages=%s\n' "$debug_messages" + printf 'reference_messages=%s\n' "$reference_messages" + printf 'analyzer_exit=%s\n' "$analyzer_exit" + printf 'debug_frames=%s\n' "$debug_frames" + printf 'expected_input_frames=%s\n' "$expected_input_frames" + printf 'debug_coverage=%s\n' "$debug_coverage" +} >>"$RUN_DIR/run_status.env" -if [[ "$playback_exit" -ne 0 || "$launch_alive" -ne 1 ]]; then +if [[ "$playback_exit" -ne 0 || "$launch_alive" -ne 1 || + "$debug_info_exit" -ne 0 || "$reference_info_exit" -ne 0 || + "$debug_messages" -eq 0 || "$reference_messages" -eq 0 || + "$analyzer_exit" -ne 0 || "$expected_input_frames" -eq 0 ]] || + ! awk -v actual="$debug_coverage" -v minimum="$MIN_DEBUG_COVERAGE" \ + 'BEGIN { exit !(actual >= minimum) }'; then exit 6 fi