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

Jul 16, 2026

Upload by Aria

iG-LIO is a tightly coupled LiDAR-inertial odometry system fusing generalized-ICP and point-to-plane constraints in an iterated error-state Kalman filter over an incremental voxel map. The report introduces an open-source ROS 2 Jazzy port of the original ROS 1 implementation and diagnoses environment-induced numerical failures that manifest only after porting, including NaN divergences. Root causes are traced to ROS 2 toolchain changes: an IMU QoS mismatch and an uninitialized oneTBB+Eigen parallel-reduce accumulator. Sensor parsing updates, additional driver support, and YAML-exposed runtime configuration are validated on Ouster and Livox datasets.

LanguageEnglish

CategoryResearch & Report

FormatPDF

Pages3

Reading Time8 min

Introduction

Diagnosing Numerical Failures

A QoS mismatch silently dropping IMU samples

An uninitialized oneTBB + Eigen reduction accumulator

Sensor Support

YAML-Configurable QoS/TF/Output Paths

arXiv :2607 .09947v1 [ cs .RO] 10 Jul 2026 A Numerically-Robust ROS 2 Port of iG-LIO: Diagnosing and Fixing Toolchain-Induced Failures in Incremental GICP LiDAR-Inertial Odometry Afonso E. Carvalho∗† David Portugal† Paulo Peixoto† 2026-07 Technical Report Abstract iG-LIO is a tightly-coupled LiDAR-inertial odometry system fusing generalized-ICP and point-to-plane constraints in an iterated error-state Kalman filter over an incremental voxel map. We report an open-source ROS 2 Jazzy port of the original ROS 1 implementation and, more importantly, the diagnosis of environmentinduced numerical failures that appear only after the port: a mechanically faithful migration – estimation mathematics left unchanged – compiled and ran, yet diverged with NaN internal values. Both causes trace to the modern ROS 2 toolchain, not the algorithm: a Quality-of-Service (QoS) mismatch that silently dropsand reorders IMU samples, and an uninitialized parallel-reduce accumulator arising from the oneTBB + Eigen combination shipped with current distributions. We further correct Ouster point-field parsing to ensure correct point cloud undistortion with newer Ouster revisions, add Velodyne Velarray M1600 support, provide both a compile-time-gated Livox CustomMsg path and a driver-free path for Livox sensors publishing standard PointCloud2 (e.g. Mid-360), and expose the runtime via YAML. The result has been validated in an Ouster OS0 Rev7, an Ouster OS1 Rev 7, and a Livox MID-360 . This report is a citable reference for the port itself, not a claim on the underlying algorithm [1] . The ROS 2 port of iG-LIO described in this document can be found at [https://github.com/Forestry-Robotics-UC/ig_lio/tree/ros2-jazzy](https://github.com/Forestry-Robotics-UC/ig_lio/tree/ros2-jazzy). 1 Introduction iG-LIO [1] is an incremental GICP-based, tightly-coupled LiDAR-inertial estimator, but its public implementation targets ROS 1, now end-of-life. Porting to ROS 2 [2] is not purely mechanical: ROS 2 replaces ROS 1’s implicit, lossless transport with an explicit QoS contract, and modern distributions bundle substantially newer oneTBBand Eigen than the code was written against. Either change can break a tightly-coupled filter silently – the port compiles, runs, and then diverges. All estimator math (voxel map, GICP + point-to-plane fusion, iterated-EKF propagation) is unchanged from upstream [1, 3]; the migration itself (catkin to ament_cmake, roscpp to rclcpp,.launch [to](to .launch.py)[ .launch.py](to .launch.py), tf to tf2 _ros) is mechanical and not detailed here. This report centers on debugging the two failures that surface only after that migration (Section 2), plus corrected sensor parsing (Section 3) and YAML-configurable QoS/TF/output paths (Section 4) . The estimator and its performance claims belong to the original authors [1] . 2 Diagnosing Numerical Failures After a mechanically correct port – math unchanged – the solver died with NaN Hessian matrix entries and aborts inside SO3::exp(nan) . Two distinct, environment-level root causes were isolated; they are the most broadly transferable outcome of this work. 2.1 A QoS mismatch silently dropping IMU samples The initial port subscribed using rclcpp::SensorDataQoS(), i.e. BEST_EFFORT with KEEP_LAST(5) , while the data source (bag or driver) published RELIABLE with a deeper queue. This pairing is compatible but silently degrades to best-effort: under the single-threaded spin_some + processing loop, backed-up IMU samples were ∗ Robotics Institute (RI), Carnegie Mellon University (CMU), Pittsburgh, USA.†Institute of Systems and Robotics (ISR), University of Coimbra (UC), Coimbra, Portugal. dropped and reordered, corrupting the signal the iterated filter’s propagation depends on. The fix subscribes RELIABLE with a deep IMU queue (KeepLast(2000)) and KeepLast(10) for the LiDAR – reliability is now a YAML parameter (Section 4)– plus a guard rejecting non-physical prediction time steps (∆t

FAQs

What numerical failures occur after the ROS 2 port of iG-LIO?

The solver produces NaN values, including NaN entries in the Hessian matrix, and aborts inside SO3::exp(nan). These divergences appear only after the migration while the estimator math remains unchanged.

How does a QoS mismatch contribute to failure in ROS 2?

The port initially used SensorDataQoS (BEST_EFFORT, KEEP_LAST(5)) while the IMU source published RELIABLE with a deeper queue. This silently drops and reorders IMU samples under the single-threaded processing loop, corrupting the filter propagation.

What toolchain-level issue causes an uninitialized reduction accumulator?

Constraint assembly uses tbb::parallel_reduce with a fixed-size Eigen matrix as the reduction value. oneTBB splits initialize the joined accumulators via default construction, and Eigen’s fixed-size default constructor does not zero-initialize storage, so uninitialized memory sums into H and b, creating NaNs.

Jul 16, 2026

Upload by Aria