Factor Graph

Kategorie: State estimation & navigation › Localisation
Schlagwörter: state-estimation, slam, probabilistic-inference, optimization, sensor-fusion

Ein bipartiter Graph, der eine Wahrscheinlichkeitsverteilung als Produkt von Faktoren darstellt und unbekannte Variablen wie Posen und Landmarken mit den Messungen und A-priori-Annahmen verknüpft, die sie einschränken.

Definition

Ein Faktorgraph (factor graph) hat zwei Arten von Knoten. Variablenknoten stehen für die zu schätzenden Unbekannten, etwa Roboterposen, Geschwindigkeiten, Sensor-Biases oder Landmarkenpositionen. Faktorknoten stehen für Informationen über sie, etwa eine A-priori-Verteilung, eine Odometriemessung zwischen zwei Posen oder eine Kamerabeobachtung einer Landmarke, und jeder Faktor ist nur mit den Variablen verbunden, die er betrifft. Der Graph beschreibt eine gemeinsame Verteilung, die dem Produkt aller seiner Faktoren entspricht, und macht so die Struktur eines Problems explizit: welche Unbekannten miteinander verknüpft sind und welche Messungen dünnbesetzte, lokale Nebenbedingungen darstellen.

Faktorgraphen wurden von Kschischang, Frey und Loeliger (2001) als allgemeiner Rahmen für Inferenz dargelegt und in der Robotik breit übernommen, vor allem durch die Arbeiten von Dellaert und Kaess zu Smoothing and Mapping. Bei gaußschen Rauschmodellen ist die Suche nach den wahrscheinlichsten Werten aller Variablen bei gegebenen Faktoren ein nichtlineares Kleinste-Quadrate-Problem: Jeder Faktor trägt ein mit seiner Informationsmatrix gewichtetes Residuum bei. Löser linearisieren die Residuen, nutzen die Dünnbesetztheit des Graphen, um das entstehende lineare System effizient zu faktorisieren, und iterieren mit Gauss-Newton oder Levenberg-Marquardt. Inkrementelle Löser wie iSAM2 aktualisieren die Lösung beim Eintreffen neuer Faktoren, ohne von Grund auf neu zu lösen. GTSAM implementiert Faktorgraphen direkt, und allgemeine Kleinste-Quadrate-Bibliotheken wie g2o und Ceres werden für dieselben Arten von Problemen verwendet.

Der wichtigste praktische Reiz ist die Modularität. Ein neuer Sensor wird zu einem neuen Faktortyp mit eigenem Residuum und Rauschmodell, sodass IMU-Präintegration, GPS, Radodometrie, LiDAR-Scan-Matches, Loop Closures und gelernte Messmodelle alle in einem Schätzer Platz finden. Posengraph-Optimierung ist ein Spezialfall, bei dem jede Variable eine Pose und jeder Faktor eine Relativposen-Nebenbedingung ist; ein allgemeiner Faktorgraph kann auch Landmarken, Geschwindigkeiten, Biases und Kalibrierparameter enthalten. Fixed-Lag-Smoother marginalisieren alte Variablen, um den Rechenaufwand zu begrenzen, um den Preis, dass einige Linearisierungspunkte eingefroren werden. Ausreißer wie falsche Loop Closures erfordern robuste Kostenfunktionen oder schaltbare Nebenbedingungen, da eine einfache Kleinste-Quadrate-Lösung schon durch einen einzigen fehlerhaften Faktor weit verschoben werden kann.

Warum es wichtig ist

Faktorgraphen sind die gemeinsame Sprache moderner SLAM- und Multisensor-Zustandsschätzung. Sie erlauben Ingenieuren, Sensoren, A-priori-Annahmen und Nebenbedingungen hinzuzufügen, ohne den Schätzer neu zu entwerfen, und machen explizit, welche Rausch- und Unabhängigkeitsannahmen jede Messung mitbringt.

Wichtige Anwendungen

SLAM-Backends für LiDAR-, visuelle und visuell-inertiale Systeme
Multisensorfusion von IMU, GPS, Radodometrie und Vision
Kalibrierung der extrinsischen Parameter von Kamera, LiDAR und IMU
Structure from Motion und Bündelausgleichung
Trajektorienoptimierung, formuliert als probabilistische Inferenz
Multi-Roboter-Kartierung mit gemeinsamen Nebenbedingungen

In der Praxis

Ein Bodenfahrzeug lokalisiert sich mit Bodenradar (GPR) gegenüber einer zuvor aufgezeichneten Karte des Untergrunds. Jeder Zeitschritt fügt eine Posenvariable hinzu; Inertial- oder Radodometrie fügt einen Faktor zwischen aufeinanderfolgenden Posen hinzu; und ein Radarabgleich mit der vorhandenen Karte fügt einen Faktor auf der aktuellen Pose hinzu. Statt ein Sensormodell für das Radar von Hand zu schreiben, kann der Messfaktor aus Daten gelernt werden, wie in Arbeiten zu gelernten Faktorgraph-Modellen für die GPR-Lokalisierung. Das Team betreibt einen inkrementellen Smoother, damit die jüngste Pose in Echtzeit verfügbar ist, und prüft nach jeder Testfahrt die Faktorresiduen, um Messtypen zu finden, deren Rauschmodelle zu eng oder zu locker eingestellt sind.

Häufige Missverständnisse

Ein Faktorgraph ist ein Löser — er ist eine Darstellung des Problems; der Löser, etwa Batch-Gauss-Newton oder inkrementelles Smoothing, ist eine separate Wahl
Faktorgraphen beschreiben nur Posengraphen — sie können Landmarken, Geschwindigkeiten, Biases, Kalibrierparameter und beliebige Messmodelle enthalten
Mehr Faktoren verbessern die Schätzung immer — Faktoren mit falschen Rauschmodellen oder Ausreißermessungen können die gesamte Lösung verfälschen, sofern keine robusten Kosten verwendet werden

Wer das wissen sollte

Ingenieure für SLAM und Zustandsschätzung
Entwickler für Sensorfusion und Navigation
Forscher, die an probabilistischer Inferenz in der Robotik arbeiten
Lokalisierungsteams für autonome Fahrzeuge

Videos

Factor Graph - 5 Minutes with Cyrill: Cyrill Stachniss
MIT Robotics - Frank Dellaert - Factor Graphs for Perception and Action: MIT Robotics

Open Source Robotics