Pose Graph Optimization

Auch genannt: PGO, pose-graph SLAM, pose graph optimisation
Kategorie: State estimation & navigation › Localisation
Schlagwörter: slam, state-estimation, optimization, localization, mapping, factor-graphs

Ein SLAM-Backend-Verfahren, das die Trajektorie eines Roboters verfeinert, indem es einen Graphen von Posen so anpasst, dass die Odometrie- und Loop-Closure-Nebenbedingungen zwischen ihnen möglichst gut übereinstimmen.

Definition

Posengraph-Optimierung (Pose Graph Optimization) ist der Vorgang, die Trajektorie eines Roboters durch Lösen eines Kleinste-Quadrate-Problems über einem Graphen zu schätzen. Jeder Knoten im Graphen ist eine Roboterpose (Position und Orientierung zu einem Zeitpunkt), und jede Kante ist eine gemessene relative Transformation zwischen zwei Posen samt ihrer Unsicherheit, meist als Informationsmatrix ausgedrückt. Kanten stammen aus der Odometrie zwischen aufeinanderfolgenden Posen und aus Loop Closures, bei denen der Roboter einen bereits besuchten Ort wiedererkennt.

Da jede Messung verrauscht ist, stimmen die Kanten nie perfekt überein: Verkettet man Odometriekanten um eine Schleife, landet der Roboter nicht genau dort, wo eine Loop-Closure-Kante ihn verortet. Die Posengraph-Optimierung findet die Menge von Posen, die den gesamten gewichteten quadratischen Fehler über alle Kanten minimiert, und verteilt die Korrektur über die ganze Trajektorie im Verhältnis dazu, wie sicher jede Messung ist.

Das Problem ist nichtlinear, weil sich Rotationen nicht additiv verknüpfen, und wird daher iterativ mit Verfahren wie Gauss-Newton oder Levenberg-Marquardt gelöst, wobei in jedem Schritt um die aktuelle Schätzung linearisiert wird. Die entstehenden linearen Systeme sind dünnbesetzt, da jede Pose nur mit wenigen anderen verbunden ist, und dünnbesetzte Löser machen Graphen mit vielen Tausend Posen handhabbar. Für diesen Schritt werden häufig Open-Source-Bibliotheken wie g2o, GTSAM und Ceres Solver verwendet.

Die Posengraph-Optimierung ist das Backend der meisten graphbasierten SLAM-Systeme. Sie unterscheidet sich vom Bundle Adjustment darin, dass Landmarken nicht als Variablen geführt werden: Nur Posen werden optimiert, was schneller ist, aber einige Informationen verwirft. Ihre bekannte Schwäche ist die Empfindlichkeit gegenüber falschen Loop Closures, weshalb robuste Kostenfunktionen und Ausreißerverwerfung übliche Ergänzungen sind.

Warum es wichtig ist

Odometrie und visuelle Odometrie driften mit der Zeit unbegrenzt, und die Posengraph-Optimierung ist der Standardweg, diese aufgelaufene Drift zu beseitigen, sobald Loop Closures gefunden sind. Sie verwandelt eine Menge lokal genauer, aber global inkonsistenter Messungen in eine Trajektorie und Karte, die über ein ganzes Gebäude oder Gelände konsistent sind.

Wichtige Anwendungen

Graphbasierte SLAM-Backends für LiDAR- und kamerabasierte Kartierung
Beseitigung aufgelaufener Drift aus Rad-, visueller und visuell-inertialer Odometrie
Kartierung über mehrere Sitzungen, mit Zusammenführung zu verschiedenen Zeiten aufgezeichneter Karten
Kartierung mit mehreren Robotern, mit Zusammenführung der Trajektorien mehrerer Roboter in ein Koordinatensystem

In der Praxis

In einem typischen LiDAR- oder visuellen SLAM-Stack fügt das Frontend jedes Mal einen neuen Knoten hinzu, wenn sich der Roboter um eine festgelegte Strecke oder einen festgelegten Winkel bewegt hat, und verbindet ihn über eine Odometriekante mit dem vorherigen Knoten. Ein Modul zur Ortserkennung schlägt Loop Closures vor, die geometrisch verifiziert werden (zum Beispiel mit ICP oder Feature-Matching), bevor sie als Kanten hinzugefügt werden. Das Backend führt dann einen Optimierer wie g2o oder GTSAM aus, entweder im Batch nach jeder Loop Closure oder inkrementell (wie bei iSAM2), und die korrigierten Posen werden zum Neuaufbau der Karte verwendet. Ein großer Teil des Einstellaufwands fließt in die jeder Kante zugewiesenen Kovarianzen und in robuste Kerne wie Huber oder Cauchy, denn eine einzige falsche Loop Closure mit hoher Konfidenz kann die ganze Karte verbiegen.

Häufige Missverständnisse

Die Posengraph-Optimierung erstellt die Karte — sie korrigiert nur die Posen; die Karte wird anschließend neu aufgebaut, indem Sensordaten an den korrigierten Posen platziert werden
Mehr Loop Closures helfen immer — eine einzige falsche Loop Closure kann die ganze Trajektorie verzerren, daher müssen Closures verifiziert und mit robusten Kosten heruntergewichtet werden
Sie ist dasselbe wie Bundle Adjustment — Bundle Adjustment optimiert auch die Positionen der Landmarken, während ein Posengraph nur Posen und relative Nebenbedingungen enthält

Wer das wissen sollte

Ingenieure für SLAM und Lokalisierung
Entwickler mobiler Roboter und autonomer Fahrzeuge
Computer-Vision-Ingenieure, die an visueller Odometrie und Kartierung arbeiten
Studierende, die probabilistische Robotik und Zustandsschätzung lernen

Videos

Understanding SLAM Using Pose Graph Optimization | Autonomous Navigation, Part 3: MATLAB

Open Source Robotics