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

Sintesi Tecnica: Un Porting di iG-LIO su ROS 2 con Robustezza Numerica

Definizione del Problema
Il sistema originale iG-LIO (Incremental GICP LiDAR-Inertial Odometry), un estimatore accoppiato strettamente che fonde GICP generalizzato e vincoli punto-piano, è stato implementato per ROS 1, che è ormai a fine vita. Sebbene una migrazione meccanica del codice base a ROS 2 (convertendo da catkin ad ament_cmake, da roscpp a rclcpp, ecc.) fosse apparsa riuscita, il porting è fallito silenziosamente nella pratica. Nonostante la matematica della stima fosse rimasta invariata, il sistema è divergente con valori interni NaN e si è abortito. Le cause profonde sono state identificate non come difetti algoritmici, ma come fallimenti indotti dall'ambiente, specifici della moderna toolchain ROS 2 e delle sue configurazioni predefinite.

Metodologia e Diagnosi
Gli autori hanno eseguito un porting "meccanicamente fedele" di iG-LIO su ROS 2 Jazzy, preservando la logica di stima centrale (voxel map, fusione GICP + punto-piano, filtro a stato di errore iterato). L'indagine si è concentrata sull'isolare il motivo per cui il sistema portato fallisse mentre la versione originale ROS 1 funzionava correttamente. Sono state identificate due distinte cause profonde a livello di ambiente:

1. Mismatch QoS (Perdita di dati IMU): Il porting iniziale sottoscriveva i dati IMU utilizzando rclcpp::SensorDataQoS() (Best Effort, Keep Last 5), mentre le sorgenti dati (driver o bag) pubblicavano con QoS Reliable. In ROS 2, questa combinazione degrada silenziosamente a Best Effort, causando la perdita e il riordinamento dei campioni IMU sotto carico. Ciò ha corrotto il segnale necessario per la propagazione del filtro iterato.
2. Riduzione Parallela Non Inizializzata (oneTBB + Eigen): L'assemblaggio dei vincoli utilizzava tbb::parallel_reduce con una Eigen::Matrix di dimensione fissa come tipo di valore di riduzione. La versione di oneTBB inclusa nelle moderne distribuzioni ROS 2 costruisce di default degli accumulatori suddivisi. Poiché una Eigen::Matrix di dimensione fissa non azzera l'archiviazione durante la costruzione predefinita, memoria non inizializzata veniva sommata alla matrice Hessiana ($H$) e al vettore ($b$), risultando in valori NaN. Questo problema era invisibile sulla vecchia toolchain utilizzata per il codice originale, ma è emerso immediatamente sulla nuova toolchain ROS 2.

Contributi Chiave
Il documento dettaglia la diagnosi e la correzione di questi fallimenti, insieme a diversi miglioramenti ingegneristici al supporto dei sensori e all'usabilità:

• Correzione QoS: La sottoscrizione IMU è stata aggiornata per utilizzare la QoS RELIABLE con una coda profonda (KeepLast(2000)), mentre le code LiDAR sono state impostate su KeepLast(10). È stata aggiunta una guardia a runtime per rifiutare passi temporali di predizione non fisici ($\Delta t \le 0$ o $\Delta t > 0.5$ s).
• Correzione della Robustezza Numerica: La riduzione dell'accumulatore non inizializzato è stata risolta avvolgendo la Eigen::Matrix in una piccola struct. I membri di questa struct sono esplicitamente inizializzati a zero in classe (tramite Eigen::Matrix::Zero()) e uniti tramite operator+. Ciò preserva la struttura a singolo modulo fuso tbb::parallel_reduce dell'upstream garantendo al contempo che ogni task TBB suddiviso parta con un valore di identità azzerato, eliminando il difetto di lettura non inizializzata senza sacrificare il parallelismo.
• Aggiornamenti del Parsing dei Sensori:
  • Ouster: Aggiornato lo struct/parser dei punti per corrispondere all'attuale layout dei campi PointCloud2 per i sensori Rev 7, ripristinando le capacità di de-skewing del movimento interrotte nel codice originale.
  • Velodyne: Aggiunto il supporto per il Velodyne Velarray M1600.
  • Livox: Implementati due percorsi di ingestione. Un percorso controllato alla compilazione per il CustomMsg specifico del driver (che fallisce con grazia se il driver è assente) e un percorso privo di driver per i sensori che pubblicano standard sensor_msgs/PointCloud2 (es. Mid-360), garantendo l'usabilità senza dipendenze da driver esterni.
• Configurazione e Output: Esposti la affidabilità della QoS, i nomi dei frame e le directory di output tramite YAML. Il sistema ora pubblica la covarianza dell'odometria in un frame conforme a REP-105 e scrive le traiettorie in formato TUM per la valutazione con evo.

Risultati e Validazione
Il porting corretto è stato validato su hardware utilizzando sensori Ouster OS0 Rev7, Ouster OS1 Rev7 e Livox MID-360. La validazione qualitativa ha comportato la sovrapposizione della traiettoria stimata dal porting ROS 2 rispetto all'implementazione originale ROS 1 per la stessa sequenza. I risultati sono stati "qualitativamente identici", confermando che la migrazione e le correzioni numeriche hanno preservato il comportamento dello stimatore. Il sistema ha eliminato con successo la divergenza NaN e gli aborti SO3::exp(nan) osservati nel porting iniziale.

Significatività e Rivendicazioni
Gli autori dichiarano esplicitamente che questo rapporto serve come riferimento citabile per il porting stesso, non come rivendicazione sulla performance o sulla novità dell'algoritmo sottostante, che appartiene agli autori originali di iG-LIO [1]. La significatività di questo lavoro risiede in due lezioni generalizzabili per l'ingegneria del software robotico:

1. La QoS come Contratto di Correttezza: In ROS 2, le impostazioni della Qualità del Servizio (Quality-of-Service) non sono semplici questioni di throughput, ma critici contratti di correttezza per gli stimatori accoppiati strettamente; le garanzie implicite di trasporto di ROS 1 non si applicano più.
2. Sensibilità della Toolchain: Le operazioni di riduzione parallela su matrici Eigen di dimensione fissa devono essere gestite con un'inizializzazione esplicita quando si utilizzano le moderne distribuzioni oneTBB, poiché i costruttori predefiniti non garantiscono più la zero-inizializzazione.

Il lavoro dimostra che un porting meccanicamente corretto è insufficiente per la stabilità numerica; è necessaria un'attenzione specifica alla toolchain dell'ambiente target e ai contratti di comunicazione per prevenire fallimenti silenziosi.