A Numerically-Robust ROS 2 Port of iG-LIO: Diagnosing and Fixing Toolchain-Induced Failures in Incremental GICP LiDAR-Inertial Odometry

Technical Summary: A Numerically-Robust ROS 2 Port of iG-LIO

Problem Statement
The original iG-LIO (Incremental GICP LiDAR-Inertial Odometry) system, a tightly-coupled estimator fusing generalized-ICP and point-to-plane constraints, was implemented for ROS 1, which is now end-of-life. While a mechanical migration of the codebase to ROS 2 (converting from catkin to ament_cmake, roscpp to rclcpp, etc.) appeared successful, the port failed silently in practice. Despite leaving the estimation mathematics unchanged, the system diverged with NaN internal values and aborted. The root causes were identified not as algorithmic flaws, but as environment-induced failures specific to the modern ROS 2 toolchain and its default configurations.

Methodology and Diagnosis
The authors performed a "mechanically faithful" port of iG-LIO to ROS 2 Jazzy, preserving the core estimation logic (voxel map, GICP + point-to-plane fusion, iterated error-state Kalman filter). The investigation focused on isolating why the ported system failed while the original ROS 1 version functioned correctly. Two distinct, environment-level root causes were identified:

1. QoS Mismatch (IMU Data Loss): The initial port subscribed to IMU data using rclcpp::SensorDataQoS() (Best Effort, Keep Last 5), while the data sources (drivers or bags) published with Reliable QoS. In ROS 2, this pairing silently degrades to Best Effort, causing IMU samples to be dropped and reordered under load. This corrupted the signal required for the iterated filter's propagation.
2. Uninitialized Parallel Reduction (oneTBB + Eigen): The constraint assembly utilized tbb::parallel_reduce with a fixed-size Eigen::Matrix as the reduction value type. The version of oneTBB bundled with modern ROS 2 distributions default-constructs split accumulators. Since a fixed-size Eigen::Matrix does not zero-initialize its storage upon default construction, uninitialized memory was summed into the Hessian matrix ($H$) and vector ($b$), resulting in NaN values. This issue was invisible on the older toolchain used for the original code but surfaced immediately on the newer ROS 2 toolchain.

Key Contributions
The paper details the diagnosis and correction of these failures, alongside several engineering enhancements to sensor support and usability:

• QoS Correction: The IMU subscription was updated to use RELIABLE QoS with a deep queue (KeepLast(2000)), while LiDAR queues were set to KeepLast(10). A runtime guard was added to reject non-physical prediction time steps ($\Delta t \le 0$ or $\Delta t > 0.5$ s).
• Numerical Robustness Fix: The uninitialized reduction accumulator was resolved by wrapping the Eigen::Matrix in a small struct. This struct's members are explicitly zero-initialized in-class (via Eigen::Matrix::Zero()) and joined via operator+. This preserves the upstream's single fused tbb::parallel_reduce structure while ensuring every split TBB task starts with a zeroed identity value, eliminating the uninitialized read fault without sacrificing parallelism.
• Sensor Parsing Updates:
  • Ouster: Updated the point struct/parser to match the current PointCloud2 field layout for Rev 7 sensors, restoring motion de-skewing capabilities broken in the original code.
  • Velodyne: Added support for the Velodyne Velarray M1600.
  • Livox: Implemented two ingestion paths. A compile-time-gated path for the driver-specific CustomMsg (which fails gracefully if the driver is missing) and a driver-free path for sensors publishing standard sensor_msgs/PointCloud2 (e.g., Mid-360), ensuring usability without external driver dependencies.
• Configuration and Output: Exposed QoS reliability, frame names, and output directories via YAML. The system now publishes odometry covariance in a REP-105 compliant frame and writes trajectories in TUM format for evaluation with evo.

Results and Validation
The corrected port was validated on hardware using Ouster OS0 Rev7, Ouster OS1 Rev7, and Livox MID-360 sensors. Qualitative validation involved overlaying the estimated trajectory from the ROS 2 port against the original ROS 1 implementation for the same sequence. The results were "qualitatively identical," confirming that the migration and numerical fixes preserved the estimator's behavior. The system successfully eliminated the NaN divergence and SO3::exp(nan) aborts observed in the initial port.

Significance and Claims
The authors explicitly state that this report serves as a citable reference for the port itself, not a claim on the underlying algorithm's performance or novelty, which belongs to the original iG-LIO authors [1]. The significance of this work lies in two generalizable lessons for robotics software engineering:

1. QoS as a Correctness Contract: In ROS 2, Quality-of-Service settings are not merely throughput concerns but critical correctness contracts for tightly-coupled estimators; implicit transport guarantees of ROS 1 no longer apply.
2. Toolchain Sensitivity: Parallel reduction operations over fixed-size Eigen matrices must be handled with explicit initialization when using modern oneTBB distributions, as default constructors no longer guarantee zero-initialization.

The work demonstrates that a mechanically correct port is insufficient for numerical stability; specific attention to the target environment's toolchain and communication contracts is required to prevent silent failures.