Warum ICP fehlschlägt: Initialisierung, Ausreißer und symmetrische Geometrie

ICP verwendet abwechselnd die Korrespondenz zwischen nächsten Nachbarn und die Schätzung starrer Transformationen. Ein kleineres Residuum beweist nicht, dass die rekonstruierte Pose korrekt ist. Einfache 2D-Formen ermöglichen es uns, Initialisierung, Ausreißer und Symmetrie ohne Sensorrauschen zu isolieren.

Ziel

Diese Implementierung minimiert die quadrierten Abstände zwischen Punkten. In jeder Iteration werden Zielnachbarn für transformierte Quellpunkte gefunden, Übereinstimmungen innerhalb eines Abstandsschwellenwerts gehalten und Rotation/Translation mittels Singulärwertzerlegung (SVD) geschätzt. Eine Determinantenkorrektur verhindert Spiegelungen. Die Iteration wird nach maximal 100 Iterationen beendet. Das Open3D ICP-Tutorial unterscheidet zwischen Punkt-zu-Punkt- und Punkt-zu-Ebene-Zielen; hier wird nur das erstere implementiert.

Eingaben und Ausführung

Laden Sie das Skript herunter und führen Sie python3 engineering_labs.py aus. Die Überprüfung erfolgte mit Python 3.12.3, NumPy 1.26.4 und Matplotlib 3.6.3. Open3D wird nicht benötigt, und es handelt sich nicht um einen Geschwindigkeitsvergleich mit dessen Implementierung.

Das Ziel ist ein L aus 3 m und 2 m langen Segmenten mit insgesamt 102 Punkten und einer doppelten Ecke. Die Quellpunkte werden durch die Umkehrung der tatsächlichen Rotation um 20° und der Translation um (0,4, −0,3) m generiert. Es wird kein Sensorrauschen hinzugefügt. Die korrekte Initialisierung beträgt 18° und (0,35, −0,25) m; die fehlerhafte 110° und (1,5, 1) m.

Drei Arten von Evidenz werden gelesen

Das Residuum entspricht dem RMSE über die akzeptierten Abstände zum nächsten Nachbarn. Die Abdeckung ist der Anteil aller Quellpunkte innerhalb des Schwellenwerts. Synthetische Referenzdaten erlauben auch Rotations- und Translationsfehler.

Diagram 1 · Use the button to switch views

Selbst bei guter Initialisierung bleibt ein Fehler bestehen: Benachbarte Punkte auf regelmäßig abgetasteten Segmenten ermöglichen eine verschobene lokale Lösung. Eine Abdeckung von 1.000 allein würde dies nicht erkennen. Ausreißer liegen bei x = 4–7 m, y = 3 m der Quelle. Laden Sie Ziel, Ein-/Ausgabe der Ausreißer und Zusammenfassung herunter.

Nicht beobachtbare Richtungen in Kreisen und Wänden

Der Kreis hat einen Radius von 1 m und 120 Punkte, wobei die Identität als die wahre Transformation definiert ist. Seine Punktmenge stimmt auch nach einer 90°-Drehung überein, sodass praktisch ein Restwert von null mit einem Lagefehler von 90° koexistiert. Weitere Iterationen können fehlende Informationen nicht ergänzen.

Eine Wand stellt ein verwandtes, aber anderes Problem dar. Der Punkt-zu-Ebene-Fehler misst die Verschiebung entlang der Wandnormalen und beschränkt die tangentiale Bewegung nur unzureichend. Wände mit endlichen Punkt-zu-Punkt-Koordinaten können Informationen aus Endpunkten und deren Platzierung gewinnen, dies ist jedoch in einem langen Korridor ohne sichtbare Endpunkte nicht mehr aussagekräftig. Der Kreis ist ein durchgeführtes Experiment; die Diskussion über die Wand erläutert die Richtungsbeschränkungen des Ziels.

Ein kleinerer Schwellenwert ist keine universelle Lösung

Der Schwellenwert von 0,5 m entfernt zwar diese Ausreißer, kann aber auch korrekte Zuordnungen eliminieren, wenn die Initialisierung weit entfernt ist. Unterschiedliche Schwellenwerte wählen unterschiedliche Punkte aus, daher sind die Residuen allein nicht direkt vergleichbar. Dieser Code stoppt die Aktualisierung bei weniger als drei akzeptierten Punkten; drei Punkte garantieren jedoch keine korrekte Geometrie.

Praktische Optionen umfassen die Odometrie-Initialisierung, die Grob-zu-Fein-Ausrichtung, die Ausreißerfilterung und die Beobachtung von Oberflächen mit unterschiedlichen Orientierungen. Messen Sie deren Auswirkungen in denselben Intervallen und unter denselben Referenzbedingungen.

Weiter zur SLAM-Evaluierung

Dies ist eine 2D-Registrierung mit Einzelbildern, kein Test der Genauigkeit der 3D-Ansteuerung oder der Robustheit gegenüber bewegten Objekten. Die SLAM-Evaluierung fügt Trajektorienfehler, Timing-Ausrichtung und fehlgeschlagene Intervalle hinzu. Variieren Sie zunächst nur den Anfangswinkel, um zu sehen, warum die Konvergenz nicht durch einen einzigen Residuenwert zusammengefasst werden kann.

Führen Sie die gleiche Berechnung lokal aus

Extrahieren Sie die vollständige ausführbare Quellcode-ZIP-Datei oder speichern Sie den Einstiegspunkt des Experiments, die generierten Kernel und den ursprünglichen PID-Quellcode in einem Ordner. PID verwendet die Python-Standardbibliothek; Kalman und ICP benötigen NumPy. Die ursprünglichen Befehle zur Grafikerstellung bleiben verfügbar.

python3 -m pip install numpy==1.26.4
python3 experiment.py icp --parameters '{"case":"good_init","threshold":0.5}' > result.json

Testen Sie bad_init, outliers und symmetric_ring. Vergleichen Sie die Schwellenwerte 10 und 0,5 für Ausreißer. Überprüfen Sie, ob der Ring auch bei nahezu null Residuum einen Rotationsfehler von 90 Grad beibehalten kann.

python3 reproduce.py --experiment icp