MAD-BA: 3D LiDAR Bundle Adjustment -- from Uncertainty Modelling to Structure Optimization

Résumé technique : MAD-BA : Ajustement de faisceau 3D LiDAR – de la modélisation de l'incertitude à l'optimisation de la structure

Énoncé du problème
Une modélisation 3D précise et globalement cohérente est cruciale pour la navigation autonome, la robotique et l'inspection des infrastructures. Bien que les capteurs LiDAR modernes capturent des millions de points par seconde, atteindre une cohérence globale reste un défi en raison de distributions de données clairsemées et irrégulières et de l'absence de repères visuels. La localisation et la cartographie simultanées (SLAM) LiDAR traditionnelles reposent souvent sur l'optimisation de graphes de poses (PGO), qui affine les trajectoires tout en maintenant la structure de la carte fixe. Ce découplage entraîne un bruit non corrigé et des incohérences dans les nuages de points. De plus, les méthodes existantes de raffinement global pour le LiDAR traitent soit l'optimisation de la pose et de la structure séparément (entraînant des incohérences), soit s'appuient sur des techniques d'enregistrement local (comme l'ICP) qui n'optimisent pas conjointement la structure globale. Par ailleurs, les mesures LiDAR sont intrinsèquement bruyantes en raison des conditions environnementales et de la physique des capteurs, pourtant de nombreux systèmes manquent d'un modèle d'incertitude généralisé pour pondérer efficacement les mesures lors de l'optimisation.

Méthodologie
L'article présente MAD-BA, un cadre unifié d'ajustement de faisceau (BA) global conçu pour optimiser simultanément les poses des capteurs et la structure de la carte 3D. La méthodologie repose sur quatre composantes principales :

1. Représentation basée sur les surfels : La carte 3D est représentée à l'aide de surfels (disques orientés), définis par un point central, une normale de surface et un rayon. Cette représentation est choisie pour son efficacité dans la fusion et les mises à jour, sa capacité à mieux capturer les surfaces minces que les approches basées sur les voxels, et son évitement des mises à jour topologiques coûteuses en calcul requises par les méthodes basées sur les maillages.
2. Association de données efficace : Pour faire correspondre les mesures LiDAR aux surfels, le système utilise des arbres kd construits via l'analyse en composantes principales (PCA). Cette structure encode la segmentation en plans et permet des requêtes de plus proches voisins efficaces. L'association de données est réalisée en faisant correspondre les feuilles de l'arbre aux surfels existants sur la base de distances euclidiennes, de distances normales et de seuils angulaires.
3. Modèle d'incertitude LiDAR généralisé : Pour répondre au bruit de mesure, les auteurs proposent un modèle d'incertitude basé sur l'échantillonnage. Au lieu de s'appuyer sur des données d'ondelettes propriétaires (souvent indisponibles), le système simule le faisceau LiDAR comme un cône basé sur la divergence du faisceau. En lançant plusieurs sous-faisceaux dans ce cône vers la moyenne d'une feuille, le système échantillonne un ensemble de portées. L'écart type de ces portées échantillonnées est calculé pour estimer l'incertitude de mesure ($\sigma$). Ce $\sigma$ est utilisé pour pondérer les résidus dans la fonction de coût, privilégiant les mesures fiables.
4. Optimisation conjointe : Le système minimise une fonction de coût en utilisant l'algorithme de Levenberg-Marquardt (LM). La fonction de coût agrège les distances point-plan pondérées par la fonction de perte robuste de Huber et l'incertitude estimée. Crucialement, l'optimisation met à jour conjointement les poses des capteurs (paramétrées dans l'algèbre de Lie $se(3)$) et les positions des surfels. Les surfels sont contraints de se déplacer uniquement le long de leur direction normale pour préserver la géométrie locale et réduire les degrés de liberté, tandis que les poses sont mises à jour dans les six degrés de liberté.

Contributions clés
L'article expose trois contributions principales :

1. Un cadre d'ajustement de faisceau évolutif et robuste, spécifiquement adapté aux données LiDAR, qui intègre l'optimisation de la pose et de la structure.
2. Un modèle d'incertitude LiDAR généralisé qui estime la fiabilité des mesures en fonction de la divergence du faisceau et de la portée, permettant une optimisation pondérée par la confiance sans nécessiter de données d'ondelettes brutes.
3. Une implémentation open source du système pour soutenir la reproductibilité et la recherche ultérieure.

Résultats expérimentaux
Le système a été évalué sur trois jeux de données publics : Newer College Dataset (NC), A Vision Benchmark in Rome (VBR) et KITTI. L'évaluation a comparé MAD-BA à des méthodes de l'état de l'art, spécifiquement BALM2 (une méthode de raffinement global qui optimise uniquement les poses) et HBA (une méthode de BA hiérarchique).

• Précision de la trajectoire : Les résultats quantitatifs utilisant l'erreur de trajectoire absolue (ATE) montrent que MAD-BA surpasse constamment BALM2 et HBA sur la plupart des séquences. Par exemple, sur la séquence NC "math-easy", MAD-BA avec incertitude a atteint une erreur RMS de 0,043 m, contre 0,087 m pour la trajectoire initiale et 0,135 m pour HBA.
• Qualité de la carte : L'évaluation sur le jeu de données NC (qui fournit des cartes de référence haute résolution) a démontré que MAD-BA améliore significativement la précision, l'achèvement et le score F de la carte par rapport aux cartes initiales et aux autres méthodes. Les cartes basées sur les surfels ont montré des écarts géométriques réduits et un meilleur alignement avec la référence.
• Robustesse et ablation :
  • Modélisation de l'incertitude : L'intégration du modèle d'incertitude a réduit le nombre d'itérations nécessaires à la convergence et amélioré la précision finale par rapport à une version où toutes les mesures étaient pondérées de manière égale.
  • Initialisation : Le système a démontré une forte tolérance aux erreurs de trajectoire initiales, convergeant de manière fiable avec un bruit gaussien allant jusqu'à 1 m en translation et 0,2 rad en rotation.
  • Densité des capteurs : Lorsqu'il a été sous-échantillonné pour simuler un capteur VLP-16, le système a maintenu son efficacité dans la plupart des scénarios, bien que les performances se soient dégradées dans les cas de mouvement vertical (par exemple, escaliers) en raison d'un champ de vision vertical insuffisant.
  • Filtrage dynamique : Les résultats qualitatifs ont indiqué que la représentation par surfels filtre implicitement les objets dynamiques (par exemple, voitures en mouvement, piétons), résultant en des cartes statiques plus propres.

Signification et revendications
L'article revendique que MAD-BA comble le fossé entre l'optimisation de la seule pose et le raffinement séparé de la structure en fournissant un cadre unifié qui réalise une cartographie globalement cohérente et robuste. En optimisant conjointement les poses et les surfels et en intégrant un modèle d'incertitude motivé par la physique, le système atténue le bruit et la parcimonie inhérents aux données LiDAR. Les auteurs affirment que leur approche surpasse les méthodes existantes basées sur le LiDAR en termes de robustesse et de précision, produisant des cartes 3D propres et de haute fidélité tout en compensant les artefacts dynamiques et la distorsion du LiDAR. Ce travail est présenté comme une étape vers une estimation d'état plus fiable en robotique, la nature open source du code visant à faciliter l'adoption par la communauté et le développement ultérieur.