Grundlagen der Bündelausgleichung – die nichtlineare Methode der kleinsten Quadrate zur Optimierung von Kameraposen und 3D-Punkten

Die Kamerapositionen und 3D-Punkte, die Structure from Motion (SFM) und Visual-SLAM (VSLAM) mittels Triangulation und PnP (Point-and-Projection) ermitteln, sind bestenfalls grobe Anfangswerte aus linearen Approximationen und sequenzieller Verarbeitung. Bildrauschen, Quantisierungsfehler in Korrespondenzpunkten und fortgepflanzte Fehler aus der sequenziellen Schätzung summieren sich und führen zu einer intern inkonsistenten Rekonstruktion. Die Bündelanpassung (Bundle Adjustment, BA) ist die abschließende Optimierung, die diese Inkonsistenz behebt, indem sie alle Kameraparameter und alle 3D-Punkte gleichzeitig verschiebt und so die Summe der Reprojektionsfehler über alle beobachteten Punkte minimiert. Der Name „Bundle Adjustment“ leitet sich davon ab, dass das Bündel der Lichtstrahlen, die von jedem 3D-Punkt zu jeder Kamera verlaufen, gleichzeitig so angepasst wird, dass es mit den beobachteten Positionen übereinstimmt.

0. 30-Sekunden-Zusammenfassung

• Die Bündelausgleichung ist ein nichtlineares Ausgleichsproblem, dessen Unbekannte die intrinsischen/extrinsischen Kameraparameter und die 3D-Punktkoordinaten sind. Ziel ist die Minimierung der Summe der quadrierten Reprojektionsfehler über alle Beobachtungen.

• Die Lösung erfolgt iterativ mit dem Gauss-Newton-Verfahren oder dem Levenberg-Marquardt-Verfahren (LM). LM interpoliert mithilfe eines Parameters \lambda stetig zwischen dem instabilen, aber schnellen Gauss-Newton-Verfahren und dem langsamen, aber stabilen Gradientenabstiegsverfahren.

• Da eine einzelne Kamera immer nur mit den tatsächlich erfassten Punkten verknüpft ist, nehmen die Jacobi- und die Hesse-Matrix eine dünnbesetzte, blockstrukturierte Form an, die durch Kamera × Punkt organisiert ist. Diese Dünnbesetztheit ist der Schlüssel zur handhabbaren Lösung großskaliger Probleme.

Der Schur-Komplement-Trick nutzt die Unabhängigkeit (Blockdiagonale) der 3D-Punktblöcke. Durch die Eliminierung der Punkte wird ein kleines, reduziertes Kamerasystem gelöst, das nur die Kameras umfasst. Dadurch lassen sich Probleme mit Zehntausenden von Punkten und Tausenden von Kameras in realistischer Zeit lösen.

Die Bündelausgleichung ist eng mit der Pose-Graph-Optimierung von SLAM verwandt. Sie unterscheidet sich dadurch, dass die 3D-Punkte selbst zu den Unbekannten gehören. Es handelt sich um eine gemeinsame Basistechnologie, die sowohl zur Vervollständigung von SfM als auch zur lokalen/globalen Optimierung von SLAM verwendet wird.

1. Welche Eingabewerte werden benötigt und was wird berechnet?

Die Eingabe besteht aus den folgenden drei Anfangsschätzungen, die als Zwischenergebnisse von SfM oder Visual-SLAM gewonnen wurden:

• Anfangswerte für die Kamerapositionen \{K_i, R_i, \mathbf{t}_i\} ( i=1,\dots,m ; in vielen Fällen sind die intrinsischen Parameter K_i bekannt oder festgelegt)

• Anfangswerte für die 3D-Punkte \{\mathbf{X}_j\} ( j=1,\dots,n ; grobe Koordinaten aus der Triangulation)

• Die Zuordnung, welche Kamera welchen Punkt beobachtet hat – d. h. die Bildkoordinaten \mathbf{u}_{ij} für jede Beobachtung (die Pixelposition, an der Kamera i den Punkt j erfasst hat)

Die Ausgabe sind Kamerapositionen und 3D-Punkte, die alle gleichzeitig feinjustiert werden und intern präziser sind. Die Ergebnisse sind über alle Beobachtungen hinweg konsistent. Die Tatsache, dass die Bündelausgleichung keine Methode zur Entwicklung einer Lösung von Grund auf ist, sondern eine Feinabstimmung darstellt, die einen bereits annähernd korrekten Anfangswert lokal verfeinert, ist für die nachfolgende Diskussion der Konvergenz relevant.

2. Die Kostenfunktion: Reprojektionsfehler

Die Diskrepanz zwischen der Projektionsposition eines 3D-Punktes \mathbf{X}_j in die Kamera i und der tatsächlich beobachteten Bildkoordinate \mathbf{u}_{ij} wird als Reprojektionsfehler bezeichnet. Schreibt man die Pose der Kamera i als R_i, \mathbf{t}_i und die Projektionsfunktion als \pi(\cdot) (die nichtlineare Abbildung, die homogene Koordinaten in Pixelkoordinaten umwandelt), so ergibt sich für eine Beobachtung das Residuum zu:

Die Bündelausgleichung minimiert die Summe der Quadrate dieses Residuums über alle beobachteten Paare \mathcal{O}=\{(i,j)\}.

\rho ist eine robuste Verlustfunktion, wie beispielsweise die Huber-Verlustfunktion. Sie verhindert, dass ein einzelner großer Ausreißer aufgrund einer Fehlanpassung die gesamte Optimierung verfälscht. Diese Gleichung hat genau dieselbe Form wie die im SfM Primer beschriebene. Die Bündelausgleichung befasst sich mit dem eigentlichen Rechenprozess der Lösung dieses Minimierungsproblems.

3. Lösung als nichtlineare Methode der kleinsten Quadrate: Vom Gauss-Newton- zum Levenberg-Marquardt-Verfahren

Indem alle Unbekannten in einem einzigen Vektor \mathbf{x} zusammengefasst werden (alle Kameraposen und alle 3D-Punkte werden gemeinsam dargestellt) und die gesamte Menge der Residuen als \mathbf{r}(\mathbf{x}) geschrieben wird, ergibt sich das Minimierungsziel \|\mathbf{r}(\mathbf{x})\|^2. Da \mathbf{r} nichtlinear ist, verwenden wir eine Taylor-Entwicklung erster Ordnung um die aktuelle Schätzung \mathbf{x}_k : \mathbf{r}(\mathbf{x}_k+\Delta\mathbf{x})\approx \mathbf{r}(\mathbf{x}_k)+J\Delta\mathbf{x} . J=\partial \mathbf{r}/\partial \mathbf{x} ist die Jacobi-Matrix. Durch Einsetzen und Auflösen nach \Delta\mathbf{x} erhalten wir die Normalgleichung des Gauß-Newton-Verfahrens:

H=J^\mathsf{T}J ist die Hesse-Matrix-Approximation (die Gauß-Newton-Approximation ohne Terme zweiter Ordnung). Das Auflösen dieser Gleichung nach \Delta\mathbf{x}, die Aktualisierung von \mathbf{x}_{k+1}=\mathbf{x}_k+\Delta\mathbf{x} und die Wiederholung dieses Vorgangs bis zur Konvergenz des Residuums bilden das gesamte Verfahren.

Das Gauß-Newton-Verfahren konvergiert schnell, wenn der Anfangswert nahe an der Lösung liegt, neigt aber bei einem ungünstigen Anfangswert zur Divergenz. Der Levenberg-Marquardt-Algorithmus (LM), unabhängig voneinander von Levenberg (1944) und Marquardt (1963) vorgeschlagen, vereinfacht dieses Problem durch Hinzufügen eines Dämpfungsterms zur Normalgleichung.

D ist üblicherweise die Diagonale von J^\mathsf{T}J (oder einer äquivalenten Skalierungsmatrix), und \lambda ist der Dämpfungskoeffizient. Ist \lambda klein, verhält sich der Algorithmus ähnlich wie das Gauss-Newton-Verfahren und konvergiert schnell; ist \lambda groß, werden kleine, sichere Schritte in Richtung des steilsten Abstiegs unternommen. Durch ein adaptives Steuerungsschema – Verkleinern von \lambda zur Beschleunigung bei sinkenden Kosten in jeder Iteration und Vergrößern von \lambda zur Verwerfung und Verkürzung des Schritts bei steigenden Kosten – vereint der LM-Algorithmus die Geschwindigkeit des Gauss-Newton-Verfahrens mit der Stabilität des steilsten Abstiegs. Nahezu jede praktische Implementierung der Bündelausgleichung (Ceres Solver, g2o, SBA und andere, siehe unten) verwendet LM oder eine eng verwandte Trust-Region-Methode.

4. Warum ist die Jacobi-Matrix dünnbesetzt?

Die Unbekannten der Bündelausgleichung sind die Kameraposition (6 Freiheitsgrade – 3 Rotationen plus 3 Translationen, wenn die intrinsischen Parameter festgelegt sind) × m Kameras und 3D-Punkte (3 Freiheitsgrade) × n Punkte, was bis zu 6m+3n Dimensionen ergibt. Bei realen SfM-Problemen mit Zehntausenden von Beobachtungen oder mehr führt die naive Behandlung dieser J^\mathsf{T}J als dichte Matrix zu einem Aufwand von O((6m+3n)^3) – nicht lösbar in realistischer Zeit.

Hierbei erweist sich die Struktur als hilfreich, die besagt, dass der Reprojektionsfehler r_{ij} „nur von den Parametern der Kamera i und des Punktes j abhängt“. Die partiellen Ableitungen nach jeder anderen Kamera k\neq i oder jedem anderen Punkt l\neq j sind identisch null.

Anders ausgedrückt: Die Zeile der Jacobi-Matrix, die durch eine einzelne Beobachtung erzeugt wird, enthält nur im Block für die entsprechende Kamera und im Block für den entsprechenden Punkt Einträge ungleich null. Die Anzahl der Zeilen wächst proportional zur Anzahl der Beobachtungen, die Anzahl der Einträge ungleich null pro Zeile bleibt jedoch konstant (Kamera 6 + Punkt 3, oder etwas mehr, wenn intrinsische Parameter berücksichtigt werden). Diese Sparsität zeigt sich in der Blockstruktur von J^\mathsf{T}J.

• B : Wechselwirkungen zwischen Kameraparametern. Dieser Wert ist null, es sei denn, die Kameras i und k beobachten einen gemeinsamen Punkt. Daher weist er eine dünnbesetzte Blockstruktur auf.

• C : Wechselwirkungen zwischen 3D-Punktparametern. Da die drei Freiheitsgrade eines Punktes j niemals mit einem anderen Punkt gekoppelt sind, handelt es sich um eine Blockdiagonalmatrix – die Voraussetzung für den Schur-Komplement-Trick im nächsten Abschnitt.

• E : Wechselwirkungen zwischen Kameras und Punkten (für jede Beobachtung (i,j) erscheint ein von null verschiedener Block).

5. Die grundlegende Pipeline

Diagram 1 · Use the button to switch views

In jeder Iteration wird der Reprojektionsfehler neu berechnet und die dünnbesetzte Jacobi-Matrix zusammengesetzt. Das reduzierte System (nur Kamera) wird mithilfe des Schur-Komplements gelöst und die Aktualisierung erfolgt über die Schrittsteuerung des LM-Algorithmus. Dieser Vorgang wird wiederholt, bis die Kostenänderung einen Schwellenwert unterschreitet oder eine maximale Anzahl von Iterationen erreicht ist.

6. Der Schur-Komplement-Trick: Sparsity in reduzierte Berechnung umwandeln

Mithilfe der Blockstruktur aus dem vorherigen Abschnitt lässt sich die Normalgleichung (J^\mathsf{T}J+\lambda D)\Delta\mathbf{x}=-J^\mathsf{T}\mathbf{r} in die Kameraaktualisierung \Delta\mathbf{c} und die Punktaktualisierung \Delta\mathbf{p} aufteilen:

(wobei B', C' die Blöcke nach Hinzufügen des Dämpfungsterms sind). Da C' eine Blockdiagonalmatrix ist, die für jeden 3D-Punkt unabhängig ist, kann jeder 3×3-Block einzeln invertiert werden. Der Aufwand ist dabei annähernd proportional zur Anzahl der Punkte n. Durch die Verwendung von C'^{-1} zur Eliminierung von \Delta\mathbf{p} ergibt sich das reduzierte Kamerasystem, das nur noch Kameras umfasst:

Die linke Seite von B'-EC'^{-1}E^\mathsf{T} wird als Schur-Komplement bezeichnet. Diese Matrix hat die Größe 6m\times 6m (und hängt nur von der Anzahl der Kameras ab, nicht von der Anzahl der Punkte n). Sobald \Delta\mathbf{c} gelöst ist, kann die Aktualisierung jedes Punktes kostengünstig wiederhergestellt werden mit

In einem typischen SfM-Problem kann die Anzahl der Punkte n um ein Vielfaches höher sein als die Anzahl der Kameras m. Anstatt also ein System mit 6m+3n Dimensionen zu lösen, genügt es, das Schur-Komplement mit der Dimension 6m zu behandeln. Dies ist die Kernidee, die die Bündelausgleichung auch bei Zehntausenden von Punkten praktisch lösbar macht. Die theoretische Grundlage bildete Triggs et al. in „Bundle Adjustment – A Modern Synthesis“ (2000). Es ist die Standardimplementierung in aktuellen Bibliotheken wie Ceres Solver und g2o.

Ceres Solver bietet mehrere Optionen zur Lösung dieses reduzierten Systems: DENSE_SCHUR, das es als dichte Matrix löst (bis zu einigen hundert Kameras); SPARSE_SCHUR, das die Sparsität durch Umordnung ausnutzt (Tausende von Kameras); und ITERATIVE_SCHUR, das den konjugierten Gradienten auf das Schur-Komplement anwendet (für noch größere Probleme). Die Wahl der Methode anhand der Problemgröße ist eine praktische Faustregel.

7. Eichfreiheit: Richtungen, in denen die Lösung nicht eindeutig bestimmt ist

Bundle Adjustment behält einen Freiheitsgrad bei, der es ermöglicht, den gesamten Parametersatz zu verändern, ohne den Wert der Kostenfunktion zu ändern. Wenn alle Kameras und alle 3D-Punkte gleichzeitig um dieselbe Rotation, Translation und Skalierung bewegt werden, bleibt der Reprojektionsfehler unverändert (im monokularen Fall ist die absolute Skalierung ebenfalls unbestimmt). Dieser Freiheitsgrad wird als Eichfreiheit bezeichnet. Wird er nicht berücksichtigt, führt er zu einer Singularität (Rangdefizit) der Normalengleichung, wodurch diese entweder unlösbar oder numerisch instabil wird.

In der Praxis wird dies umgangen, indem die Position der ersten beiden Kameras oder eine Basislänge festgelegt wird oder indem man sich darauf verlässt, dass der Dämpfungsterm des LM-Modells diese singuläre Richtung implizit regularisiert. Sind Informationen zur absoluten Skalierung oder Position verfügbar – beispielsweise von GPS oder einer IMU –, bietet es sich an, diese als zusätzliche Randbedingung zur Festlegung der Eichfreiheit zu verwenden.

8. Der Unterschied zur Pose-Graph-Optimierung

Die Pose-Graph-Optimierung, die in Loop Closure Primer behandelt wird, gehört zum selben mathematischen Rahmen wie die Bündelausgleichung, da sie mit einem robusten Verlust ein nichtlineares Kleinste-Quadrate-Residuum minimiert, das mithilfe der \mathrm{Log}-Abbildung erstellt wird. Der Unterschied liegt in den tatsächlichen Unbekannten.

In realen Visual-SLAM-Systemen ist eine Arbeitsteilung üblich: Lokale Bündelausgleichung (lokale BA) mit 3D-Punkten verfeinert den Bereich um Keyframes framebasiert, während eine ressourcenschonende Pose-Graph-Optimierung ohne explizite 3D-Punkte die globale Trajektorie schnell korrigiert, sobald ein Schleifenschluss erkannt wird. Vollständige Bündelausgleichung mit 3D-Punkten (globale BA) ist genauer, aber rechenintensiver und kann daher in Echtzeit-Situationen nicht häufig ausgeführt werden.

9. Repräsentative Implementierungen

• Ceres Solver: Eine von Google entwickelte, universelle Bibliothek für nichtlineare Ausgleichsrechnung, die seit 2010 produktiv eingesetzt wird. Sie verfügt über integrierte Schur-basierte Solver und dient als Backend für die Bündelausgleichung in vielen SfM/SLAM-Implementierungen, darunter COLMAP.

• g2o: Ein von Kümmerle et al. auf der ICRA 2011 veröffentlichtes Framework zur Graphoptimierung, das sowohl die Pose-Graph-Optimierung von SLAM als auch die Bündelausgleichung innerhalb desselben Frameworks ermöglicht. Es wird häufig als Backend der ORB-SLAM-Familie verwendet.

• SBA (Sparse Bundle Adjustment): Eine frühe, öffentlich verfügbare Implementierung, die speziell für die Sparse Bundle Adjustment entwickelt wurde und 2009 von Lourakis und Argyros in den ACM Transactions on Mathematical Software veröffentlicht wurde. Sie wird oft als repräsentatives Beispiel angeführt, das den Schur-Komplement-Trick explizit implementiert.

• Integrierte Bundle Adjustment (BA) von COLMAP: Intern wird der Ceres Solver verwendet, der bei jedem Schritt der inkrementellen SfM automatisch zwischen lokaler und globaler Bundle Adjustment wechselt.

10. Schwierige Bedingungen und häufige Fehlerfälle

• Unzureichende Startwerte: Bundle Adjustment ist eine lokale Optimierung. Liegt der Startwert weit von der wahren Lösung entfernt, kann die Konvergenz zu einer falschen lokalen Lösung führen oder ganz ausbleiben. Die Qualität des Startwerts, der durch Triangulation oder PnP gewonnen wird, bestimmt die endgültige Genauigkeit.

• Punkte mit wenigen Beobachtungen oder geringer Parallaxe: Ein Punkt, der nur anhand einer sehr geringen Anzahl von Bildern beobachtet wurde oder eine geringe Parallaxe aufweist, neigt zu einer schlecht konditionierten Jacobi-Matrix und kann einen großen Restfehler verursachen, der sich ausschließlich in Tiefenrichtung konzentriert.

• Starke Ausreißer-Kontamination: Bei vielen Fehlanpassungen kann selbst ein robuster Verlust die Ausreißer nicht vollständig kompensieren, und korrekte Nachbarpunkte können ebenfalls verzerrt werden.

• Extrem große Probleme: Bei Rekonstruktionen im Stadtmaßstab mit Millionen von Beobachtungen werden die Rechen- und Speicherkosten selbst mit dem Schur-Komplement nicht vernachlässigbar. Daher muss das Problem aufgeteilt und parallelisiert oder mit Näherungsverfahren (z. B. durch Vergröberung des Vertrauensbereichs des LM-Algorithmus) kombiniert werden.

• Nicht berücksichtigte Eichungsfreiheit: Wie bereits erwähnt, führt das Vergessen der Eichungsfixierung zu numerischer Instabilität, Konvergenzfehlern oder unphysikalischen Lösungen.

11. Praktische Entscheidungen

• Wenn Sie im letzten Schritt von SfM eine hohe Genauigkeit benötigen, ist die Durchführung einer vollständigen Bündelausgleichung am Ende – unabhängig von der gewählten Strategie (inkrementell oder global) – Standard. Die Standardeinstellungen einer bestehenden Implementierung wie COLMAP führen in der Regel nicht zu falschen Ergebnissen.

• Für Echtzeit-SLAM ist die vollständige Bündelausgleichung für jedes einzelne Frame zu rechenaufwändig. Ein praktischer Ansatz kombiniert lokale Bündelausgleichung anhand der aktuellsten Keyframes mit einer Pose-Graph-Optimierung, die nur beim Schleifenabschluss ausgeführt wird.

– Wenn Sie Ihre eigene Pipeline von Grund auf neu entwickeln, ist es sinnvoll, auf einer Bibliothek wie Ceres Solver oder g2o aufzubauen. Die Entwicklung einer Schur-Komplement-Implementierung von Grund auf bietet im Vergleich zum Aufwand für die Korrektheitsprüfung nur geringen Lernnutzen.

– Für großflächige, stadtweite Rekonstruktionen sollten Sie anstelle einer einzelnen Bündelausgleichung Methoden in Betracht ziehen, die das Problem regional aufteilen und hierarchisch integrieren (dies wird häufig auch als Vorstufe für Multi-View Stereo verwendet, das weiter unten beschrieben wird).

12. Zusammenfassung

Die Bündelausgleichung ist ein nichtlineares Ausgleichsproblem, das den Reprojektionsfehler über alle Kameras und 3D-Punkte hinweg minimiert und iterativ mit dem Levenberg-Marquardt-Verfahren gelöst wird. Der Schur-Komplement-Trick, der die Sparsität der Kamera-Punkt-Beobachtungsbeziehung ausnutzt, ermöglicht es, diese Optimierung selbst bei Zehntausenden von Punkten in realistischer Zeit zu lösen. Obwohl er denselben mathematischen Rahmen wie die Pose-Graph-Optimierung verwendet, hängt die Wahl zwischen den beiden letztendlich davon ab, ob 3D-Punkte explizit gehalten werden. Es handelt sich um eine gemeinsame Basistechnologie, die die Genauigkeit von SfM und SLAM maßgeblich beeinflusst.

Referenzen

• Triggs, McLauchlan, Hartley & Fitzgibbon, Bundle Adjustment — A Modern Synthesis (Vision Algorithms: Theory and Practice, 2000)
• Kümmerle, Grisetti, Strasdat, Konolige & Burgard, g2o: A General Framework for Graph Optimization (ICRA 2011)
• Lourakis & Argyros, SBA: A Software Package for Generic Sparse Bundle Adjustment (ACM Transactions on Mathematical Software, 2009)
• Ceres Solver official documentation: Non-linear Least Squares
• Ceres Solver official Dokumentation: Schur-basierte lineare Löser
• Hartley & Zisserman, Multiple View Geometry in Computer Vision (offizielle Autorenseite)