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

Technische Samenvatting: Een Numeriek Robuuste ROS 2-port van iG-LIO

Probleemstelling
Het oorspronkelijke iG-LIO (Incremental GICP LiDAR-Inertial Odometry) systeem, een strak gekoppelde estimator die generalized-ICP en punt-naar-vlak beperkingen combineert, werd geïmplementeerd voor ROS 1, dat nu het einde van de levenscyclus heeft bereikt. Hoewel een mechanische migratie van de codebase naar ROS 2 (het converteren van catkin naar ament_cmake, roscpp naar rclcpp, etc.) succesvol leek, faalde de port in de praktijk zonder waarschuwing. Ondanks het feit dat de wiskundige schatting ongewijzigd bleef, divergeerde het systeem met NaN interne waarden en stopte het voortijdig. De onderliggende oorzaken werden geïdentificeerd als geen algoritmische gebreken, maar als door de omgeving geïnduceerde fouten die specifiek zijn voor de moderne ROS 2 toolchain en de standaardconfiguraties.

Methodologie en Diagnose
De auteurs voerden een "mechanisch getrouwe" port van iG-LIO uit naar ROS 2 Jazzy, waarbij de kern van de schattingslogica (voxel map, GICP + punt-naar-vlak fusie, iteratieve error-state Kalman filter) behouden bleef. Het onderzoek richtte zich op het isoleren van de reden waarom de geporteerde versie faalde terwijl de originele ROS 1 versie correct functioneerde. Twee afzonderlijke, op omgevingsniveau liggende oorzaken werden geïdentificeerd:

1. QoS Mismatch (IMU Gegevensverlies): De initiële port abonneerde zich op IMU-gegevens met behulp van rclcpp::SensorDataQoS() (Best Effort, Keep Last 5), terwijl de gegevensbronnen (drivers of bags) publiceerden met Reliable QoS. In ROS 2 degradeert deze combinatie stilzwijgend naar Best Effort, wat ervoor zorgt dat IMU-samples worden gedropt en geherordend onder belasting. Dit corrumpeerde het signaal dat vereist is voor de propagatie van het iteratieve filter.
2. Niet-geïnitialiseerde Parallelle Reductie (oneTBB + Eigen): De constructie van de beperkingen maakte gebruik van tbb::parallel_reduce met een vastgezet Eigen::Matrix als de reductiewaarde-type. De versie van oneTBB die gebundeld is met moderne ROS 2 distributies, voert standaardconstructie uit op split accumulatoren. Aangezien een vastgezet Eigen::Matrix zijn opslag niet nul-initialiseert bij standaardconstructie, werd ongeïnitialiseerd geheugen opgeteld bij de Hessiaan-matrix ($H$) en vector ($b$), wat resulteerde in NaN waarden. Dit probleem was onzichtbaar op de oudere toolchain die voor de originele code werd gebruikt, maar kwam direct aan het licht op de nieuwere ROS 2 toolchain.

Belangrijkste Bijdragen
Het artikel beschrijft de diagnose en correctie van deze fouten, samen met verschillende engineering-verbeteringen voor sensoren en bruikbaarheid:

• QoS Correctie: De IMU-abonnement werd bijgewerkt om RELIABLE QoS te gebruiken met een diepe wachtrij (KeepLast(2000)), terwijl LiDAR-wachtrijen werden ingesteld op KeepLast(10). Een runtime-bewaker werd toegevoegd om niet-fysieke voorspellings-tijdstappen ($\Delta t \le 0$ of $\Delta t > 0.5$ s) af te wijzen.
• Numerieke Robuustheid Fix: De ongeïnitialiseerde reductie-accumulator werd opgelost door de Eigen::Matrix te verpakken in een kleine struct. De leden van deze struct worden expliciet nul-geïnitialiseerd in-class (via Eigen::Matrix::Zero()) en samengevoegd via operator+. Dit behoudt de upstream single fused tbb::parallel_reduce structuur terwijl gegarandeerd wordt dat elke split TBB-taak begint met een genullerde identiteitswaarde, waardoor de ongeïnitialiseerde leesfout wordt geëlimineerd zonder de parallelliteit op te offeren.
• Sensor Parsing Updates:
  • Ouster: De puntstructuur/parser is bijgewerkt om overeen te komen met de huidige PointCloud2 veldlay-out voor Rev 7 sensoren, waardoor de motion de-skewing mogelijkheden die in de originele code defect waren, zijn hersteld.
  • Velodyne: Ondersteuning toegevoegd voor de Velodyne Velarray M1600.
  • Livox: Twee ingestiepaden geïmplementeerd. Een compile-time-gated pad voor de driver-specifieke CustomMsg (die gracieus faalt als de driver ontbreekt) en een driver-vrij pad voor sensoren die standaard sensor_msgs/PointCloud2 publiceren (bijv. Mid-360), wat bruikbaarheid garandeert zonder externe driver-afhankelijkheden.
• Configuratie en Output: QoS betrouwbaarheid, frame namen en output-directories zijn geëxposeerd via YAML. Het systeem publiceert nu odometrie covariantie in een REP-105 compliant frame en schrijft trajecten in TUM-formaat voor evaluatie met evo.

Resultaten en Validatie
De gecorrigeerde port werd gevalideerd op hardware met behulp van Ouster OS0 Rev7, Ouster OS1 Rev7 en Livox MID-360 sensoren. Kwalitatieve validatie bestond uit het overlappen van de geschatte traject van de ROS 2 port tegenover de originele ROS 1 implementatie voor dezelfde sequentie. De resultaten waren "kwalitatief identiek", wat bevestigt dat de migratie en numerieke fixes het gedrag van de estimator hebben behouden. Het systeem heeft de NaN divergentie en SO3::exp(nan) aborts succesvol geëlimineerd die werden waargenomen in de initiële port.

Significantie en Claims
De auteurs stellen expliciet dat dit rapport dient als een citeerbare referentie voor de port zelf, en niet als een claim op de onderliggende algoritmische prestaties of nieuwheid, die toebehoort aan de originele iG-LIO auteurs [1]. De significantie van dit werk ligt in twee generaliseerbare lessen voor robotica-software engineering:

1. QoS als een Correctheidsovereenkomst: In ROS 2 zijn Quality-of-Service instellingen niet louter zaken van doorvoer, maar kritieke correctheidsovereenkomsten voor strak gekoppelde estimators; de impliciete transportgaranties van ROS 1 zijn niet langer van toepassing.
2. Toolchain Gevoeligheid: Parallelle reductie-operaties over vastgezet Eigen matrices moeten expliciet worden gehanteerd met initialisatie wanneer moderne oneTBB distributies worden gebruikt, aangezien standaardconstructeurs geen garantie bieden voor nul-initialisatie.

Het werk demonstreert dat een mechanisch correcte port onvoldoende is voor numerieke stabiliteit; specifieke aandacht voor de doelomgeving's toolchain en communicatiecontracten is vereist om stille fouten te voorkomen.