💻 computer science

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

Cet article présente un portage numériquement robuste de l' système d'odométrie LiDAR-inertielle iG-LIO vers ROS 2 Jazzy, détaillant le diagnostic et la résolution de défaillances critiques induites par la chaîne d'outils — spécifiquement les désaccords de QoS et les accumulateurs de réduction parallèle non initialisés — tout en ajoutant le support pour les capteurs modernes Ouster, Velodyne et Livox.

Auteurs originaux : Afonso E. Carvalho, David Portugal, Paulo Peixoto

Publié 2026-07-14

📖 6 min de lecture🧠 Analyse approfondie

CC BY 4.0

Auteurs originaux : Afonso E. Carvalho, David Portugal, Paulo Peixoto

Article original sous licence CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ Ceci est une explication générée par l'IA de l'article ci-dessous. Elle n'a pas été rédigée ni approuvée par les auteurs. Pour une précision technique, consultez l'article original. Lire la clause de non-responsabilité complète

Imaginez que vous avez un robot explorateur super intelligent nommé iG-LIO. Ce robot est un maître de la navigation ; il combine un scanner laser rotatif (LiDAR) et un capteur de mouvement (IMU) pour construire une carte 3D parfaite du monde tout en déterminant exactement où il se trouve. La version originale de ce robot a été construite pour un ancien système d'exploitation appelé ROS 1.

Récemment, une équipe d'ingénieurs a tenté de transférer ce robot vers un tout nouveau système d'exploitation moderne appelé ROS 2. Ils se sont dit : « C'est juste un travail de traduction ! Nous allons garder le cerveau du robot exactement le même, nous allons juste changer la langue qu'il parle. » Ils ont fait la traduction, le robot a démarré. Mais alors, le désastre a frappé : le cerveau du robot s'est mis à hurler des absurdités, remplissant sa mémoire d'erreurs « NaN » (Not a Number) et plantant. C'était comme une voiture parfaitement saine qui, après avoir reçu une nouvelle peinture, refusait soudainement de rouler parce que la nouvelle pompe de la station-service ne correspondait pas à l'embout.

L'équipe a réalisé que le cerveau du robot (les mathématiques) était sain. Le problème était l'environnement dans lequel il vivait désormais. Ils ont trouvé deux coupables sournois cachés dans le nouveau système d'exploitation qui ont brisé le robot, et ils les ont réparés.

Le premier coupable : le mélange « Best Effort »

Imaginez que le capteur de mouvement du robot (l'IMU) est un messager frénétique courant vers le cerveau du robot pour lui crier des mises à jour sur la façon dont le robot penche ou tourne. Dans l'ancien système, le cerveau du robot attendait patiemment chaque message, peu importe l'encombrement du couloir.

Dans le nouveau système, le robot a reçu l'ordre d'utiliser un service de livraison « Best Effort » (au mieux). C'est comme un facteur qui dit : « Je vais essayer de livrer ces lettres, mais si le sac devient trop plein, je vais simplement jeter les plus anciennes et espérer que vous receviez le reste. » Parce que le robot traitait les données lentement, le messager s'est embouteillé. Le transporteur « Best Effort » a commencé à jeter et à mélanger l'ordre des mises à jour de mouvement.

Le cerveau du robot, qui repose sur une chaîne ininterrompue et parfaite de données de mouvement pour rester équilibré, a été confus par les morceaux manquants. Il a essayé de calculer une trajectoire basée sur une chronologie brisée et a fini par une catastrophe mathématique (valeurs NaN).

La correction : L'équipe a changé le contrat de livraison. Ils ont dit au cerveau du robot : « Plus de "Best Effort". Nous avons besoin d'une livraison Reliable (fiable). » Ils ont mis en place une salle d'attente massive (une file d'attente de 2000 échantillons) afin que le messager puisse décharger toutes les mises à jour sans en perdre une seule. Ils ont également ajouté une garde de sécurité : si le temps entre les mises à jour est étrange (inférieur à 0 seconde ou supérieur à 0,5 seconde), le robot ignore simplement cette étape au lieu de planter.

Le second coupable : le piège de la « Boîte Vide »

Le deuxième problème était encore plus sournois. Le cerveau du robot utilise un outil de traitement parallèle super rapide (appelé oneTBB) pour effectuer les tâches lourdes. Imaginez une équipe d'ouvriers (threads) essayant de compter un tas de pierres. Ils divisent le tas, chaque ouvrier compte sa propre pile, puis ils additionnent leurs totaux.

Dans l'ancien système, les ouvriers commençały avec des seaux vides qui étaient magiquement remis à zéro. Dans le nouveau système, les ouvriers recevaient des seaux qui semblaient vides mais qui contenaient en réalité des débris aléatoires et poussiéreux parce que la nouvelle usine ne les avait pas nettoyés au préalable. Lorsque les ouvriers additionnaient leurs totaux, ils ajoutaient accidentellement ces débris aléatoires au compte final. Ce « déchet » était si mauvais qu'il a transformé les mathématiques du robot en pur gaspillage (NaNs).

La correction : L'équipe n'a pas arrêté d'utiliser les ouvriers parallèles rapides. Au lieu de cela, ils ont enveloppé les seaux dans une gaine spéciale « Zero-First » (Zéro d'abord). Désormais, avant que tout ouvrier ne commence à compter, il est obligé d'essuyer son seau et de repartir de exactement zéro. Cela a permis de conserver la vitesse du traitement parallèle tout en garantissant que les mathématiques soient propres.

Nouveaux gadgets et meilleures cartes

Au-delà de la correction des plantages, l'équipe a amélioré la boîte à outils du robot :

Nouveaux scanners : Ils ont mis à jour le robot pour qu'il comprenne les derniers scanners laser (comme l'Ouster OS0 et l'OS1 Rev 7) afin qu'il ne soit pas confus par leurs nouveaux formats de données. Ils ont également ajouté le support pour un Velodyne Velarray M1600 spécifique.
Flexibilité Livox : Pour les capteurs Livox, le robot peut maintenant fonctionner de deux manières. Il peut communiquer avec le pilote spécial si vous l'avez, ou il peut simplement écouter le flux de données standard (comme un capteur Mid-360) sans avoir besoin de logiciels supplémentaires. Cela signifie que les utilisateurs n'ont plus besoin de chercher des pilotes spécifiques.
Paramètres faciles : Tout est désormais contrôlé par un simple fichier texte (YAML). Vous pouvez dire au robot à quel point il doit être fiable, quel nom donner à ses cartes et où enregistrer ses journaux de voyage.

Est-ce que cela a fonctionné ?

L'équipe a testé le robot sur du matériel réel, incluant l'Ouster OS0 Rev7, l'Ouster OS1 Rev 7 et le Livox MID-360. Ils ont exécuté la même séquence de test sur la nouvelle version ROS 2 et sur l'ancienne version ROS 1. Le résultat ? Les trajectoires que le robot a tracées étaient qualitativement identiques. Le robot naviguait aussi bien qu'avant, prouvant que les corrections n'ont pas changé la façon dont le robot réfléchit, elles ont simplement empêché le nouveau système d'exploitation de le briser.

En résumé, déplacer un robot complexe vers un nouveau système n'est pas seulement une question de traduction ; c'est une question de compréhension des nouvelles règles de la route. En corrigeant les contrats de livraison et en nettoyant les seaux, l'équipe a sauvé le robot d'un crash silencieux et l'a remis sur la voie de l'exploration du monde.

Résumé Technique : Un portage de iG-LIO vers ROS 2 numériquement robuste

Énoncé du problème
Le système iG-LIO (Incremental GICP LiDAR-Inertial Odometry) original, un estimateur à couplage fort fusionnant le GICP généralisé et des contraintes de point-à-plan, a été implémenté pour ROS 1, qui est désormais en fin de vie. Bien qu'un portage mécanique du code vers ROS 2 (conversion de catkin en ament_cmake, de roscpp en rclcpp, etc.) ait semblé réussi, le portage a échoué silencieusement en pratique. Malgré une mathématique d'estimation inchangée, le système a divergé avec des valeurs internes NaN et s'est interrompu. Les causes profondes ont été identifiées non pas comme des failles algorithmiques, mais comme des échecs induits par l'environnement, spécifiques à la chaîne d'outils et aux configurations par défaut de la version moderne de ROS 2.

Méthodologie et diagnostic
Les auteurs ont réalisé un portage « fidèlement mécanique » de iG-LIO vers ROS 2 Jazzy, préservant la logique d'estimation centrale (carte de voxels, fusion GICP + point-à-plan, filtre de l'état d'erreur itéré). L'investigation s'est concentrée sur l'isolement de la raison pour laquelle le système porté a échoué alors que la version originale sous ROS 1 fonctionnait correctement. Deux causes profondes distinctes au niveau de l'environnement ont été identifiées :

Incompatibilité de QoS (Perte de données IMU) : Le port initial s'abonnait aux données IMU en utilisant rclcpp::SensorDataQoS() (Best Effort, Keep Last 5), tandis que les sources de données (drivers ou bags) publiaient avec une QoS Reliable. Dans ROS 2, ce couplage se dégrade silencieusement en Best Effort, provoquant la perte et le réordonnancement des échantillons IMU sous charge. Cela a corrompu le signal requis pour la propagation du filtre itéré.
Réduction parallèle non initialisée (oneTBB + Eigen) : L'assemblage des contraintes utilisait tbb::parallel_reduce avec un type de valeur de réduction Eigen::Matrix de taille fixe. La version de oneTBB incluse dans les distributions modernes de ROS 2 construit par défaut des accumulateurs divisés. Comme une Eigen::Matrix de taille fixe ne initialise pas son stockage à zéro lors de la construction par défaut, de la mémoire non initialisée a été additionnée à la matrice Hessienne ( $H$ ) et au vecteur ( $b$ ), entraînant des valeurs NaN. Ce problème était invisible sur l'ancien outil utilisé pour le code original, mais est apparu immédiatement sur la nouvelle chaîne d'outils de ROS 2.

Contributions clés
L'article détaille le diagnostic et la correction de ces échecs, ainsi que plusieurs améliorations d'ingénierie concernant le support des capteurs et l'utilisabilité :

Correction de la QoS : L'abonnement IMU a été mis à jour pour utiliser une QoS RELIABLE avec une file d'attente profonde (KeepLast(2000)), tandis que les files d'attente LiDAR ont été fixées à KeepLast(10). Une garde à l'exécution a été ajoutée pour rejeter les pas de temps de prédiction non physiques ( $\Delta t \le 0$ ou $\Delta t > 0,5$ s).
Correction de la robustesse numérique : L'accumulateur de réduction non initialisé a été résolu en enveloppant l' Eigen::Matrix dans une petite structure. Les membres de cette structure sont explicitement initialisés à zéro lors de la construction (via Eigen::Matrix::Zero()) et joints via operator+. Cela préserve la structure de tbb::parallel_reduce fusionnée tout en garantissant que chaque tâche TBB divisée commence avec une valeur d'identité à zéro, éliminant ainsi le défaut de lecture non initialisée sans sacrifier le parallélisme.
Mises à jour du parsing des capteurs :
- Ouster : Mise à jour de la structure et du parseur de points pour correspondre à la disposition actuelle des champs PointCloud2 pour les capteurs Rev 7, restaurant les capacités de dé-skewing de mouvement brisées dans le code original.
- Velodyne : Ajout du support pour le Velodyne Velarray M1600.
- Livox : Implémentation de deux chemins d'ingestion. Un chemin piloté par la compilation pour le CustomMsg spécifique au driver (qui échoue gracieusement si le driver est absent) et un chemin sans driver pour les capteurs publiant des sensor_msgs/PointCloud2 standards (ex: Mid-360), assurant l'utilisabilité sans dépendances de drivers externes.
Configuration et sortie : Exposition de la fiabilité QoS, des noms de frames et des répertoires de sortie via YAML. Le système publie désormais la covariance de l'odométrie dans une frame conforme à la REP-105 et écrit les trajectoires au format TUM pour l'évaluation avec evo.

Résultats et validation
Le port corrigé a été validé sur matériel utilisant des capteurs Ouster OS0 Rev7, Ouster OS1 Rev7 et Livox MID-360. La validation qualitative a consisté à superposer la trajectoire estimée du port ROS 2 à l'implémentation ROS 1 originale pour la même séquence. Les résultats étaient « qualitativement identiques », confirmant que la migration et les corrections numériques ont préservé le comportement de l'estimateur. Le système a réussi à éliminer la divergence NaN et les abandons SO3::exp(nan) observés dans le port initial.

Signification et revendications
Les auteurs déclarent explicitement que ce rapport sert de référence citable pour le portage lui-même, et non d'une revendication sur la performance ou la nouveauté de l'algorithme sous-jacent, qui appartient aux auteurs originaux de iG-LIO [1]. La signification de ce travail réside dans deux leçons généralisables pour l'ingénierie logicielle robotique :

La QoS comme contrat de correction : Dans ROS 2, les paramètres de Qualité de Service ne sont pas de simples questions de débit, mais des contrats de correction critiques pour les estimateurs à couplage fort ; les garanties de transport implicites de ROS 1 ne s'appliquent plus.
Sensibilité de la chaîne d'outils : Les opérations de réduction parallèle sur des matrices Eigen de taille fixe doivent être gérées avec une initialisation explicite lors de l'utilisation des distributions modernes de oneTBB, car les constructeurs par défaut ne garantissent plus l'initialisation à zéro.

Le travail démontre qu'un portage mécaniquement correct est insuffisant pour la stabilité numérique ; une attention spécifique à l'environnement cible, à sa chaîne d'outils et à ses contrats de communication est nécessaire pour prévenir les échecs silencieux.

Le premier coupable : le mélange « Best Effort »

Le second coupable : le piège de la « Boîte Vide »

Nouveaux gadgets et meilleures cartes

Est-ce que cela a fonctionné ?

Articles similaires