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

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

Lizenzen

Open Access
Paper: CC BY 4.0

Alles Folgende stützt sich auf das vollständige Paper unter seiner Lizenz CC BY 4.0; die Abbildung wird mit Nennung der Autoren gezeigt.

Kurz gesagt

Scan-to-Map-ICP mit einer Anfangs- und einer Endpose pro Scan und erlaubten Sprüngen zwischen Scans, plus Loop Closure über Höhenbilder; 0,59 % RTE auf der KITTI-Rangliste bei 60 ms pro Scan.

Warum es wichtig ist

Die Methode benötigt nur Punktkoordinaten und Zeitstempel pro Punkt und verarbeitet fahrzeugmontierte, Segway-montierte und handgeführte LiDAR in einem Framework. Ihre Odometrie im Fahrprofil läuft mit 60-80 ms pro Scan auf einem einzigen CPU-Thread, und das vollständige SLAM nutzt nur LiDAR-Daten.

Problem

Ein rotierendes Mehrstrahl-LiDAR nimmt jeden Scan während der Bewegung auf, daher müssen die Punkte um die Sensorbewegung korrigiert werden. Die meisten Odometriemethoden entzerren einen Scan einmalig mit einem Modell konstanter Geschwindigkeit, das bei plötzlichen Orientierungs- oder Beschleunigungsänderungen scheitert, während zeitkontinuierliche Methoden mit glatten Trajektorien der hochfrequenten, wackeligen Bewegung realer Plattformen nicht folgen können.

Lösungsansatz

CT-ICP beschreibt jeden Scan durch eine Anfangs- und eine Endpose, interpoliert für jeden Punkt anhand seines Zeitstempels eine Pose (slerp für die Rotation, linear für die Translation) und löst ein Punkt-zu-Ebene-Scan-to-Map-ICP mit schwachen Nebenbedingungen, die die Anfangspose nahe an der vorherigen Endpose halten und die Beschleunigung begrenzen. Die lokale Karte ist eine dichte Punktwolke in einem dünnbesetzten Voxelgitter (bis zu 20 Punkte pro Voxel), und ein Loop-Closure-Modul gleicht 2D-Merkmale auf Höhenbildern aggregierter lokaler Karten ab, verfeinert Treffer mit ICP und optimiert einen g2o-Posengraphen.

Wichtigste Erkenntnisse

Wenn die Anfangspose eines Scans von der Endpose des vorherigen Scans abweichen darf, wird hochfrequente Bewegung aufgefangen, die glatte zeitkontinuierliche Trajektorien nicht darstellen können.
Elastische Verzerrung innerhalb des Scans ist bei Rohdaten wichtig: Sie durch Entzerrung mit konstanter Geschwindigkeit zu ersetzen, erhöht den RTE auf KITTI-raw von 0,55 % auf 0,79 % und auf KITTI-360 von 0,45 % auf 0,60 %.
Normalen und Planaritätsgewichte werden aus der dichten lokalen Karte statt aus dem aktuellen Scan berechnet, was reichere Nachbarschaften für die Punkt-zu-Ebene-Residuen liefert.
Eine dünnbesetzte Voxelkarte bietet Nachbarschaftszugriff in konstanter Zeit statt des logarithmischen Zugriffs von kd-Bäumen, wodurch eine dichte Karte in Echtzeit nutzbar bleibt.
Auf bewegungskorrigierten KITTI-Scans liegen alle verglichenen Methoden nahe beieinander; die Unterschiede zeigen sich bei rohen, unkorrigierten Scans.

Hauptergebnisse

KITTI-Odometrie-Rangliste (Testsequenzen 11-21): 0,59 % RTE, damals erster Platz unter Methoden mit öffentlichem Code.
Mittlerer RTE auf Sequenzen mit Ground Truth: 0,53 % auf bewegungskorrigiertem KITTI (60 ms/Scan), 0,55 % auf KITTI-raw (65 ms) und 0,45 % auf KITTI-360 (70 ms); IMLS-SLAM, MULLS und pyLiDAR F2M erreichen 0,71-1,61 % auf KITTI-raw und 0,68-1,55 % auf KITTI-360.
KITTI-CARLA (simuliert): 0,09 % RTE gegenüber 0,81 % für die beste andere Odometrie; ParisLuco: 1,11 % gegenüber 1,26 % für IMLS-SLAM.
Profil für hochfrequente Bewegung: 0,55 % RTE auf Newer College (pyLiDAR F2M 2,20 %) und 1,17 % auf NCLT 2012-01-08 (pyLiDAR F2M 19,80 %), bei 430 ms und 180 ms pro Scan.
Loop Closure senkt den ATE von 6,22 m auf 0,66 m auf KITTI-raw 00, von 29,87 m auf 1,07 m auf KITTI-360 00 und von 29,16 m auf 9,65 m auf ParisLuco; auf KITTI-CARLA Town01 bleibt er nahe 0,2 m (0,21 m auf 0,26 m).
Jeder Aufbau und Abgleich eines Höhengitters dauert im Mittel 1,1 s, plus 1,2 s für die Posengraph-Optimierung, wenn eine Schleife gefunden wird.

Einschränkungen

Loop Closure erfordert, dass die Sensorbewegung überwiegend eben ist und die extrinsische Kalibrierung die z-Achse an der Bodennormalen ausrichtet, was die unterstützten Sensoraufbauten und Bewegungen einschränkt.
Loop Closure läuft im Hauptthread (1,1 s pro Höhengitter plus 1,2 s pro Posengraph-Optimierung), daher ist das vollständige SLAM noch nicht echtzeitfähig; die Autoren geben an, dass separate Threads es echtzeitfähig machen würden.
Die lokale Karte vergisst keine Punkte in einem gleitenden Fenster, daher verunreinigt eine fehlerhafte Registrierung sie; das robuste Profil, das davor schützt, erhöht die Laufzeit (430 ms pro Scan auf Newer College, 180 ms auf NCLT).
Der für alle Experimente verwendete Gauss-Newton-Solver hat von Hand berechnete Jacobi-Matrizen, daher lassen sich seine Nebenbedingungen nicht ändern; die flexiblere Ceres-Option benötigt für den Echtzeiteinsatz mehrere Threads.
Die Zahl der Loop-Closure-Kandidaten (10) muss an die erwartete Drift angepasst werden, und auf KITTI-CARLA verbesserte sich der ATE nach Loop Closure nicht.

Wirkung auf das Fachgebiet

Finalist für ICRA 2022 Outstanding Paper, mit 301 Zitationen. Die Arbeit veröffentlicht die CT-ICP-Odometrie in C++ (github.com/jedeschaud/ct_icp), die sie als öffentlichen Open-Source-Code mit permissiver Lizenz beschreibt, bringt die Schleifenerkennung und das Posengraph-Backend in pyLiDAR-SLAM ein (github.com/Kitware/pyLiDAR-SLAM) und stellt die verwendeten Datensätze in der von ihr beschriebenen Form online bereit.

Zusammenfassung

Mehrstrahl-LiDAR-Sensoren werden in der Robotik zunehmend eingesetzt, insbesondere bei autonomen Fahrzeugen für Lokalisierungs- und Wahrnehmungsaufgaben, die beide auf der Fähigkeit beruhen, eine präzise Karte der Umgebung aufzubauen. Dafür schlagen wir eine neue Echtzeit-Odometriemethode nur mit LiDAR namens CT-ICP (für Continuous-Time ICP) vor, die mit einem neuartigen Verfahren zur Schleifenerkennung zu einem vollständigen SLAM ergänzt wird. Der Kern dieser Methode ist die Einführung der Kombination aus Kontinuität innerhalb des Scan-Matchings und Diskontinuität zwischen Scans. Sie erlaubt sowohl die elastische Verzerrung des Scans während der Registrierung für höhere Präzision als auch eine höhere Robustheit gegenüber hochfrequenten Bewegungen dank der Diskontinuität.

Wir bauen auf dieser Odometrie ein vollständiges SLAM auf, das eine schnelle, rein LiDAR-basierte Schleifenerkennung auf Grundlage von 2D-Matching von Höhenbildern nutzt und einen Posengraphen mit Schleifenbedingungen liefert. Um die Robustheit der Methode zu zeigen, haben wir sie auf sieben Datensätzen getestet: KITTI, KITTI-raw, KITTI-360, KITTI-CARLA, ParisLuco, Newer College und NCLT, in Fahrszenarien und Szenarien mit hochfrequenter Bewegung. Sowohl die CT-ICP-Odometrie als auch die Schleifenerkennung sind online verfügbar. CT-ICP ist derzeit unter den Methoden mit öffentlich zugänglichem Code die erste auf der KITTI-Odometrie-Rangliste, mit einem mittleren relativen Translationsfehler (Relative Translation Error, RTE) von 0,59 % und einer mittleren Zeit pro Scan von 60ms auf einer CPU mit einem einzigen Thread.

Open Source Robotics