Fix GNSS validation and add 1x compressed-map GICP pipeline

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()

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

GICP_plusplus/cfg/front_quality_replay.yaml

@@ -0,0 +1,41 @@
+/**:
+  ros__parameters:
+    # 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
+    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
+
+    # 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
+
+    # 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

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

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,6 +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
+    # 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
@@ -149,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)
 
@@ -212,6 +219,9 @@
     # the bytes to the primary PointCloud2. Downstream steps see a single cloud in
     # primary frame with one coherent timebase.
     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.1   # seconds; NON-LUMINAR fallback matching + tie-break only.
@@ -240,7 +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.
-    localization/lidar_concat/aux_time_offsets: []
+    # 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
@@ -424,6 +437,7 @@
     # 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
+    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
@@ -490,6 +504,11 @@
     # GICP Registration Parameters
     # 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/maxOptimizationTimeMsCooperative: 0.0
 
     # Number of neighbors used to compute per-point covariances (typical: 20)
     gicp/correspondenceRandomness: 20
@@ -671,6 +690,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

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 <cmath>
+#include <limits>
+#include <vector>
+
+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 <typename Container>
+bool selectBracketedImuRange(
+    const Container& newest_to_oldest, double start_time, double end_time,
+    double older_tolerance_s,
+    std::vector<typename Container::value_type>& 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<double>::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<double>::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