CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure

Auteurs: Pierre Dellenbach, Jean-Emmanuel Deschaud, Bastien Jacquet, François Goulette · ICRA 2022

Licences

• Accès libre
• Article: CC BY 4.0

Tout ce qui suit s'appuie sur l'article complet sous sa licence CC BY 4.0 ; sa figure est montrée en citant les auteurs.

En bref

ICP balayage-carte avec une pose de début et une pose de fin par balayage et des sauts autorisés entre balayages, plus une fermeture de boucle par images d'élévation ; 0,59 % de RTE au classement KITTI à 60 ms par balayage.

Pourquoi c'est important

La méthode ne nécessite que les coordonnées des points et un horodatage par point, et traite dans un même cadre des LiDAR montés sur voiture, sur Segway ou tenus à la main. Son odométrie en profil conduite s'exécute en 60-80 ms par balayage sur un seul thread CPU, et le SLAM complet n'utilise que des données LiDAR.

Problème

Un LiDAR multifaisceaux rotatif enregistre chaque balayage pendant qu'il se déplace, si bien que les points doivent être corrigés du mouvement du capteur. La plupart des méthodes d'odométrie corrigent la distorsion d'un balayage une seule fois avec un modèle à vitesse constante, qui échoue lors de changements soudains d'orientation ou d'accélération, tandis que les méthodes en temps continu à trajectoires lisses ne peuvent pas suivre le mouvement saccadé à haute fréquence des plateformes réelles.

Solution proposée

CT-ICP décrit chaque balayage par une pose de début et une pose de fin, interpole une pose pour chaque point à partir de son horodatage (slerp pour la rotation, linéaire pour la translation) et résout un ICP balayage-carte point-plan avec des contraintes faibles qui maintiennent la pose de début près de la pose de fin précédente et limitent l'accélération. La carte locale est un nuage de points dense dans une grille de voxels creuse (jusqu'à 20 points par voxel), et un module de fermeture de boucle met en correspondance des caractéristiques 2D sur des images d'élévation de cartes locales agrégées, affine les correspondances par ICP et optimise un graphe de poses g2o.

Idées clés

• Permettre à la pose de début d'un balayage de différer de la pose de fin du balayage précédent absorbe les mouvements à haute fréquence que des trajectoires continues dans le temps et lisses ne peuvent pas représenter.
• La déformation élastique intra-balayage compte sur les données brutes : la remplacer par une correction de distorsion à vitesse constante fait passer la RTE de 0,55 % à 0,79 % sur KITTI-raw et de 0,45 % à 0,60 % sur KITTI-360.
• Les normales et les poids de planéité sont calculés à partir de la carte locale dense plutôt que du balayage courant, ce qui donne des voisinages plus riches pour les résidus point-plan.
• Une carte de voxels creuse offre un accès aux voisinages en temps constant au lieu de l'accès logarithmique des arbres kd, ce qui garde une carte dense utilisable en temps réel.
• Sur les balayages KITTI corrigés du mouvement, toutes les méthodes comparées sont proches ; les différences apparaissent sur les balayages bruts, non corrigés.

Principaux résultats

• Classement d'odométrie KITTI (séquences de test 11-21) : 0,59 % de RTE, premier à l'époque parmi les méthodes à code public.
• RTE moyenne sur les séquences avec vérité terrain : 0,53 % sur KITTI corrigé du mouvement (60 ms/balayage), 0,55 % sur KITTI-raw (65 ms) et 0,45 % sur KITTI-360 (70 ms) ; IMLS-SLAM, MULLS et pyLiDAR F2M atteignent 0,71-1,61 % sur KITTI-raw et 0,68-1,55 % sur KITTI-360.
• KITTI-CARLA (simulé) : 0,09 % de RTE contre 0,81 % pour la meilleure autre odométrie ; ParisLuco : 1,11 % contre 1,26 % pour IMLS-SLAM.
• Profil de mouvement à haute fréquence : 0,55 % de RTE sur Newer College (pyLiDAR F2M 2,20 %) et 1,17 % sur NCLT 2012-01-08 (pyLiDAR F2M 19,80 %), à 430 ms et 180 ms par balayage.
• La fermeture de boucle réduit l'ATE de 6,22 m à 0,66 m sur KITTI-raw 00, de 29,87 m à 1,07 m sur KITTI-360 00 et de 29,16 m à 9,65 m sur ParisLuco ; sur KITTI-CARLA Town01, elle reste proche de 0,2 m (de 0,21 m à 0,26 m).
• Chaque construction et mise en correspondance d'une grille d'élévation prend 1,1 s en moyenne, plus 1,2 s pour l'optimisation du graphe de poses lorsqu'une boucle est trouvée.

Limites

• La fermeture de boucle exige que le mouvement du capteur soit essentiellement plan et que l'étalonnage extrinsèque aligne l'axe z sur la normale au sol, ce qui limite les configurations de capteurs et les mouvements pris en charge.
• La fermeture de boucle s'exécute sur le thread principal (1,1 s par grille d'élévation plus 1,2 s par optimisation du graphe de poses), si bien que le SLAM complet n'est pas encore temps réel ; les auteurs indiquent que des threads séparés le rendraient temps réel.
• La carte locale n'oublie pas de points selon une fenêtre glissante, de sorte qu'un mauvais recalage la pollue ; le profil robuste qui protège contre cela augmente le temps d'exécution (430 ms par balayage sur Newer College, 180 ms sur NCLT).
• Le solveur de Gauss-Newton utilisé pour toutes les expériences a des jacobiennes calculées à la main, de sorte que ses contraintes ne peuvent pas être modifiées ; l'option Ceres, plus flexible, nécessite plusieurs threads pour un usage en temps réel.
• Le nombre de candidats à la fermeture de boucle (10) doit être adapté à la dérive attendue, et sur KITTI-CARLA l'ATE ne s'est pas amélioré après la fermeture de boucle.

Impact sur le domaine

Finaliste du prix ICRA 2022 Outstanding Paper, avec 301 citations. L'article publie l'odométrie CT-ICP en C++ (github.com/jedeschaud/ct_icp), qu'il décrit comme un code open source public et permissif, intègre la détection de boucles et le back-end de graphe de poses dans pyLiDAR-SLAM (github.com/Kitware/pyLiDAR-SLAM), et met en ligne les jeux de données utilisés sous la forme qu'il décrit.

Résumé

Les capteurs LiDAR multifaisceaux sont de plus en plus utilisés en robotique, en particulier sur les voitures autonomes pour des tâches de localisation et de perception, qui reposent toutes deux sur la capacité à construire une carte précise de l'environnement. Pour cela, nous proposons une nouvelle méthode d'odométrie temps réel uniquement LiDAR appelée CT-ICP (pour Continuous-Time ICP), complétée en un SLAM complet par une nouvelle procédure de détection de boucles. Le cœur de cette méthode est l'introduction combinée de la continuité dans la mise en correspondance d'un balayage et de la discontinuité entre balayages. Elle permet à la fois la déformation élastique du balayage pendant le recalage, pour une précision accrue, et une robustesse accrue aux mouvements à haute fréquence grâce à la discontinuité.

Nous construisons un SLAM complet au-dessus de cette odométrie, en utilisant une détection de boucles rapide purement LiDAR fondée sur la mise en correspondance 2D d'images d'élévation, qui fournit un graphe de poses avec des contraintes de boucle. Pour montrer la robustesse de la méthode, nous l'avons testée sur sept jeux de données : KITTI, KITTI-raw, KITTI-360, KITTI-CARLA, ParisLuco, Newer College et NCLT, dans des scénarios de conduite et de mouvements à haute fréquence. L'odométrie CT-ICP et la détection de boucles sont toutes deux disponibles en ligne. CT-ICP est actuellement première, parmi les méthodes donnant accès à un code public, au classement d'odométrie KITTI, avec une erreur de translation relative (Relative Translation Error, RTE) moyenne de 0,59 % et un temps moyen par balayage de 60ms sur un CPU avec un seul thread.

Liens

• arXiv 2109.12979: https://arxiv.org/abs/2109.12979
• DOI 10.48550/arXiv.2109.12979: https://doi.org/10.48550/arXiv.2109.12979
• PDF: https://arxiv.org/pdf/2109.12979v1
• Code: https://github.com/jedeschaud/ct_icp