Iterative Closest Point

Auch genannt: ICP, ICP registration
Kategorie: State estimation & navigation › Localisation
Schlagwörter: point-cloud, registration, lidar, localization, 3d-perception

Ein Algorithmus, der zwei Punktwolken ausrichtet, indem er jeden Punkt wiederholt mit seinem nächsten Nachbarn in der anderen Wolke paart und die Starrkörpertransformation bestimmt, die am besten zu diesen Paaren passt.

Definition

Iterative Closest Point (ICP) ist ein Algorithmus zur Registrierung zweier 3D-Punktmengen, also zum Finden der Rotation und Translation, die die eine am besten auf die andere ausrichten. Er wurde 1992 von Besl und McKay vorgestellt, und ein eng verwandtes Verfahren veröffentlichten Chen und Medioni etwa zur selben Zeit. ICP ist ein grundlegendes Werkzeug für LiDAR-Odometrie, Scan-Matching, 3D-Rekonstruktion und die Verfeinerung von Objektposen.

Der Algorithmus wechselt zwischen zwei Schritten. Zuerst findet er für jeden Punkt der Quellpunktwolke den nächstgelegenen Punkt in der Zielpunktwolke, was eine Menge vorläufiger Korrespondenzen ergibt. Dann berechnet er die Starrkörpertransformation, die den Abstand zwischen diesen Paaren minimiert; für Punkt-zu-Punkt-Abstände gibt es dafür eine geschlossene Lösung mithilfe der Singulärwertzerlegung. Die Quelle wird um diese Transformation verschoben, und der Vorgang wiederholt sich, bis die Änderung zwischen den Iterationen klein wird.

ICP konvergiert nur gegen ein lokales Minimum und braucht daher eine vernünftige Anfangsschätzung, typischerweise aus Odometrie, einer IMU oder einem groben globalen Registrierungsverfahren. Ist die anfängliche Fehlausrichtung groß, sind die nächsten Nachbarn falsch, und der Algorithmus kann in einer falschen Ausrichtung hängen bleiben.

Viele Varianten verbessern Geschwindigkeit und Robustheit. Punkt-zu-Ebene-ICP misst den Abstand jedes Punkts zur Tangentialebene an seinem Partner und konvergiert in strukturierten Szenen meist schneller. Generalized ICP modelliert die lokale Oberflächenkovarianz in beiden Punktwolken. Praktische Implementierungen dünnen die Punkte außerdem aus, nutzen k-d-Bäume für die Nachbarsuche und verwerfen Paare, die zu weit auseinanderliegen oder inkonsistente Flächennormalen haben.

Warum es wichtig ist

Die meisten LiDAR-basierten Lokalisierungs- und Kartierungssysteme verwenden ICP oder eine seiner Varianten, um zu schätzen, wie sich der Roboter zwischen Scans bewegt hat, und um Loop Closures zu verifizieren. Außerdem ist es ein Standardweg, die Pose eines Objekts vor dem Greifen zu verfeinern, wenn ein 3D-Modell des Objekts vorliegt.

Wichtige Anwendungen

LiDAR-Odometrie und Scan-Matching für mobile Roboter
Verifikation von Loop Closures in SLAM
Verfeinerung von Objektposen zum Greifen aus Tiefendaten
Zusammenführen mehrerer Scans zu einem einzigen 3D-Modell
Ausrichten von Live-Sensordaten an einer vorhandenen Karte zur Lokalisierung

In der Praxis

Ein Team, das LiDAR-Odometrie für einen mobilen Roboter aufbaut, dünnt typischerweise jeden Scan mit einem Voxelgitter aus, nutzt die vorherige Bewegungsschätzung oder integrierte IMU-Daten als Anfangsschätzung und führt Punkt-zu-Ebene-ICP gegen den vorherigen Scan oder eine lokale Karte aus. Bibliotheken wie PCL, Open3D und libpointmatcher bieten fertige Implementierungen. Die wichtigsten Einstellparameter sind der maximale Korrespondenzabstand, die Konvergenzschwelle und die Iterationsgrenze, und Ingenieure prüfen den abschließenden Fitness-Wert, um Fehlschläge an geometrisch merkmalsarmen Orten wie langen Fluren oder offenen Feldern zu erkennen, wo ICP entlang der Richtung ohne Zwangsbedingung gleiten kann.

Häufige Missverständnisse

ICP findet die global beste Ausrichtung — es konvergiert gegen ein lokales Minimum und braucht eine gute Anfangsschätzung
ICP funktioniert bei jeder Geometrie — in degenerierten Szenen wie flachen Böden oder langen Tunneln sind manche Richtungen unbestimmt, und das Ergebnis kann driften
ICP bewältigt Skalierung und Verformung — Standard-ICP schätzt nur eine Starrkörpertransformation; skalierte oder nicht starre Ausrichtung erfordert andere Verfahren

Wer das wissen sollte

Perception-Ingenieure, die mit LiDAR und Tiefenkameras arbeiten
Entwickler für SLAM und Lokalisierung
Manipulationsingenieure, die modellbasierte Objektposenschätzung betreiben
Praktiker in 3D-Scanning und Rekonstruktion

Videos

Iterative Closest Point (ICP) - Computerphile: Computerphile
Iterative Closest Point (ICP) - 5 Minutes with Cyrill: Cyrill Stachniss

Wikipedia

Iterative Closest Point Algorithm

Open Source Robotics