Object Pose 6D Estimation 中文
← Back to the knowledge system
06 Geometric estimation 3D Vision Visual SLAM Robotics

2D–3D Correspondence & Perspective-n-Point

2D–3D Correspondences
and PnP

Given a camera model and model-point/image-pixel correspondences, PnP recovers the rotation and translation from object or world coordinates to the camera. System performance depends on correspondence correctness, geometric observability, RANSAC design, and a consistent imaging model in final optimization, beyond the solver name alone.

Open the PnP Lab Start with the problem definition

Problem Contract

Recover object-to-camera from known 3D points and their pixels

This site uses right-handed frames, column vectors, and the OpenCV optical frame. Given object-frame 3D points Pi, corresponding pixels pi, intrinsics K, and distortion D, PnP estimates ^C R_O and ^C t_O so that transformed model points project back to the observations through the same camera model.

PnP observation model
pi=π ⁣(K,D,CROPi+CtO)+ϵi,i=1,…,n\mathbf p_i=\pi\!\left(\mathbf K,\mathbf D,{}^{C}\mathbf R_{O}\mathbf P_i+{}^{C}\mathbf t_{O}\right)+\boldsymbol\epsilon_i,\qquad i=1,\ldots,n
π includes perspective division, distortion, and intrinsics; Pi and ti must use the same length units. OpenCV-style rvec and tvec encode object/world → camera, rather than the camera's pose in the object/world frame.
Object pose in camera frame
CPi=CRO OPi+CtO{}^{C}\mathbf P_i={}^{C}\mathbf R_{O}\,{}^{O}\mathbf P_i+{}^{C}\mathbf t_{O}
Common in 6D object pose: CAD / object points pass through ^C T_O into the camera frame, then project to the image. t directly gives the object origin's coordinates in the camera frame.
Camera pose in object / world frame
ORC=(CRO)⊤,OtC=−(CRO)⊤CtO{}^{O}\mathbf R_{C}=({}^{C}\mathbf R_{O})^\top,\qquad {}^{O}\mathbf t_{C}=-({}^{C}\mathbf R_{O})^\top{}^{C}\mathbf t_{O}
SLAM and camera localization often need camera-to-world. Negating t alone is insufficient; rotate it with Rᵀ. The camera center is C_O = −Rᵀt.
6 DoF

There are 6 unknowns, but rotation has nonlinear orthogonality constraints; simply counting two equations per point is insufficient.

→
P3P

Three known interpoint distances and three camera bearings define the minimal problem, which usually has multiple discrete candidates.

→
4th point

Practical APIs often require or recommend a fourth correspondence to validate and rank P3P candidates; check point-count conventions for each API.

→
n points

More well-distributed correspondences provide redundancy for robust estimation, weighting, and nonlinear optimization.

Where Correspondences Come From

PnP explains correspondences; it does not find them

Correspondences may come from fiducials, feature matches, semantic keypoints, or dense object coordinates. Their noise, correlations, spatial layouts, and failure modes differ, calling for different sampling, weighting, and diagnostics.

Correspondence sources3D side2D sideStrengthsMain risks
Semantic keypointsPredefined CAD corners, holes, or axis endpointsNetwork heatmaps or coordinate regressionClear semantics; few correspondencesOcclusion, swapped symmetric points, concentrated layouts
Local feature matching3D features from SfM / textured CAD modelsSIFT, SuperPoint, or foundation-model featuresSuitable for camera relocalization and textured objectsRepeated textures and mismatches require robust estimation
Dense object coordinatesCAD / NOCS coordinates for each foreground pixelPixel grids or sampled foreground pointsMany correspondences and broad coverage; suitable for learned systemsNeighboring pixel errors are correlated; count does not equal independent information
Fiducials / planar boardsKnown square corners or calibration-board coordinatesSubpixel corner detectionHigh precision and controlled engineering conditionsStrictly coplanar; often two pose candidates
Rendering / template matchingSurface points or silhouettes under candidate posesObserved edges, masks, or feature locationsCan be combined with iterative refinementCorrespondences change with pose, making this an alternating optimization problem
IDENTITY

Point order is part of the data

imagePoints[i] must observe objectPoints[i]. Swapping symmetric points can produce a geometrically consistent but semantically incorrect pose.

INDEPENDENCE

Ten thousand pixels do not imply ten thousand independent constraints

Dense coordinate errors are highly correlated locally; sample, cluster, or weight by predicted uncertainty.

BASELINE

Coverage matters more than count

A few points spread across object faces and image regions can be more stable than many points concentrated locally.

VISIBILITY

Do not force correspondences for invisible model points

Self-occlusion, truncation, and external occlusion change the available point set; training and inference should use consistent visibility semantics.

Solver Taxonomy

Hypothesis generation, general initialization, and planar solvers serve different roles

Choose a solver by minimal sample size, 3D layout, need for multiple solutions, RANSAC use, and subsequent refinement. A single benchmark's mean speed is insufficient.

SolversPoint countGeometryCandidatesTypical roleCautions
P3P / AP3P3 in theory; common APIs use a 4th point to disambiguateNoncollinear minimal setUp to 4 candidatesRANSAC hypothesis generation; AP3P is a more efficient and stable algebraic implementationEnumerate candidates and rank using extra points, positive depth, and residuals
EPnPn ≥ 4General points; also supports planar configurationsUsually returns oneO(n) noniterative general initialization, commonly used with many correspondencesNonlinear refinement remains advisable with noise, small samples, or ill-conditioned layouts
SQPnPn ≥ 3General and coplanar pointsCan return multiple optimal candidatesFast, globally optimal general-purpose solverGlobal optimality applies to its objective; it does not fix incorrect correspondences, incorrect K, or object-symmetry ambiguity
IPPEn ≥ 4Must be coplanarExplicitly returns 2Planar targets, AR markers, calibration boardsThe two solutions can have similar residuals for small, nearly frontal targets
IPPE_SQUAREExactly 4 pointsSquare with a prescribed point orderExplicitly returns 2Dedicated fast solution for square markersSize, origin, and corner order must exactly match the API specification
ITERATIVE / LMUsually n ≥ 4; initialization requirements depend on the APIGeneral or coplanar pointsConverges to a local solution depending on initializationMinimizes pixel reprojection error; often used for final refinementNot a global initializer; starting pose, robust loss, and parameterization affect convergence
ROBUST MINIMAL

AP3P + RANSAC

With many mismatches, propose candidates quickly from small samples, then score against all correspondences.

GENERAL

SQPnP or EPnP

Generate a general initial pose from many points without obvious outliers; still check positive depth and refine.

PLANAR

IPPE / IPPE_SQUARE

Retain both candidates explicitly, rather than letting a generic single-solution API silently choose the wrong branch.

FINAL

LM / VVS / BA

Minimize reprojection error from a trusted initial pose and inlier set, incorporating weights, robust kernels, or priors.

Interactive Lab

Examine correspondence quality, sampling budget, and geometry together

The first view shows how pixel residuals and thresholds form a consensus set; the second computes exact RANSAC budgets for different minimal sample sizes; the third compares spatial, planar, nearly collinear, and concentrated layouts. The lab separates solver implementation from the robust-estimation logic shared by PnP methods.

Interactive · PnP & Robust Estimation

2D–3D Correspondence, RANSAC, and Degeneracy Lab

See how a pose hypothesis produces pixel residuals, calculate RANSAC success probabilities, then compare the observability of spatial, planar, nearly collinear, and concentrated point layouts.
object → camera pixel residual · px deterministic demo
Hypothesis projection Within threshold Outside threshold Lines indicate correspondences, not physical ray lengths

Consensus

Current Consensus Set

8 / 10Thresholding uses pixel residuals under the current pose hypothesis

Residual

Reprojection RMS

—All correspondences: —

Coverage

2D Coverage

—Observation convex-hull area / image area

Pose contract

OpenCV-Style Output

CTOTransforms object model points into the camera frame
This is not a numerical implementation of a PnP solver. It demonstrates the observation–projection–residual–inlier layer shared by solvers. A practical pipeline should generate candidates with P3P/AP3P, EPnP, SQPnP, or IPPE, then filter and refine them using these residual rules.

RANSAC Is a Pipeline

RANSAC should return more than the minimal solution from one lucky sample

The standard pipeline combines minimal-set hypotheses, full-data consensus, adaptive budgets, local optimization, and refitting with all inliers. Thresholds, sample size, and degeneracy checks jointly control robustness and speed.

  1. 01

    Sample a minimal set

    Randomly select s points from all 2D–3D correspondences; reject duplicates, nearly collinear sets, and clearly ill-conditioned layouts before forming a sample.

  2. 02

    Generate every valid candidate

    P3P/IPPE may return multiple poses. Check rotation validity, positive depth, and finite values for every candidate.

  3. 03

    Score all correspondences

    Compute reprojection residuals with the same K, distortion model, and pixel coordinates. Form a consensus set with threshold τ, optionally incorporating confidence or covariance.

  4. 04

    Keep the best and adapt the budget

    Rank by inlier count, truncated cost, or an M-estimator; estimate the required iterations from the best current inlier ratio and stop early when appropriate.

  5. 05

    Optimize locally and refit all inliers

    Do not use a minimal-set solution as the final result. Refit with all inliers, update the inlier set, then apply LM/VVS or BA-style refinement.

  6. 06

    Report quality diagnostics

    Log inlier count/ratio, RMS/quantiles, image coverage, minimum depth, candidate separation, and the termination reason.

Ideal RANSAC iteration budget
N≥log⁡(1−p)log⁡(1−ws)N\geq \frac{\log(1-p)}{\log(1-w^s)}
p is the desired success probability, w the inlier ratio, and s the minimal sample size. A small decrease in w or increase in s can greatly increase N. The formula assumes independent uniform sampling and nondegenerate all-inlier samples.
Pixel reprojection residual
ri=∥pi−π ⁣(K,D,RPi+t)∥2,i∈I  ⟺  ri<τr_i=\left\|\mathbf p_i-\pi\!\left(\mathbf K,\mathbf D,\mathbf R\mathbf P_i+\mathbf t\right)\right\|_2,\qquad i\in\mathcal I\iff r_i<\tau
τ uses the pixel units of the current imagePoints. If inputs are undistorted into normalized rays, convert the threshold to that coordinate scale; do not reuse 3 px directly.

Degeneracy & Ambiguity

Solvability, uniqueness, and stability are distinct

Strictly planar targets are solvable through a homography or IPPE, but often require handling two candidates. Nearly collinear points lack key directional constraints. Concentrated points may be full-rank yet amplify pixel noise into large pose errors through poor conditioning.

COLLINEAR

Nearly collinear: reject the minimal set

Check the second eigenvalue of the centered 3D point covariance, or triangle area; adding points on the same line does not help.

PLANAR

Planar: retain and rank both solutions

For frontal, distant, or small targets, reprojection errors can be similar. Use temporal information, noncoplanar points, or task priors to disambiguate.

CLUSTERED

Concentrated: an elongated or flat error valley

Expanding the 3D baseline and image convex hull is usually more effective than increasing point count alone, especially for out-of-plane rotation and depth.

SYMMETRY

Symmetric: correspondence identity is ambiguous

If observations cannot distinguish two model points, output equivalent poses or multiple hypotheses, or use symmetry-aware scoring rather than forcing a unique label.

Refinement & Uncertainty

A useful task pose requires correct refinement after algebraic initialization

Final optimization should use the projection function consistent with the observations, optionally adding robust losses, keypoint covariances, pose priors, and temporal priors. It improves local accuracy but cannot automatically recover from an incorrect branch or severely wrong correspondences.

Weighted robust reprojection refinement
min⁡ξ∑i∈Iρ ⁣(ri(ξ)⊤Σi−1ri(ξ)),T←exp⁡(Δξ^)T\min_{\boldsymbol\xi}\sum_{i\in\mathcal I}\rho\!\left(\mathbf r_i(\boldsymbol\xi)^\top\mathbf\Sigma_i^{-1}\mathbf r_i(\boldsymbol\xi)\right),\qquad \mathbf T\leftarrow\exp(\widehat{\Delta\boldsymbol\xi})\mathbf T
ξ parameterizes SE(3) locally, Σi describes 2D observation uncertainty, and ρ may be a Huber/Cauchy robust kernel. Left versus right updates change Jacobian definitions and must match the implementation's derivation.
HESSIAN / JᵀWJ

Local conditioning and covariance

The optimization Jacobian's spectrum reveals weakly constrained pose directions; a small residual does not make every degree of freedom trustworthy.

MULTI-HYPOTHESIS

Candidate separation

For planar or symmetric scenes, retain multiple candidates and scores so tracking, depth, or robot tasks can disambiguate later.

TASK SPACE

Pixel error differs from task error

Distant small objects may have good reprojection but inaccurate depth. Grasping should also evaluate TCP, collision, and approach-direction errors.

Across Application Domains

PnP output semantics depend on the system

6D POSE

CAD / object → camera

Model points are in the object frame; the output is CTO. Often combined with detection, segmentation, keypoint, or dense object-coordinate networks.

SLAM

map / world → camera

Matching 3D map points to current-frame features yields CTW; trajectory output usually inverts it to WTC.

ROBOTICS

camera pose enters a transform chain

PnP supplies a visual relative pose; hand–eye extrinsics must convert it to the base / end-effector frame, with consistent meters, millimeters, and timestamps.

AR / MARKER

Planar ambiguity and temporal stability

Square markers suit IPPE_SQUARE; rendering still requires candidate disambiguation, temporal filtering, and OpenCV-to-graphics frame conversion.

Engineering recipe · OpenCV Python

Separate hypotheses, consensus, and refinement in code

This example generates RANSAC candidates with AP3P, then applies VVS refinement to all inliers. Set thresholds from the current resolution and keypoint noise. 
ok, rvec, tvec, inliers = cv2.solvePnPRansac(
    object_points, image_points, K, dist_coeffs,
    iterationsCount=1000,
    reprojectionError=3.0,
    confidence=0.999,
    flags=cv2.SOLVEPNP_AP3P,
)

if ok and inliers is not None and len(inliers) >= 4:
    ids = inliers[:, 0]
    rvec, tvec = cv2.solvePnPRefineVVS(
        object_points[ids], image_points[ids],
        K, dist_coeffs, rvec, tvec,
    )
DTYPE / SHAPE

Explicitly check float32/float64, N×3, N×2, and contiguous memory; squeezing a single point must not break array shapes.

MULTIPLE SOLUTIONS

Planar or minimal problems require solvePnPGeneric to enumerate candidates before ranking them with task information.

POST CHECK

Reproject and log inlier quantiles, convex hull, minimum Z, det(R), and rotation/translation differences between candidates.

Debug Protocol

Verify correspondences and the camera model before blaming the solver

PnP errors often arise in preprocessing or postprocessing: swapped point order, stale K after resizing, mixed distortion states, incorrect CAD units, or reversed output direction. Visual reprojection and staged checks are more effective than blindly changing flags.

  1. 01

    Reproject input model points with the ground-truth pose

    If even ground truth misses imagePoints, check point order, K/D, image transforms, frames, and annotations before PnP.

  2. 02

    Plot point IDs and residual vectors

    A consistent global offset suggests a principal-point/crop error; radial edge residuals suggest distortion; a few long arrows suggest mismatches.

  3. 03

    Check output direction and camera center

    OpenCV rvec/tvec map model points into the camera. For camera pose, use Rᵀ and −Rᵀt, rather than negating t alone.

  4. 04

    Check Z for every transformed point

    Most visible model points should have Z > 0. Reject negative-depth candidates; explain cases with few visible points using crop and occlusion information.

  5. 05

    Check 3D layout and 2D coverage

    Apply SVD to centered 3D points and compute the image convex hull. Many concentrated inliers can still provide weak constraints.

  6. 06

    Enumerate solutions before adding iterations

    Obtain all candidates for planar and P3P cases; an incorrect branch is not a small local error that extra LM iterations will necessarily cross.

  7. 07

    Compare solvers last

    Hold inputs, inlier sets, refinement, and output direction fixed before comparing speed, failure rate, and task error.

Algorithms, APIs, and Primary SourcesExpand official OpenCV documentation and representative papers

Next geometry topic · 07

3D–3D Alignment & Point Cloud Registration

When RGB-D, stereo, or multiview reconstruction supplies two sets of 3D correspondences, pixel reprojection becomes rigid alignment. The next topic covers Kabsch, Umeyama, SVD reflection correction, RANSAC, point-to-point / plane ICP, initialization, and local optima.
Explore the full topic