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

2026-07-10

AI Summary

This work addresses numerical instability issues—such as IMU data misordering and NaN divergence caused by uninitialized accumulators in parallel computations—that emerged when migrating iG-LIO from ROS 1 to ROS 2 Jazzy due to toolchain changes. Without altering the original algorithm, this study is the first to identify and rectify implicit numerical errors stemming from the interaction between ROS 2 QoS configurations and the oneTBB+Eigen computational stack. By refining QoS policies, initialization protocols, and parallel computation workflows, the system’s robustness is significantly enhanced. The approach also extends support to modern LiDAR sensors, including Ouster OS0/OS1 Rev7 and Livox MID-360. An open-source implementation with multi-device YAML configuration demonstrates stable real-world hardware deployment.

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 environment-induced 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 drops and 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.

Problem

• numerical robustness
• ROS 2 migration
• LiDAR-inertial odometry
• toolchain-induced failures
• QoS mismatch

Innovation

• ROS 2 porting
• numerical robustness
• QoS mismatch
• parallel-reduce accumulator
• LiDAR-inertial odometry