Iterative Closest Point (ICP)

In each iteration, ICP establishes a correspondence between points in the source cloud and the target cloud based on a closest-point criterion in Euclidean space. This is typically accelerated using a k-d tree data structure to avoid brute-force O(N²) search. The assumption is that the nearest neighbor is the correct match, which is only valid when the two clouds are already roughly aligned. This hard assignment of correspondences makes ICP sensitive to outliers and partial overlap, as every source point is forced to find a partner in the target set.

Once correspondences are fixed, ICP computes the optimal rigid transformation (rotation R and translation t) that minimizes the sum of squared distances between matched point pairs. This is solved in closed form using singular value decomposition (SVD) or unit quaternions via the Horn method. The SVD approach decomposes the cross-covariance matrix of the centered point sets, enforcing orthogonality on R to ensure a valid rotation. This step is deterministic and computationally efficient, guaranteeing the global minimum for the current correspondence set.

ICP alternates between correspondence finding and transformation estimation until a termination criterion is met. Convergence is declared when the change in mean squared error (MSE) between iterations falls below a threshold, or a maximum iteration count is reached. Critically, ICP converges monotonically to a local minimum of the cost function, not necessarily the global optimum. The final alignment quality is therefore heavily dependent on the initial pose guess. Poor initialization can trap the algorithm in a suboptimal basin of attraction.

The standard point-to-point error metric minimizes Euclidean distance between matched points. A superior variant, point-to-plane, minimizes the distance from the source point to the tangent plane at the matched target point. This metric assumes the target surface is locally planar and allows the source cloud to slide along flat surfaces, leading to faster convergence and better accuracy on structured environments. The cost function becomes a linear least-squares problem solvable via the normal equations, requiring surface normals for the target cloud.

Real-world point clouds contain noise, occlusions, and non-overlapping regions that violate the nearest-neighbor assumption. Robust ICP variants incorporate outlier rejection strategies:

• Maximum distance threshold: Reject pairs exceeding a user-defined distance.
• Trimmed ICP: Use only a percentage of the best-matching pairs.
• Robust loss functions: Replace squared error with Huber or Tukey biweight losses to down-weight outliers. These mechanisms prevent spurious correspondences from corrupting the transformation estimate.

Beyond rigid registration, Generalized ICP (GICP) unifies point-to-point and point-to-plane into a probabilistic framework by modeling both point clouds as Gaussian distributions. Colored ICP extends the metric to include photometric consistency, minimizing joint geometric and color reprojection errors for RGB-D data. Sparse ICP uses sparsity-inducing norms to handle massive datasets. These variants adapt the core algorithm to specific sensor modalities and noise characteristics while preserving the iterative correspondence-optimization loop.

An alternative to ICP that maps a point cloud into a set of local normal distributions to represent surfaces as piecewise-smooth probability density functions. Instead of finding explicit point correspondences, NDT maximizes the likelihood that points from the source scan lie on the target's statistical surface model. This approach is often more robust to initial misalignment and can converge faster than ICP in large-scale urban environments. Widely used in autonomous vehicle localization for matching LiDAR scans to high-definition maps.

A refinement of standard ICP that minimizes the perpendicular distance from source points to the tangent planes of corresponding target points, rather than point-to-point Euclidean distance. This variant converges significantly faster on smooth surfaces and is less susceptible to sliding along flat geometries. The cost function uses surface normals to constrain alignment, making it the preferred choice for aligning structured environments like building interiors and manufactured parts with well-defined planar features.

The process of determining the rigid-body transformation—a 6-DOF rotation and translation—that defines the spatial relationship between two sensor coordinate frames. ICP is a core algorithm for automatic extrinsic calibration, aligning overlapping point clouds from sensors like LiDAR and cameras without physical measurement tools. Accurate extrinsic calibration is a hard prerequisite for any sensor fusion system; errors here propagate directly into downstream perception and localization tasks.

The computational problem of building a map of an unknown environment while tracking an agent's pose within it. ICP serves as the scan-matching backbone in many LiDAR-based SLAM systems, aligning incoming scans to the growing map to correct odometry drift. In loop closure detection, ICP refines the alignment between a current scan and a previously visited location, enabling globally consistent map construction. Modern SLAM frameworks often combine ICP with factor graph optimization for robust performance.

An alternative to dense ICP that first extracts distinctive geometric primitives—such as corners, edges, or FPFH descriptors—from point clouds and matches them to establish correspondences. This approach dramatically reduces computational load and is more robust to large initial displacements. Common in global registration pipelines, feature matching provides an initial alignment that ICP then refines locally. Techniques like RANSAC are used to reject outlier correspondences before computing the transformation.

A probabilistic framework that unifies standard ICP and point-to-plane ICP by modeling both source and target points as Gaussian distributions. GICP attaches a covariance matrix to each point, representing local surface uncertainty, and minimizes the probabilistic distance between corresponding distributions. This formulation automatically adapts to local surface geometry—behaving like point-to-plane on flat surfaces and point-to-point on corners—yielding superior convergence in heterogeneous environments with mixed geometry.