PnP-Grundlagen – Wiederherstellung der Kameraposition anhand von 3D-Punkten und einem Bild

Angenommen, ein mit einer Kamera aufgenommenes Bild enthält mehrere bekannte 3D-Punkte auf einer Karte, die Punkten im Bild entsprechen. Das Problem, die Position und Ausrichtung der Kamera zu bestimmen, wird als PnP (Perspective-n-Point) bezeichnet. Es findet breite Anwendung in der visuellen SLAM-Kartenverfolgung, beim Überlagern virtueller Objekte in AR, bei der Hand-Auge-Kalibrierung für Roboter und bei der Lagebestimmung von Vermessungskameras.

0. Zusammenfassung (30 Sekunden)

• Die Eingabe besteht aus der intrinsischen Kameramatrix K, bekannten 3D-Punkten \mathbf X_i und den zugehörigen Bildpunkten \mathbf u_i. Die Ausgabe besteht aus einer Rotation R und einer Translation t.

• Es minimiert den Reprojektionsfehler der Projektionsgleichung \mathbf u_i\sim K(R\mathbf X_i+t). Mit 3 Punkten liefert P3P Lösungskandidaten. Bei vier oder mehr virtuellen Kontrollpunkten ermöglicht Redundanz die Erkennung von Ausreißern.

EPnP stellt jeden Punkt als Linearkombination von vier virtuellen Kontrollpunkten dar und löst so das Problem für viele Punkte schnell. Eine abschließende nichtlineare Optimierung, wie beispielsweise der Levenberg-Marquardt-Algorithmus, verfeinert das Ergebnis.

Wenn Ausreißer in die Korrespondenzen gelangen, kann die gesamte Pose-Schätzung zusammenbrechen. Daher wird sie mit RANSAC-PnP, Tiefenprüfungen und Frame-zu-Frame-Konsistenz verifiziert.

Entartung und Divergenz treten häufig auf, wenn die Punkte nahezu koplanar sind, die Parallaxe gering ist, die intrinsischen Parameter fehlerhaft sind oder Rolling-Shutter-Effekte oder dynamische Objekte in der Szene vorhanden sind.

1. Das Projektionsmodell

Diagram 1 · Use the button to switch views

Abbildung 1 – Übereinstimmende IDs definieren 3D-2D-Korrespondenzen. PnP bildet eine Pose-Hypothese, verwirft Ausreißer durch Reprojektionsresiduum und verfeinert R,t, das Weltkoordinaten in Kamerakoordinaten abbildet.

Sei ein Punkt im Kamerakoordinatensystem \mathbf X_c=R\mathbf X_w+t. Im Lochkameramodell sind die normalisierten Bildkoordinaten

und die Pixelkoordinaten werden über die intrinsische Matrix

als \mathbf u\sim K\mathbf X_c . R\in SO(3) ist die Rotation und t die Translation. Bei Linsenverzerrung ist eine Verzerrungskorrektur vor und nach der Projektion erforderlich.

Die Unbekannten sind die 6 Freiheitsgrade (3 Rotations- und 3 Translationsfreiheitsgrade). Ausgehend von n Korrespondenzen zwischen 3D-Punkten \mathbf X_i und Beobachtungen \mathbf u_i wird der Reprojektionsfehler minimiert.

wird minimiert. \pi ist die Perspektivteilung und \rho ein robuster Verlustfaktor wie z. B. Huber.

Verwechseln Sie die Transformation nicht mit der Kameraposition.

OpenCVs solvePnP gibt rvec, tvec für die Transformation zurück, die Objekt-/Weltpunkte in das Kamerakoordinatensystem abbildet. Um den Kameramittelpunkt in der Welt zu erhalten, … Für Koordinaten verwenden Sie \mathbf C_w=-R^Tt; für eine Kamerapose invertieren Sie T_{cw}, um T_{wc} zu erhalten. Es ist ein häufiger Fehler, tvec selbst als Weltposition der Kamera zu behandeln. Vermeiden Sie außerdem, Verzerrungen zweimal anzuwenden, wenn die Eingabebildpunkte bereits entzerrt wurden.

2. P3P, AP3P und EPnP

P3P (Perspective-3-Point) berechnet die Entfernung vom Kamerazentrum mithilfe der Bildwinkel von drei Punkten und der Abstände zwischen den 3D-Punkten und hat bis zu vier Lösungen. Die korrekte Lösung wird durch Vergleich mit einem vierten Punkt oder der bekannten Pose der Karte ausgewählt. AP3P ist eine schnelle Variante, die die Lösung algebraisch umstrukturiert.

Bei vielen Punkten drückt EPnP (Efficient PnP) jeden 3D-Punkt als gewichtete Summe von vier virtuellen Kontrollpunkten aus.

Die Kamerakoordinaten der Kontrollpunkte werden mithilfe linearer Gleichungen ermittelt, und daraus werden Rotation und Translation abgeleitet. Da der Rechenaufwand nahezu linear mit der Anzahl der Punkte skaliert, eignet sich dieses Verfahren gut, um aus den zahlreichen Landmarken von SLAM eine initiale Pose zu erstellen. Nach der initialen Lösung wird der Reprojektionsfehler iterativ mit dem Levenberg-Marquardt-Algorithmus verfeinert.

3. RANSAC-PnP

Korrespondenzen zwischen Merkmalen und Punkten können sich mit ähnlichen Mustern, sich bewegenden Objekten und fehlerhaften Karten-IDs vermischen. Der Standardansatz ist RANSAC: Es wird eine vorläufige Pose aus einer minimalen Punktmenge erstellt, jede Korrespondenz neu projiziert und die Anzahl der Inlier innerhalb eines Schwellenwerts gezählt. Die Anzahl der benötigten Iterationen N wird anhand der Ausreißerrate \epsilon, der minimalen Stichprobengröße s und der Erfolgswahrscheinlichkeit p bestimmt durch:

Da die erforderliche Anzahl an Iterationen mit der Ausreißerrate stark ansteigt, sollte \epsilon vorab mithilfe eines Merkmalspunktverhältnistests, einer gitterbasierten Dispersion oder einer dynamischen Objektmaske reduziert werden. OpenCVs solvePnPRansac wird durch explizite Angabe der Punktanzahl, der Flags (EPNP, P3P, SQPNP usw.), des Reprojektionsschwellenwerts und des Konfidenzintervalls verwendet.

4. Erkennung von Degeneration

Koplanare Punktmengen

Wenn alle 3D-Punkte in derselben Ebene liegen, werden Tiefe und Pose aus PnP mehrdeutig, und die Geometrie lässt sich ebenso gut durch eine Homographie erklären. Die Schachbrettkalibrierung verwendet bewusst eine Ebene, aber Sie müssen Blickwinkel und Punktanordnungen wählen, die die Freiheitsgrade der Pose ausreichend einschränken. Ein einzelner, frontal betrachteter AR-Marker, der in der Tiefe instabil wird, ist dasselbe Phänomen.

Geringe Bildabdeckung und lange Reichweite

PnP verwendet direkt ein Bild und bekannte 3D-Punkte, daher ist die Parallaxe zwischen den Einzelbildern keine notwendige Eingangsgröße. Die Konditionierung ist jedoch ungünstig, wenn Korrespondenzen nur einen kleinen Bildbereich einnehmen, das Ziel weit entfernt und klein erscheint oder die 3D-Punkte nur geringe Tiefenvariationen aufweisen. Akzeptieren Sie kein Ergebnis allein aufgrund der Anzahl der Inlier: Überprüfen Sie die Bildabdeckung, den RMSE der Reprojektion und die Pose-Kovarianz bzw. die Empfindlichkeit gegenüber Störungen und fusionieren Sie bei Bedarf eine IMU oder einen Tiefensensor.

Kalibrierung und Timing

Fehler in Brennweite, Hauptpunkt und Verzeichnung führen zu einem systematischen Reprojektionsfehler an jedem Punkt. Ein Objektiv, dessen intrinsische Matrix sich mit Zoom, Temperatur oder Fokus ändert, muss neu kalibriert werden. Bei Fahrzeugen und Drohnen liefert PnP eine „verzerrte“ Kamerapose, wenn das Zeilen-Timing eines Rolling Shutter nicht mit der IMU synchronisiert ist.

5. Die Rolle von PnP in Visual SLAM

In Visual SLAM wird die Anzahl der zuvor triangulierten Kartenpunkte berücksichtigt. Mit zunehmender Anzahl an Keyframes kann die Kamerapose Frame für Frame mit PnP verfolgt werden. Bei fixierter Pose werden neue Punkte trianguliert. Sobald genügend Keyframes vorhanden sind, optimiert Bundle Adjustment gemeinsam Pose und Map. Dies lässt sich am besten als Arbeitsteilung verstehen: PnP ist das ressourcenschonende Frontend, während Bundle Adjustment die globale Konsistenz sicherstellt.

6. Checkliste für die Implementierung

1. Kalibrieren Sie K und die Verzerrung mithilfe eines Schachbrettmusters oder Ähnlichem und notieren Sie den Reprojektionsfehler.

2. Richten Sie die Einheiten (m/mm) und das Koordinatensystem der 3D-Punkte am korrigierten Zustand der Bildpunkte aus.

3. Schränken Sie Korrespondenzen mithilfe eines Verhältnistests, der Suche nach nächsten Nachbarn und der zeitlichen Verfolgung ein.

4. Entfernen Sie Ausreißer mit RANSAC-PnP und speichern Sie die Verteilung der Ausreißer sowie den Reprojektionsfehler.

5. Prüfen Sie, ob die Tiefe positiv ist, die Posenänderung physikalisch plausibel ist und die Differenz zum vorherigen Frame angemessen ist.

6. Bei ungünstigen Bedingungen kann auf eine IMU, Tiefenmessung, eine Homographie oder eine Reinitialisierung zurückgegriffen werden.

Minimale Implementierungssequenz

Mit OpenCV wird zunächst rvec, tvec, inliers aus solvePnPRansac ermittelt. Anschließend werden nur die Inlier an solvePnPRefineLM übergeben. Abschließend werden mit projectPoints der RMSE der Inlier und die Fehlerverteilung selbst berechnet. Ein erfolgreicher API-Aufruf allein erkennt weder übermäßige Abweichungen noch gehäufte Punkte oder physikalisch unmögliche Positionen.

ok, rvec, tvec, inliers = cv2.solvePnPRansac(
    object_points, image_points, K, dist,
    flags=cv2.SOLVEPNP_EPNP,
    reprojectionError=3.0, confidence=0.999, iterationsCount=200,
)
if not ok or inliers is None or len(inliers) < 6:
    raise RuntimeError("PnP failed or has too few inliers")

idx = inliers.ravel()
rvec, tvec = cv2.solvePnPRefineLM(
    object_points[idx], image_points[idx], K, dist, rvec, tvec
)
projected, _ = cv2.projectPoints(object_points[idx], rvec, tvec, K, dist)
rmse = np.sqrt(np.mean(np.sum(
    (projected.reshape(-1, 2) - image_points[idx].reshape(-1, 2)) ** 2,
    axis=1,
)))
R, _ = cv2.Rodrigues(rvec)
camera_center_world = -R.T @ tvec.reshape(3, 1)

Weder 3.0 px noch sechs Inlier stellen einen universellen Akzeptanzschwellenwert dar; sie dienen lediglich als Startwerte für dieses Beispiel. Schwellenwerte werden anhand der Bildauflösung, der Merkmalsgenauigkeit und des zulässigen Positionsfehlers der Anwendung abgeleitet. Zudem wird überprüft, ob die Inlier nicht in einer Bildecke gehäuft auftreten und ob jeder Punkt die Kamerabildtiefe Z_c>0 aufweist.

7. Zusammenfassung

PnP ist die Brücke, die Korrespondenzen zwischen einer 3D-Karte und einem 2D-Bild in eine Kamerapose mit 6 Freiheitsgraden umwandelt. Eine erste Lösung wird mit P3P/EPnP erstellt, Ausreißer werden mit RANSAC entfernt und anschließend durch nichtlineare Optimierung verfeinert. Nur durch die gleichzeitige Verwaltung von Punktanordnung, Kalibrierung, Parallaxe und Zeitsynchronisation wird eine stabile Pose-Schätzung für Visual SLAM oder AR erreicht.

Referenzen

• OpenCV — Kamerakalibrierung und 3D-Rekonstruktion
• Lepetit et al., EPnP
• Gao et al., Vollständige Lösungsklassifizierung für das perspektivische Dreipunktproblem
• Hartley und Zisserman, Mehransichtsgeometrie