Object Pose 6D Estimation 中文
← Back to the knowledge system
07 3D Vision Visual SLAM Robotics Robotic Grasping

3D–3D Alignment & Point Cloud Registration

3D–3D Alignment
and Point Cloud Registration

When RGB-D, stereo, or multiview reconstruction lifts observations into 3D, pose estimation becomes geometric alignment. Known correspondences permit a single SVD solve; unknown correspondences require alternating matching and transform updates. Their input contracts, convergence basins, and failure modes differ fundamentally.

Open the Registration Lab Start with the three problem types

Problem Contract

Specify source → target before choosing a solver

This site uses column vectors and {}^T T_S to map source-frame points into the target frame. Source points are qi and targets pi. The same equation can represent CAD→camera, previous→current frame, scan→map, or camera→robot base.

Rigid 3D–3D alignment
pi≃TRSqi+TtS,TRS∈SO(3)\mathbf p_i \simeq {}^{T}\mathbf R_{S}\mathbf q_i+{}^{T}\mathbf t_{S},\qquad {}^{T}\mathbf R_{S}\in SO(3)
qi is in source and pi in target; t uses the point-coordinate units. The result is source-to-target. Its inverse uses Rᵀ and −Rᵀt, rather than negating t alone.
KNOWN CORRESPONDENCE

Point set alignment

Semantic keypoints, shared surfels, NOCS coordinates, or trajectory timestamps already identify points. The objective is fixed, and SVD solves it in one step.

UNKNOWN CORRESPONDENCE

Point Cloud Registration

Raw point clouds have no shared point order. Nearest neighbors, local descriptors, or probabilistic models must infer which observations represent the same surface location.

PARTIAL OVERLAP

Only the overlap constrains pose

Occlusion, fields of view, and scan paths create nonoverlapping regions. Forcing every source point to match a target inevitably brings outliers into the solution.

SOURCE

Data to move, such as CAD, a current scan, or camera frame t−1.

→
TTS

Rigid transform re-expressing source data in the target frame.

→
TARGET

Fixed reference, such as an observed cloud, map, camera frame t, or robot base.

↔
INVERSE

STT = [Rᵀ, −Rᵀt]; swapping source and target requires inversion.

Closed-form Alignment

Centering isolates translation; SVD recovers rotation

Kabsch / Orthogonal Procrustes gives the optimal rigid rotation and translation for known correspondences with isotropic squared errors. Umeyama adds a global scale within the same structure and explicitly prevents mirrored rotations.

Centroids and cross-covariance
qˉ=1n∑iqi,pˉ=1n∑ipi,H=∑i(qi−qˉ)(pi−pˉ)⊤=UΣV⊤\bar{\mathbf q}=\frac{1}{n}\sum_i\mathbf q_i,\quad \bar{\mathbf p}=\frac{1}{n}\sum_i\mathbf p_i,\quad \mathbf H=\sum_i(\mathbf q_i-\bar{\mathbf q})(\mathbf p_i-\bar{\mathbf p})^\top=\mathbf U\mathbf\Sigma\mathbf V^\top
Here H is defined as source-centered × target-centeredᵀ. Swapping these factors requires transposing the resulting R formula. Apparently opposite formulas often reflect different covariance directions.
Proper rigid transform
D=diag⁡(1,1,det⁡(VU⊤)),R=VDU⊤,t=pˉ−Rqˉ\mathbf D=\operatorname{diag}(1,1,\det(\mathbf V\mathbf U^\top)),\quad \mathbf R=\mathbf V\mathbf D\mathbf U^\top,\quad \mathbf t=\bar{\mathbf p}-\mathbf R\bar{\mathbf q}
D corrects a possible mirrored solution in O(3) back to SO(3). RᵀR≈I alone is insufficient: reflections are also orthogonal, so check det(R)≈+1.
Umeyama similarity transform
pi≃sRqi+t,s=tr⁡(ΣD)∑i∥qi−qˉ∥2,t=pˉ−sRqˉ\mathbf p_i\simeq s\mathbf R\mathbf q_i+\mathbf t,\quad s=\frac{\operatorname{tr}(\mathbf\Sigma\mathbf D)}{\sum_i\|\mathbf q_i-\bar{\mathbf q}\|^2},\quad \mathbf t=\bar{\mathbf p}-s\mathbf R\bar{\mathbf q}
The common 1/n factor is omitted from numerator and denominator. s models one isotropic scale; it cannot fix nonuniform CAD deformation, depth-dependent bias, or incorrect intrinsics.
KABSCH

Optimal rotation at fixed scale

Usually the optimal rotation of centered point sets; adding centroid translation yields full rigid alignment.

PROCRUSTES

A broader problem family

The literature may allow translation, scale, or reflection. Check the permitted transform group when a method is called Procrustes.

UMEYAMA

Scale estimation with reflection prevention

Suitable for monocular trajectory scale alignment and category-level canonical coordinates; usually disable scale estimation when RGB-D and CAD both use meters.

Minimal geometry

Enough points do not guarantee stable geometry

1 POINT

Constrains only a translation combination; rotation is entirely unobservable.

2 POINTS

Constrains one direction, but rotation around the connecting line remains free.

3 NON-COLLINEAR

Three noiseless, identified points can define a handed rigid frame.

COLLINEAR / CLUSTERED

Rank deficiency or poor conditioning; some rotation directions remain unreliable even with small residuals.

From uniform least squares to weighted alignment

If point precision differs, weight centroids and covariance by confidence. Directional uncertainty, such as greater depth uncertainty along a viewing ray, requires more than a scalar weight: use covariance, GICP, or a full maximum-likelihood objective. Robust kernels suit heavy-tailed noise, but many outlier correspondences still call for RANSAC consensus first.

Interactive Lab

Run three registration mechanisms

The closed-form view performs 3×3 SVD on synthetic 3D correspondences, with scale and reflection toggles. The robust view samples noncollinear triples, scores all correspondences, and refits all inliers. The ICP view runs the same nearest-neighbor ↔ rigid-update mechanism as 3D point-to-point ICP on a 2D slice to expose convergence basins.

Interactive · 3D–3D Registration

Umeyama, Rigid RANSAC, and ICP Lab

Explore known, incorrect, and unknown correspondences: use centroids and SVD for closed-form alignment, build robust consensus from minimal sets, then examine ICP's sensitivity to initialization, overlap, and correspondence distance.
source → target right-handed · det(R)=+1 distance · m
source target transformed source Point IDs indicate known one-to-one correspondences

Centroid

Centroid Displacement

—Recover translation after rotation and scale

Spectrum

Covariance Singular Values

—Tiny singular values indicate geometric degeneracy

Handedness

Raw Orthogonal Solution

——

Scale / translation

Estimated Parameters

——

Fit

Correspondence RMSE

—A proper rotation cannot fit a mirrored target with zero error
Closed-form alignment assumes known, semantically correct correspondences. Kabsch/Umeyama neither search nearest neighbors nor detect mismatches; they solve the optimal rigid/similarity transform for given correspondences and squared loss. Outliers require weighting, RANSAC, truncated losses, or other robust estimation.

Robust Paired Alignment

Established correspondences can still contain many mismatches

Semantic keypoints, feature matches, and network-predicted 3D–3D correspondences can contain gross errors. Instead of sending all points directly to SVD, generate models from minimal sets, establish full-data consensus, then refit the consensus set.

Rigid correspondence residual
ri=∥pi−(Rqi+t)∥2,i∈I  ⟺  ri<τ3Dr_i=\left\|\mathbf p_i-(\mathbf R\mathbf q_i+\mathbf t)\right\|_2,\qquad i\in\mathcal I\iff r_i<\tau_{3D}
τ3D has length units, not pixels. It should reflect depth noise, voxel size, object scale, and point covariance; millimeter and meter data cannot use the same numerical threshold.
  1. 01

    Sample three noncollinear correspondences

    Check both source and target triangle areas, edge-length ratios, and duplicate indices; exclude ill-conditioned samples from model estimation.

  2. 02

    Generate a proper rigid transform with a closed-form solve

    Estimate R and t, rejecting nonfinite results, det(R) far from +1, or candidates clearly outside task priors.

  3. 03

    Score all correspondences

    Form inliers from 3D residuals, optionally using truncated loss, an M-estimator, or Mahalanobis distance normalized by point covariance.

  4. 04

    Refit all inliers

    A minimal triple only proposes a hypothesis. Recompute the final R and t by SVD on the best consensus set, then update inliers and quality metrics.

From Unknown Pose to Coarse Alignment

Globally align raw point clouds before ICP

When source and target have unknown correspondences and a large relative pose, nearest neighbors are usually incorrect. Local geometric descriptors and robust consistency checks generate wide-baseline hypotheses. The aim is to enter the local optimizer's correct basin, before seeking final millimeter accuracy.

  1. 01

    Unify the data contract

    Check source / target direction, frames, meters or millimeters, timestamps, depth scale, and axes; first crop background irrelevant to the task.

  2. 02

    Preprocess at the appropriate scale

    Choose voxel downsampling to match object size and sensor noise; record normal-estimation radius, neighbor count, and orientation rules.

  3. 03

    Generate wide-baseline candidates

    Compute FPFH or learned local descriptors and cross-check feature matches; propose coarse transforms with RANSAC, FGR, or other robust global methods.

  4. 04

    Verify geometric consistency

    Check edge-length preservation, normal angles, correspondence distances, transform validity, overlap, and symmetry equivalence alongside feature similarity.

  5. 05

    Refine locally at multiple scales

    Reduce voxel sizes and correspondence thresholds from coarse to fine; combine robust kernels and correspondence filtering with point-to-plane, GICP, or Colored ICP.

  6. 06

    Validate at the task level

    Report fitness, inlier RMSE, thresholds, overlap, pose errors, and failure causes; add rendering, silhouettes, collision, or loop-consistency checks as needed.

Canonical global-to-local chain

voxel downsample→normals→FPFH / learned feature→mutual matches→RANSAC / FGR→multi-scale ICP
REPETITION

Repeated structures

Shelves, gears, cylinders, and regular hole arrays create many locally similar descriptors, requiring global geometry or symmetry awareness.

OVERLAP

Low overlap

Many descriptor matches can contain few correspondences from shared visible surfaces; estimate overlap explicitly.

NORMAL

Normal orientation

FPFH and point-to-plane rely on normals. Flipped orientations, incorrect radii, and neighborhoods crossing boundaries can disrupt matching and optimization.

SYMMETRY

Multiple equivalent solutions

Symmetry can let multiple transforms align geometry equally well. Preserve the equivalence class instead of marking another correct symmetric pose as failure.

Local Registration

ICP alternates correspondence and transform updates

Classic ICP transforms source points with the current pose, finds target nearest neighbors, filters unreliable pairs, solves an incremental transform, and repeats until cost or pose changes are small. Pose-dependent correspondences make the objective nonconvex, so initialization determines the convergence basin.

Point-to-point ICP
min⁡R,t∑(i,j)∈K∥pj−(Rqi+t)∥22\min_{\mathbf R,\mathbf t}\sum_{(i,j)\in\mathcal K}\left\|\mathbf p_j-(\mathbf R\mathbf q_i+\mathbf t)\right\|_2^2
For a fixed correspondence set K, closed-form rigid alignment is available; recompute nearest neighbors after every update. Equal penalties in all three directions suit unreliable normals or relatively uniform surface sampling.
Point-to-plane ICP
min⁡R,t∑(i,j)∈K[nj⊤(Rqi+t−pj)]2\min_{\mathbf R,\mathbf t}\sum_{(i,j)\in\mathcal K}\left[\mathbf n_j^\top(\mathbf R\mathbf q_i+\mathbf t-\mathbf p_j)\right]^2
Penalize only the component along target normals, then solve an SE(3) increment after local linearization. Large planes strongly constrain their normal direction but may leave tangential motion weak; boundaries, color, or other structure add constraints.
① TRANSFORM

Move source with the current T

→
② ASSOCIATE

Nearest-neighbor or projective correspondences

→
③ REJECT

Distance, normals, boundaries, and cross-checks

→
④ SOLVE

Estimate and compose ΔT

→
⑤ TEST

Cost, increment, and iteration limit

↺
MethodInputMain objectiveInitializationStrengthsMain risks
Kabsch / Orthogonal ProcrustesKnown, consistently ordered 3D–3D correspondencesSquared point-to-point errorNot requiredClosed-form SVD solution; suitable without outliers or for RANSAC inlier refittingMismatches directly corrupt the solution; det(R) = −1 must be handled
UmeyamaKnown, consistently ordered 3D–3D correspondencesSquared point-to-point error under a similarity transformNot requiredAlso estimates scale; common for trajectories, NOCS, and alignment across scalesMetric point clouds should usually fix s = 1; scale can absorb incorrect units or model bias
Point-to-point ICPTwo raw point cloudsNearest-neighbor point-to-point distanceUsually needs an initial pose near ground truthSimple implementation; no normals requiredSlow convergence along planes; sensitive to density differences, partial overlap, and local optima
Point-to-plane ICPPoint clouds + target normalsSigned distance along target normalsUsually needs an initial pose near ground truthUsually converges faster than point-to-point near smooth surfacesNormal orientation, neighborhood scale, and local plane quality determine results
Generalized ICPPoint clouds + local covariancesCovariance-weighted local plane errorUsually needs a coarse initial poseUnifies point-to-point and point-to-plane ideas using local structure on both sidesSensitive covariance/neighborhood settings; higher computation cost
Colored ICPColored point clouds + normalsGeometric + photometric termsUsually needs a coarse initial poseColor gradients add tangential constraints in geometrically degenerate regionsExposure, white balance, non-Lambertian reflection, and cross-frame color differences can mislead optimization
NDTSource points + target voxel GaussiansLikelihood of points under local probability distributionsGenerally still affected by initializationCompresses the target into voxel distributions; common in laser scan matchingVoxel resolution and optimization step size govern smoothness, speed, and local extrema
FPFH + RANSAC / FGRDownsampled clouds + normals + local descriptorsFeature and geometric consistencyCan start from an unknown poseProvides global or wide-baseline initialization for ICPRepeated structure, low overlap, and incorrect normal/scale settings cause false matches
CORRESPONDENCE GATE

A larger distance threshold is not always better

Too small a threshold leaves no correspondences with poor initialization; too large a threshold draws nonoverlapping regions and wrong surfaces into optimization. Tighten it progressively across scales.

ROBUST LOSS

Robust kernels suppress large residuals

Huber, Cauchy, and Tukey kernels reduce remaining outlier weights through IRLS, but cannot create correct correspondences or missing overlap.

TRIMMING

Retain trusted residual quantiles

With approximate overlap known, discard the largest residual fraction. An incorrect overlap estimate may remove genuine matches or retain outliers.

MULTI-SCALE

Coarse levels widen the basin; fine levels improve accuracy

Larger voxels smooth local geometry and reduce computation; decreasing voxel sizes and thresholds then restores detail.

Evaluation & Acceptance

Registration scores require thresholds and overlap definitions to be reproducible

Evaluate correspondence residuals, cloud coverage, ground-truth pose, surface distances, and downstream tasks. Each answers a different question; a good RMSE can hide low overlap, symmetry, or an incorrect local solution.

Fitness and inlier RMSE
fitness⁡=∣Kτ∣∣Q∣,RMSE⁡inlier=1∣Kτ∣∑(i,j)∈Kτdij 2\operatorname{fitness}=\frac{|\mathcal K_\tau|}{|\mathcal Q|},\qquad \operatorname{RMSE}_{inlier}=\sqrt{\frac{1}{|\mathcal K_\tau|}\sum_{(i,j)\in\mathcal K_\tau}d_{ij}^{\,2}}
Kτ denotes inlier correspondences within maximum distance τ. High fitness can result from an excessive τ; low RMSE may cover only a tiny region. Report τ, source point count, sampling scale, and overlap rules together.
Pose error with ground truth
eR=cos⁡−1 ⁣(tr⁡(R^Rgt⊤)−12),et=∥t^−tgt∥2e_R=\cos^{-1}\!\left(\frac{\operatorname{tr}(\hat{\mathbf R}\mathbf R_{gt}^{\top})-1}{2}\right),\qquad e_t=\|\hat{\mathbf t}-\mathbf t_{gt}\|_2
For rotational symmetry, minimize error over equivalent poses or use symmetry-aware surface metrics. Comparing only one Rgt can incorrectly reject geometrically equivalent poses.
PAIR RESIDUAL

Are correspondences consistent?

Inspect RMS, median, P90/P95, and residual histograms; means can hide long tails and local errors.

COVERAGE

Is the shared surface sufficient?

Check bidirectional nearest neighbors, overlap, and spatial distribution together to avoid aligning only a local plane.

POSE GT

Was the pose recovered correctly?

With ground truth, report rotation, translation, and scale errors, accounting for transform direction and symmetry equivalence.

TASK CHECK

Does it work downstream?

For reconstruction, check loop and surface consistency; for grasping, TCP, collisions, and approach direction; for 6D pose, CAD rendering and visible silhouettes.

Recommended log contract

Minimum Fields to Log for Each Registration

source→target · units · voxel · normal radius · init source · solver · max correspondence · robust loss · iterations · inlier/all · fitness · RMSE/P95 · ΔR/Δt · overlap · stop reason

Across Application Domains

One registration core serves four system roles

6D POSE · RGB-D

CAD / object → observed camera cloud

Segment target depth, back-project a camera cloud, initialize with a network pose or global features, then refine with geometric/color ICP to estimate CTO. Handle partial overlap between the visible surface and full CAD model.

VISUAL / LIDAR SLAM

scan → scan or scan → map

Consecutive frames provide useful motion initialization; ICP, GICP, or NDT recover relative motion. Mismatches accumulate into drift, requiring loop closures and pose-graph optimization.

ROBOTICS

Calibration, workpiece localization, and multisensor extrinsics

Rigid transforms between corresponding geometry or scans enter the base–camera–tool chain. Synchronization, structural flex, and unit errors often exceed solver errors.

GRASPING

Transfer grasps after model → scene registration

Transfer offline CAD grasp poses into the scene using sceneTmodel ; correct local surface alignment still does not guarantee correct global object pose, collision avoidance, or gripper reachability.

Engineering recipe · Open3D Python

Save global candidates and local refinement separately

The example shows pipeline boundaries. Practical systems should explicitly log voxels, normal/feature radii, RANSAC checkers, maximum correspondence distance, and source/target direction. 
# source -> target
source_down, source_fpfh = preprocess(source, voxel)
target_down, target_fpfh = preprocess(target, voxel)

coarse = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
    source_down, target_down, source_fpfh, target_fpfh,
    mutual_filter=True,
    max_correspondence_distance=1.5 * voxel,
    estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
    ransac_n=3,
    checkers=checkers,
    criteria=criteria,
)

fine = o3d.pipelines.registration.registration_icp(
    source, target, 0.4 * voxel, coarse.transformation,
    o3d.pipelines.registration.TransformationEstimationPointToPlane(),
)
DIRECTION

Most libraries return a 4×4 source-to-target transform. Swapping inputs should invert the result, rather than merely change the translation sign.

NORMALS

Before point-to-plane ICP, verify target normals exist at an appropriate scale with stable orientation. After downsampling, re-estimate or correctly aggregate them.

ARTIFACTS

Save coarse and fine transforms, per-level metrics, and aligned clouds. A final matrix alone cannot distinguish global from local failures.

Debug Protocol

Verify one known correspondence before scaling up to a full cloud

Registration errors often come from direction, units, depth back-projection, cloud filtering, and normals, beyond optimizer settings. Layered tests—from single points and known transforms to outlier-free closed-form solves and real unknown correspondences—locate the failing stage.

  1. 01

    Log source / target frames, units, and ranges

    Compare AABBs, centroids, median point spacing, and depth ranges. Meter/millimeter errors invalidate thresholds and can be temporarily hidden by accidentally enabled scale estimation.

  2. 02

    Verify transform direction with one test point

    Calculate p = Rq+t by hand, check that library output is source-to-target, then verify T⁻¹(Tq)=q.

  3. 03

    Start with synthetic noiseless known correspondences

    Kabsch should reach numerical-precision residuals with det(R)≈+1. If it fails, first check H direction, multiplication side, and centroid translation.

  4. 04

    Add noise, mismatches, and missing data gradually

    Change one factor at a time and log inlier ratio, pose error, and degenerate sample count; avoid guessing ten parameters at once in a real scene.

  5. 05

    Visualize coarse and fine results separately

    If coarse alignment enters the wrong symmetry branch, fine ICP can stay there despite a good fit. If correct coarse alignment worsens during refinement, inspect correspondence thresholds, normals, and nonoverlap.

  6. 06

    Draw correspondence lines colored by residual

    Long lines near silhouettes/occlusions suggest partial overlap; consistent directional offsets suggest extrinsics or synchronization; links across repeated structures suggest descriptor ambiguity.

  7. 07

    Check task geometry and numerical termination together

    Inspect CAD rendering, surface coverage, pose jumps, collisions, or loop closures alongside converged=true.

Algorithms, Implementations, and Primary SourcesExpand representative papers and official implementation documentation

Next systems topic · 08

The General Pose Estimation Pipeline

The next topic connects detection, segmentation, geometric cues, hypothesis solving, pose refinement, confidence, and tracking into a complete 6D pose pipeline, including where RGB-only and RGB-D branches enter the system.
Explore the full topic