Point Cloud Processing · Vitreous

What is Iterative Closest Point (ICP)?

Point-cloud registration solves the problem of placing two 3D scans in the same coordinate frame, and ICP does this by repeatedly matching nearby points and fitting a rigid transform. Learn point-to-point and point-to-plane ICP, Fast Global Registration, their mathematics and failure modes, and how to build a coarse-to-fine registration pipeline in the Telekinesis Agentic OS.

Imagine that a robot has a CAD-derived point cloud of a metal bracket and a fresh scan from a depth camera. Both clouds describe the same part, but they sit in different coordinate frames. Until the robot finds the rotation and translation between them, it cannot compare the scan with the model, report the part’s pose, or place a tool accurately.

1. What is Point-Cloud Registration?

Point-cloud registration is the process of finding the spatial transformation that brings two overlapping 3D point sets into alignment. The moving cloud is usually called the source and the reference cloud the target. The result is a transform that expresses source points in the target frame.

Iterative Closest Point (ICP) is the best-known local registration method. It begins from a rough alignment, guesses which source and target points correspond, fits a rigid transform to those pairs, moves the source, and repeats. The name is almost the complete algorithm: the correspondences are chosen iteratively from nearby points.

That simple loop is powerful, but the word local matters. If the initial pose is close enough, nearest points are useful approximations to the unknown physical correspondences. If the clouds begin far apart, the nearest points may belong to unrelated surfaces, and ICP can confidently converge to the wrong pose.

This is why a practical robotics pipeline often has two stages. Fast Global Registration (FGR) uses geometric features to estimate a coarse pose without requiring the clouds to start together. ICP then uses the actual surface geometry to refine that pose. FGR answers “roughly where does this scan belong?”; ICP answers “what small rigid motion makes these overlapping surfaces agree?”

1.1 The Complete Coarse-to-Fine Workflow

Two point clouds → one shared coordinate frame

Source point cloudMoving scan or object model
Target point cloudReference scan and coordinate frame
Prepare the geometryFilter, downsample, and estimate normals
Fast Global RegistrationMatch FPFH features for a coarse 6D pose
ICP refinementAlternate closest-point matching and rigid pose updates
Validate the alignmentCheck overlap, residuals, conditioning, and pose limits
Registered geometrySource-to-target rigid transform and aligned cloud

Global feature matching supplies the basin of attraction; local ICP uses dense surface geometry to obtain the final precision.

This practical coarse-to-fine pipeline combines feature-based Fast Global Registration with local Iterative Closest Point refinement and explicit validation before a robot uses the resulting pose. Adapted as an explanatory diagram fromZhou et al., “Fast Global Registration”.

Registration begins with two point clouds expressed in different coordinate frames. The source is the cloud we will move. The target is the reference it should align with. If their starting poses are far apart, simply pairing each source point with its nearest target point can create completely wrong correspondences.

The workflow therefore prepares both clouds before alignment. Downsampling reduces cost and makes density more even. Surface normals describe which way the local geometry faces, while FPFH descriptors summarize the shape around selected points. Those descriptors let similar regions propose matches even when the clouds are initially far apart.

Fast Global Registration uses the proposed feature matches to estimate a coarse rigid pose while rejecting many inconsistent pairs. This first transform only needs to bring the clouds into the same neighborhood. Once they are close, ICP can discard the descriptors and alternate between finding nearby geometric correspondences and updating the rotation and translation.

The final transform is still only the optimizer’s best answer. Validation checks whether enough well-distributed points agree, whether the residual error is acceptable, and whether the overlap makes physical sense. Only then should the registered geometry enter a robot-planning pipeline. The following sections build this coarse-to-fine story from the transformation matrix upward.

1.2 What the 4×4 Transformation Matrix Means

Rigid registration estimates a rotation and translation. For a source point pi∈R3\mathbf p_i\in\mathbb R^3, its transformed position is

p^i=Rpi+t.\hat{\mathbf p}_i = \mathbf R\mathbf p_i + \mathbf t.

Here, pi\mathbf p_i is source point ii; R∈SO(3)\mathbf R\in SO(3) is a 3×33\times3 rotation matrix; t∈R3\mathbf t\in\mathbb R^3 is a translation vector; and p^i\hat{\mathbf p}_i is the source point expressed in the target frame. SO(3)SO(3) means that R\mathbf R preserves lengths and angles and has determinant +1+1, so the optimizer cannot scale or mirror the object.

Robotics software usually combines these terms into a homogeneous matrix:

T=[Rt0T1],[p^i1]=T[pi1].\mathbf T= \begin{bmatrix} \mathbf R & \mathbf t\\ \mathbf 0^{\mathsf T} & 1 \end{bmatrix}, \qquad \begin{bmatrix}\hat{\mathbf p}_i\\1\end{bmatrix} = \mathbf T \begin{bmatrix}\mathbf p_i\\1\end{bmatrix}.

T∈SE(3)\mathbf T\in SE(3) is the complete rigid transform, while the final coordinate value of 1 makes translation possible through matrix multiplication. The matrix is directional: a transform that maps source → target cannot be silently used as target → source. Its inverse performs the opposite mapping.

1.3 The ICP Loop: Match, Fit, Move, Repeat

Suppose the current transform estimate is T(k)\mathbf T^{(k)} at iteration kk. One ICP iteration has four steps:

input: source P, target Q, initial transform T, distance gate d_max

repeat until convergence or max_iterations:
    P_moved ← transform(P, T)
    pairs   ← nearest_target_neighbors(P_moved, Q)
    pairs   ← discard pairs farther than d_max
    ΔT      ← fit_rigid_update(pairs, chosen_error_metric)
    T       ← ΔT · T

return T

The correspondence step treats the pose as fixed. The fitting step treats the correspondences as fixed. Alternating makes a difficult joint problem manageable, but it also explains ICP’s sensitivity: a poor pose creates poor pairs, and poor pairs create a poor pose update.

Interactive ICP iteration

Watch correspondences and the pose update each other

target source
Iteration0
Correspondence RMSE—
Accepted pairs—
erroriteration

Closest points are paired using the current pose.

Change the starting pose, then step through the loop. ICP alternates between estimating correspondences and estimating the rigid motion—not both at once.

The original method by Besl and McKay established the classic closest-point loop and showed monotonic convergence of its mean-square objective to a local minimum. “Monotonic” means the optimized error does not increase under the method’s assumptions. It does not mean that the minimum is the physically correct alignment.

1.4 How Closest-Point Correspondences Are Built

Let the source cloud be P={pi}i=1NP=\{\mathbf p_i\}_{i=1}^{N} and the target cloud be Q={qj}j=1MQ=\{\mathbf q_j\}_{j=1}^{M}. For each transformed source point, basic ICP chooses the nearest target point:

c(i)=arg⁡min⁡j∈{1,…,M}  ∥Rpi+t−qj∥2.c(i)=\underset{j\in\{1,\dots,M\}}{\arg\min}\; \left\|\mathbf R\mathbf p_i+\mathbf t-\mathbf q_j\right\|_2.

NN and MM are the numbers of source and target points; c(i)c(i) is the target index assigned to source point ii; and ∥⋅∥2\|\cdot\|_2 is Euclidean distance. A spatial index such as a k-d tree avoids comparing every source point with every target point.

Real clouds only overlap partially, so the nearest point is not automatically a valid match. A maximum correspondence distance rejects pair ii when

∥Rpi+t−qc(i)∥2>dmax,\left\|\mathbf R\mathbf p_i+\mathbf t-\mathbf q_{c(i)}\right\|_2>d_{max},

where dmaxd_{max} is the distance gate in the point cloud’s own units. A large gate keeps more overlap but also accepts unrelated geometry. A small gate rejects outliers but may leave too few pairs when the initial pose is rough. Coarse-to-fine registration often begins with a wider gate and reduces it for final refinement.

Point density matters too. If one scan samples a flat surface much more densely, it can contribute many nearly identical pairs and dominate the least-squares fit. Voxel downsampling helps equalize this influence while also reducing the cost of neighbor searches.

1.5 Point-to-Point ICP Fits Paired Samples

Point-to-point ICP minimizes the Euclidean displacement between every accepted source point and its matched target point:

Ep2p(R,t)=∑i∈Cwi∥Rpi+t−qc(i)∥22.E_{p2p}(\mathbf R,\mathbf t)= \sum_{i\in\mathcal C} w_i\left\|\mathbf R\mathbf p_i+\mathbf t-\mathbf q_{c(i)}\right\|_2^2.

Ep2pE_{p2p} is the total squared registration error; C\mathcal C is the set of accepted source indices; wiw_i is an optional correspondence weight; and the other symbols retain their definitions above. With wi=1w_i=1, the rigid update has a closed-form solution: subtract both point-set centroids, use the cross-covariance of paired points to recover the optimal rotation with singular value decomposition, then align the centroids with translation.

This objective is easy to understand and requires no surface normals. It works well for structured clouds, sharp geometry, or similar sampling. Its weakness is that the target is a surface, but the objective sees only discrete samples of that surface. Two scans can describe the same smooth plane while their sample points do not land at identical locations. Point-to-point ICP spends effort pulling one sample toward another even when both already lie on the same physical surface.

1.6 Point-to-Plane ICP Fits the Local Surface

Point-to-plane ICP changes what counts as error. At target point qc(i)\mathbf q_{c(i)}, let nc(i)\mathbf n_{c(i)} be a unit surface normal. Instead of measuring the full vector between paired points, it projects that vector onto the normal:

ri=nc(i)T(Rpi+t−qc(i)),r_i= \mathbf n_{c(i)}^{\mathsf T} \left(\mathbf R\mathbf p_i+\mathbf t-\mathbf q_{c(i)}\right), Ep2l(R,t)=∑i∈Cwiri2.E_{p2l}(\mathbf R,\mathbf t)= \sum_{i\in\mathcal C}w_i r_i^2.

rir_i is the signed distance from transformed source point ii to the target’s local tangent plane; nc(i)\mathbf n_{c(i)} is the plane normal; and Ep2lE_{p2l} is the weighted sum of squared normal distances. Motion along the plane is ignored in the local approximation because it does not move the source away from the underlying surface.

Interactive residual geometry

Move one source point along and away from a surface

local tangent plane at qᵢpoint-to-pointpoint-to-plane
Point-to-point residual—
Point-to-plane residual—

What to notice: sliding parallel to the local plane increases Euclidean distance, but not the signed normal distance. Point-to-plane ICP therefore models the nearby surface rather than insisting on one exact sampled point.

The diagram is two-dimensional for clarity. In 3D, the target normal defines a tangent plane and the same dot product measures signed distance to it.

Chen and Medioni introduced this surface-oriented view for registering range images. It can converge in fewer iterations on smooth surfaces because its local objective better approximates surface-to-surface distance. But it adds a dependency: the target normals must be meaningful.

Normals are usually estimated from a neighborhood around each target point. If qˉ\bar{\mathbf q} is the neighborhood centroid, the local covariance is

C=1K∑j=1K(qj−qˉ)(qj−qˉ)T.\mathbf C=\frac{1}{K}\sum_{j=1}^{K} (\mathbf q_j-\bar{\mathbf q})(\mathbf q_j-\bar{\mathbf q})^{\mathsf T}.

KK is the number of neighboring target points and C\mathbf C is a 3×33\times3 covariance matrix. The eigenvector associated with the smallest eigenvalue points in the direction of least local variation, which is the estimated normal. A tiny neighborhood follows noise; an oversized neighborhood smooths across edges and small features. This is the engineering meaning of normal_search_radius and normal_max_neighbors.

For a small pose update, rotation can be linearized. Let ω∈R3\boldsymbol\omega\in\mathbb R^3 be a small rotation vector and Δt∈R3\Delta\mathbf t\in\mathbb R^3 a small translation. Then

RΔp≈p+ω×p.\mathbf R_{\Delta}\mathbf p\approx \mathbf p+\boldsymbol\omega\times\mathbf p.

The symbol ×\times is the 3D cross product. Substituting this approximation into each point-to-plane residual makes it linear in the six unknown pose increments [ωT,ΔtT]T[\boldsymbol\omega^{\mathsf T},\Delta\mathbf t^{\mathsf T}]^{\mathsf T}. One least-squares solve produces the next incremental transform. Because the approximation assumes a small update, point-to-plane ICP still needs a reasonable starting pose.

1.7 Why Normals and Geometry Can Make the Pose Unobservable

Registration needs geometry that constrains all six rigid degrees of freedom. A single infinite plane constrains translation along its normal and two rotations, but motion within the plane and rotation around its normal leave the same plane unchanged. The optimizer’s normal equations then become poorly conditioned.

This is not a software bug. The measurements simply do not contain enough information. Corners, intersecting planes, curved surfaces, holes, and asymmetric features add constraints. A symmetric cylinder still leaves rotation around its axis ambiguous; a sphere leaves every rotation ambiguous. If the robot must recover that missing degree of freedom, add another view, include color or learned features, use a known fixture, or carry a prior from odometry.

1.8 Outliers, Partial Overlap, and Robust Kernels

Least squares gives large residuals disproportionate influence because the penalty grows as r2r^2. That is undesirable when points come from clutter, occlusion, moving objects, or non-overlapping model regions. A robust objective replaces the square with a loss ρ\rho:

Erobust(T)=∑i∈Cρ ⁣(riσ).E_{robust}(\mathbf T)=\sum_{i\in\mathcal C} \rho\!\left(\frac{r_i}{\sigma}\right).

ρ\rho is a robust loss such as Tukey’s biweight; rir_i is a point-to-point distance or signed point-to-plane residual; and σ\sigma is the assumed noise scale. Small residuals remain influential, while sufficiently large residuals are downweighted or capped.

The noise scale is not a generic “accuracy” control. If σ\sigma is far too large, outliers look ordinary and keep their influence. If it is too small, legitimate correspondences are suppressed before the pose has converged. Robust kernels complement geometric gates; they do not rescue a completely wrong initialization.

1.9 Fast Global Registration Finds the Coarse Pose

ICP searches locally because it pairs points by spatial proximity. FGR starts differently: it describes local geometry around downsampled points, then matches points whose descriptors look alike even when their coordinates are far apart.

The Telekinesis Skill computes surface normals and Fast Point Feature Histograms (FPFH). An FPFH descriptor summarizes angular relationships between a point’s normal and neighboring normals. It is designed to be more stable under rigid motion than raw coordinates, so a corner on the source model can be proposed as a match for a similar corner in the target scan.

Descriptor matches contain outliers. Repeated holes, flat regions, and similar corners can produce plausible but false pairs. FGR’s central contribution is to optimize one robust objective over the candidate matches rather than repeatedly sampling small match sets as RANSAC does:

EFGR(T)=∑(i,j)∈Kρμ ⁣(∥pi−Tqj∥2).E_{FGR}(\mathbf T)= \sum_{(i,j)\in\mathcal K} \rho_{\mu}\!\left(\left\|\mathbf p_i-\mathbf T\mathbf q_j\right\|_2\right).

K\mathcal K is the set of candidate feature correspondences; (i,j)(i,j) identifies one candidate source–target pair; and ρμ\rho_{\mu} is a robust Geman–McClure-style penalty controlled by scale μ\mu. Through an equivalent line process, every pair receives a weight

lij=(μμ+∥pi−Tqj∥22)2.l_{ij}=\left( \frac{\mu}{\mu+\|\mathbf p_i-\mathbf T\mathbf q_j\|_2^2} \right)^2.

lijl_{ij} lies between 0 and 1. A geometrically consistent match keeps a high weight; a large-residual match approaches zero. FGR alternates between updating these weights and updating the pose.

Interactive robust matching

See how FGR keeps useful feature matches and suppresses outliers

source feature pointstarget feature points
Strong matches—
Suppressed matches—
weightresidual

Small μ is selective: only already-consistent matches keep much weight. Graduated non-convexity begins with a smoother objective and lowers μ so bad matches lose influence as the pose improves.

Line opacity represents the line-process weight. Candidate feature matches are fixed during FGR’s inner optimization; their influence changes with residual.

The optimization uses graduated non-convexity. It starts with a large μ\mu, creating a smoother problem in which the optimizer can make broad progress. It then reduces μ\mu, making the loss more selective so inconsistent matches lose influence. The candidate set stays fixed during the inner optimization, so FGR avoids repeated closest-point searches.

FGR is global in the practical sense that it does not require an already-close pose. It does not guarantee the globally optimal answer for every scene. Repeated geometry, weak features, low overlap, or incorrect scale can still defeat it. Its output should be treated as an initialization and validated or refined.

1.10 Choosing the Right Stage

MethodCorrespondence cueError being minimizedNeeds a close initial pose?Best role
Fast Global RegistrationSimilar FPFH descriptorsRobust distance over feature matchesNo rough pose requiredCoarse alignment
Point-to-point ICPSpatial nearest neighborEuclidean point distanceYesSimple local refinement
Point-to-plane ICPSpatial nearest neighbor + target normalSigned tangent-plane distanceYesFast, accurate surface refinement

Use point-to-point ICP when normals are unavailable or unreliable, or when distinct sampled geometry provides good constraints. Use point-to-plane ICP when the target has clean surface normals and smooth overlap. Use FGR when no trustworthy initial transform exists, then refine its result with ICP at full or progressively finer resolution.

1.11 When Registration Fails

The residual alone can be deceptive. A model can align neatly with the wrong repeated feature and still achieve a small error. Build validation around the task:

  • Overlap: require enough accepted correspondences distributed across the object, not concentrated on one small patch.
  • Residual distribution: inspect median and tail errors, not only the mean or RMSE.
  • Pose plausibility: reject translations, rotations, or frame directions that violate the robot cell’s physical limits.
  • Geometric conditioning: detect when flat or symmetric geometry leaves a degree of freedom weakly constrained.
  • Repeatability: perturb the initial pose or resample the cloud; a stable solution should return to nearly the same transform.
  • Task outcome: measure pick, inspection, or localization success after registration. A low point error is not automatically a useful tool pose.

2. How to Use Registration in the Telekinesis Agentic OS

Telekinesis exposes all three stages as Vitreous Skills. The following example shows the intended coarse-to-fine structure. Distances are illustrative: they must be scaled to the units, point spacing, and expected pose error of your clouds.

import numpy as np
from telekinesis import vitreous

# Coarse alignment from local geometric features.
coarsely_aligned = vitreous.register_point_clouds_using_fast_global_registration(
    source_point_cloud=source_point_cloud,
    target_point_cloud=target_point_cloud,
    initial_transformation_matrix=np.eye(4),
    normal_radius=0.02,
    normal_max_neighbors=20,
    feature_radius=0.05,
    feature_max_neighbors=30,
    max_correspondence_distance=0.015,
)

The FGR Skill currently returns a registered point cloud. The ICP Skills return a 4×44\times4 transformation matrix, which you apply separately. If you already have a rough transform from calibration, odometry, a fixture, or another registration stage, point-to-point refinement is direct:

point_to_point_transform = vitreous.register_point_clouds_using_point_to_point_icp(
    source_point_cloud=source_point_cloud,
    target_point_cloud=target_point_cloud,
    initial_transformation_matrix=rough_transform,
    max_iterations=100,
    max_correspondence_distance=0.02,
    use_robust_kernel=True,
    loss_type="tukey_loss",
    noise_standard_deviation=0.005,
)

aligned_source = vitreous.apply_transform_to_point_cloud(
    point_cloud=source_point_cloud,
    transformation_matrix=point_to_point_transform,
)

For a clean target surface with useful normals, switch to point-to-plane refinement:

point_to_plane_transform = vitreous.register_point_clouds_using_point_to_plane_icp(
    source_point_cloud=source_point_cloud,
    target_point_cloud=target_point_cloud,
    initial_transformation_matrix=rough_transform,
    max_iterations=100,
    max_correspondence_distance=0.02,
    normal_search_radius=0.01,
    normal_max_neighbors=30,
    use_robust_kernel=True,
    loss_type="tukey_loss",
    noise_standard_deviation=0.005,
)

Parameter values must share the cloud’s units. In a cloud measured in metres, 0.02 means two centimetres; in millimetres it means two hundredths of a millimetre. Confirm units before tuning anything.

Try it out

Use point-to-point ICP in Vitreous

See the exact Skill interface for local registration using Euclidean correspondence error.

Open point-to-point ICP docs

Try it out

Use point-to-plane ICP in Vitreous

Configure target-normal estimation, robust loss, correspondence distance, and local refinement.

Open point-to-plane ICP docs

Try it out

Start with Fast Global Registration

Build the coarse feature-based alignment that supplies a useful initialization for ICP.

Open FGR docs

Runnable examples

Run the Vitreous registration examples

Open the Telekinesis example launcher containing the point-cloud registration workflows.

View Python examples

2.1 A Practical Tuning Order

  1. Confirm both clouds use the same units and scale.
  2. Remove obvious outliers and isolate the object or overlapping scene region.
  3. Choose a voxel size that preserves task-critical geometry.
  4. Set the normal radius to cover several point spacings without crossing unrelated surfaces.
  5. Set the FPFH feature radius larger than the normal radius so descriptors see enough local shape.
  6. Run FGR when the initial pose is unknown.
  7. Begin ICP with a correspondence gate wide enough for the coarse pose error, then tighten it.
  8. Enable a robust kernel when partial overlap or clutter remains.
  9. Validate the transform with held-out geometry and the downstream robot task.

Do not tune max_iterations first. If correspondences are wrong, more iterations merely optimize the wrong problem for longer. Fix preprocessing, initialization, neighborhood scale, and distance gates before increasing the iteration budget.

3. How to Benchmark a Registration Pipeline

The classic papers answer different questions. Besl and McKay established the behavior of the local closest-point loop over several geometric representations. Chen and Medioni demonstrated surface-based registration of multiple range images. Zhou, Park, and Koltun evaluated FGR as a fast global initializer and reported that it matched or exceeded the accuracy of contemporary global pipelines while running at least an order of magnitude faster in their experiments. Those results explain the algorithms; they do not predict latency or accuracy for a hosted Skill on your point clouds.

For a robot deployment, benchmark the complete pipeline:

MetricDefinitionWhat it reveals
Rotation errorAngle of RgtTRest\mathbf R_{gt}^{\mathsf T}\mathbf R_{est}Orientation accuracy
Translation error∥test−tgt∥2\|\mathbf t_{est}-\mathbf t_{gt}\|_2Position accuracy in physical units
Inlier RMSERMSE over gated correspondencesLocal surface fit
Fitness / overlapFraction of points with valid matchesHow much geometry supports the pose
Recall at toleranceFraction of trials below pose thresholdsReliability across scenes
End-to-end latencyTransfer + preprocessing + registration + validationWhether the pipeline meets the robot cycle time
Task successSuccessful picks, insertions, or inspectionsWhether geometric accuracy is operationally sufficient

Evaluate easy and adversarial cases separately: high and low overlap, clean and noisy depth, symmetric objects, clutter, occlusion, different initial rotations, and target-absent scenes. Report failures rather than averaging them away. Registration is most dangerous when it produces a plausible transform for an invalid match.

4. Where to Go Next

5. References