PaperlayerView exact revision on arXiv

Source-linked AI summary

Fast and Robust Iterative Closest Point

Juyong Zhang, Yuxin Yao, Bailin Deng

arXiv:2007.07627v3cs.CVcs.GRcs.RO

TL;DR

ICP is widely used for rigid point-set registration but converges slowly and is sensitive to noise, outliers, missing data, and partial overlap. The paper recasts ICP as an MM method, accelerates it with Anderson acceleration, and introduces Welsch-based robust point-to-point and point-to-plane formulations. The resulting methods achieve similar or better accuracy than Sparse ICP while being significantly faster.

  • Problem

    ICP’s slow convergence and sensitivity to noise, outliers, missing data, and partial overlaps limit robust rigid registration.

  • Method

    The paper uses an MM interpretation of ICP with Anderson acceleration, Lie algebra parameterization, and Welsch-based robust metrics for point-to-point and point-to-plane registration.

  • Results

    The robust ICP schemes achieve similar or better registration accuracy than Sparse ICP while being significantly faster.

  • Takeaways & Limitations

    The methods provide efficient and robust solutions for rigid registration when data contain imperfections such as noise, outliers, or partial overlaps.

  • Takeaways & Limitations

    Like other ICP-based methods, the robust methods rely on good initial alignment and may perform poorly when initialization deviates substantially from ground truth.

Abstract

from arXiv · show

The Iterative Closest Point (ICP) algorithm and its variants are a fundamental technique for rigid registration between two point sets, with wide applications in different areas from robotics to 3D reconstruction. The main drawbacks for ICP are its slow convergence as well as its sensitivity to outliers, missing data, and partial overlaps. Recent work such as Sparse ICP achieves robustness via sparsity optimization at the cost of computational speed. In this paper, we propose a new method for robust registration with fast convergence. First, we show that the classical point-to-point ICP can be treated as a majorization-minimization (MM) algorithm, and propose an Anderson acceleration approach to speed up its convergence. In addition, we introduce a robust error metric based on the Welsch's function, which is minimized efficiently using the MM algorithm with Anderson acceleration. On challenging datasets with noises and partial overlaps, we achieve similar or better accuracy than Sparse ICP while being at least an order of magnitude faster. Finally, we extend the robust formulation to point-to-plane ICP, and solve the resulting problem using a similar Anderson-accelerated MM strategy. Our robust ICP methods improve the registration accuracy on benchmark datasets while being competitive in computational time.

1 INTRODUCTION

The paper addresses ICP’s slow convergence and sensitivity to noisy, outlier-contaminated, and partially overlapping point sets using accelerated MM-based robust formulations. Its methods achieve similar or better accuracy than Sparse ICP while being faster, and extend to point-to-plane registration.

  • Classical ICP has linear convergence and can lose alignment accuracy with noise, outliers, and partial overlaps.
  • ICP is treated as an MM algorithm, enabling Anderson acceleration of its fixed-point iterations.The surrogate construction provides monotonic target-energy decrease.
  • The point-to-point method uses Lie algebra transformation parameterization and a stabilization strategy that guarantees monotonic target-energy decrease.
  • The Welsch-based robust point-to-point metric is less sensitive to outliers and partial overlaps and is solved with MM and Anderson acceleration.
  • The robust methods achieve similar or better registration accuracy than Sparse ICP while being significantly faster.
  • The robust formulation extends to point-to-plane ICP without requiring point-pair rejection.

2 RELATED WORK

Related work improves registration through better initialization, faster alignment updates, robust objectives, global optimization, learning, and accelerated iterative solvers. These approaches trade accuracy, robustness, convergence behavior, or computational cost in different ways.

  • ICP alternates closest-point correspondence updates with alignment updates and depends on both good initialization and robust alignment updates.
  • Feature descriptors, point feature histograms, and congruent co-planar point sets have been used to determine coarse initial alignments.A later congruent-set approach improves efficiency to linear time complexity.
  • ICP alignment updates minimize point-to-point or point-to-plane distances, while local quadratic approximants and symmetrized objectives target faster convergence.
  • Global optimization and truncated least-squares methods address registration quality and outlier insensitivity, while deep learning has also been applied to registration.Global optimization incurs higher computational costs.
  • Anderson acceleration has been applied to fixed-point, local-global, and ADMM solvers, including prior acceleration of ICP.

3 CLASSICAL ICP REVISITED

Classical point-to-point ICP alternates closest-point correspondence and rigid alignment updates. Its alignment step is an MM surrogate minimization, so ICP monotonically decreases the target function toward a local minimum.

  • Rigid registration optimizes a rotation matrix and translation vector to align source points P with target points Q.
  • The ICP alignment objective is a surrogate for the original registration objective constructed from the current transformation.
  • The rigid-transformation constraint requires R to remain a rotation matrix in the special orthogonal group SO(d).
  • ICP alternates finding each source point’s closest target correspondence and minimizing the ℓ2 distance between corresponding points.The alignment step has a closed-form SVD solution.
  • MM constructs an upper-bounding surrogate that is tight at the current iterate, then minimizes it to obtain the next iterate.
  • ICP is therefore an MM algorithm that decreases its target function monotonically until convergence to a local minimum.

4 FAST AND ROBUST ICP

The paper accelerates ICP by viewing it as an MM fixed-point iteration and applying stabilized Anderson acceleration, while improving robustness through a Welsch-based metric. Lie-algebra parameterization avoids Euler-angle singularities, and a decreasing robustness parameter progressively excludes outliers.

  • Anderson acceleration: Classical ICP is interpreted as an MM algorithm and fixed-point iteration, enabling Anderson acceleration to improve its linear convergence.The MM formulation monotonically decreases the target energy toward a local minimum.
  • Anderson acceleration: Anderson acceleration combines the current iterate with a history of m previous iterates to compute an accelerated fixed-point update.The method uses residual information from recent iterates through a linear least-squares acceleration step.
  • Transformation representation: Lie-algebra parameterization makes affine Anderson updates valid rigid transformations and avoids the singularities of Euler-angle gimbal lock.The transformation logarithms lie in the vector space se(d), where affine combinations remain suitable for acceleration before mapping back to SE(d).
  • Stabilization: A stabilization safeguard accepts an accelerated iterate only when it decreases the target function, preserving monotonic energy decrease despite Anderson instability.The paper uses a simpler stabilization strategy than AA-ICP while retaining monotonicity.
  • Experimental comparison: The Lie-algebra formulation converges faster than AA-ICP and an Euler-angle alternative, with Euler-angle iterates marked near gimbal lock.Figure 2 compares target energy and RMSE across transformation representations and stabilization strategies.
  • Robust ICP: The Welsch metric is bounded, reducing sensitivity to outliers and partial overlaps while still penalizing point-set deviations.Its MM solver uses Anderson acceleration, and distant points receive lower Gaussian weights, becoming effectively excluded beyond approximately 3ν.
  • Robust ICP: The robust solver is guaranteed to converge under the paper’s MM formulation, unlike non-convex ℓp optimization methods whose convergence requires strong assumptions.The comparison specifically contrasts the MM solver with IRLS and ADMM approaches used for sparse ICP.
  • Robust ICP: Gradually decreasing ν removes outliers while retaining more correspondences initially, producing lower RMSE than fixing ν at νmax or νmin.A fixed small ν can exclude most point pairs, whereas a fixed large ν retains outliers.

5 EXTENSION TO POINT-TO-PLANE ICP

The paper extends robust ICP to point-to-plane alignment by combining Welsch’s function with an Anderson-accelerated MM strategy. The method uses se(3) parameterization, linearization, line search, and acceptance checks to maintain valid, decreasing iterates.

  • Point-to-plane ICP: Point-to-plane ICP minimizes distances from transformed source points to tangent planes at corresponding target points, enabling faster convergence through local surface linearization.It retains sensitivity to outliers and partial overlaps, motivating the robust extension.
  • Robust formulation: Welsch’s function replaces the point-to-plane target metric, and MM constructs a surrogate problem for the robust alignment update.The solver alternates correspondence estimation with an assignment step and surrogate minimization.
  • Optimization: The robust update is expressed in six-dimensional se(3) variables and solved by first-order linearization, reducing the proxy problem to a linear system.The variables represent three parameters for each of the rotation-related and translation-related submatrices.
  • Optimization: Because linearization may increase the target function, line search selects a transformation that decreases it, using the lowest-energy step if necessary.The search proceeds along the direction from the current to the candidate se(3) variable.
  • Acceleration: Anderson acceleration combines recent iterates and mapped updates in se(3), accepting the accelerated value only when it decreases the robust target function.At a local minimum, the corresponding se(3) variable is a fixed point of the point-to-plane mapping.

6 RESULTS

Experiments on synthetic and real-world partially overlapping point clouds show that the robust ICP methods generally match or exceed Sparse ICP accuracy at lower computational cost, while remaining sensitive to initialization and extremely small overlap.

  • Evaluation setup: The evaluation compares point-to-point and point-to-plane ICP variants, including AA-ICP, Sparse ICP, and symmetric ICP, across synthetic and real-world registration datasets.The experiments report RMSE and computational time, including averages and medians across multiple datasets and models.
  • Synthetic data: On the small-overlap monkey example, both robust methods achieve the lowest RMSE while being significantly faster than point-to-point ICP.Fast ICP also matches classical ICP and AA-ICP accuracy with less computational time.
  • Synthetic data: For up to 20% added outliers, robust point-to-point and point-to-plane methods outperform their respective ICP variants in accuracy, with robust point-to-plane ICP best overall.At 50% outliers, all methods produce similarly large RMSE values; better initialization allows the proposed methods to recover reasonable results.
  • Synthetic data: Across 25 partially overlapping point-cloud pairs, the robust methods and Sparse ICP are more accurate overall, while the proposed methods are significantly faster than Sparse ICP.The proposed methods achieve the best average or median RMSE in more instances.
  • Real-world data: On RGB-D SLAM and ETH laser datasets, robust methods, Sparse ICP, and symmetric ICP account for most lowest average or median RMSE values while the proposed methods use lower computational cost.On 3DMatch, ICP-based methods perform better above 40% overlap, whereas DGR achieves the best accuracy at lower overlap ratios.
  • Limitations: The robust methods rely on good initial alignment: controlled tests recover low RMSE when rotations are not too large, but very small overlap can still cause incorrect registration.Robust point-to-plane ICP reaches RMSE at the scale of 1×10^-4 up to 30°; robust point-to-point ICP reaches 1×10^-3 up to 50°.

7 CONCLUSION AND FUTURE WORK

The paper develops accelerated and robust ICP methods for noisy, outlier-contaminated, and partially overlapping point clouds. The authors report accuracy comparable to or better than Sparse ICP with substantially faster computation, while identifying several directions for improvement.

  • The robust ICP schemes achieve similar or better accuracy than Sparse ICP while being significantly faster.
  • The methods target rigid registration with noisy data, outliers, and partial overlap.
  • Future work: Future work includes machine learning for coarse initialization, improved control of ν on very small overlaps, and extension to symmetric ICP.

APPENDIX A COMPUTING MATRIX LOGARITHMS

This appendix derives a numerical procedure for computing logarithms of transformation matrices in SE(d). It uses real Schur decomposition and evaluates the logarithm through structured diagonal blocks without requiring matrices to have no negative eigenvalues.

  • The procedure avoids the no-negative-eigenvalue restriction required by existing inverse scaling and squaring methods.
  • The decomposition rearranges the Schur factors into a structured form whose diagonal blocks are 1-by-1 or 2-by-2 matrices.
  • Each 2-by-2 diagonal block is treated as a rotation matrix parameterized by an angle θ ∈ [0, π].
  • The algorithm computes log(U′) from the structured blocks and then obtains log(T).
  • The method computes log(T) for transformation matrices T ∈ SE(d) using a real Schur decomposition.

APPENDIX B SURROGATE FUNCTION FOR EQ. (13)

The appendix establishes that the function in Eq. (15) is a surrogate for the target function in Eq. (13) at the current transformation. Monotonicity of χν supports this surrogate relationship.

  • Eq. (15) is shown to be a surrogate function for Eq. (13) at the current transformation R(k), t(k).
  • The derivation substitutes each ψν(Di) term in Eq. (13) with χν and uses the latter’s monotonic increase on [0, +∞).

APPENDIX C CHOICES OF νmin

This appendix explains heuristic choices for νmin so point pairs affected by sampling differences remain available to the alignment step. The choices adapt to point spacing and tangent-plane distances under non-uniform sampling.

  • Point-to-point method: νmin is chosen large enough to include point pairs whose deviations arise from differences in sampling locations.
  • Point-to-point method: For point-to-point ICP, the heuristic sets νmin from the representative spacing EQ between neighboring target points.
  • Point-to-plane method: For point-to-plane ICP, νmin is selected so points in the overlapping region remain included in alignment.
  • Adaptive heuristics: With non-uniform sampling, the method estimates representative source and target spacings from nearest-neighbor distances.
  • Adaptive heuristics: The tangent-plane construction bounds the relevant distance using Hq(p) and Hq(s) when selecting νmin.

APPENDIX D CALCULATION OF GRADIENT J(k) IN EQ. (24)

This appendix derives the gradient J(k) in Eq. (24) by differentiating the objective with respect to the transformation variables and their rotation–translation parameterization.

  • Gradient variables: The gradient J(k) is calculated at T(k) using the actual variables eT = [δT, uT]T.The variables δ and u parameterize rotation and translation components of the transformation.
  • Gradient computation: The gradient of B(k) is represented using element-wise matrix multiplication and summation over matrix elements.The appendix defines ◦ as element-wise multiplication and (·)sum as summation of matrix elements.
  • Transformation derivatives: Derivatives with respect to δ and u are obtained from the rotation matrix R and the se(3) translation representation.The rotation derivative uses the j-th standard basis vector and identity matrix, while translation derivatives provide the corresponding columns of D.

APPENDIX E SETTINGS OF EXPERIMENTS

The experiments follow open-source default settings with specified termination criteria, sparsity parameters, downsampling rules, and pretrained-model choices for competing methods.

  • Termination criteria: ICP, ICP-l, and AA-ICP terminate after 1000 iterations or when ∥∆T∥2F < 10−5.∆T is the transformation difference between consecutive iterations.
  • Sparse ICP settings: Sparse ICP uses p = 0.8 on the RGB-D SLAM dataset and p = 0.4 elsewhere, while Sparse ICP-l uses p = 0.4 throughout.These parameter choices are specified separately for the two sparse ICP variants.
  • Point-cloud preprocessing: CPD and GMM-Reg downsample large point clouds to 15000 and 5000 points, respectively, using farthest point sampling.The thresholds follow the computational-cost and implementation recommendations described for each method.
  • Point-cloud preprocessing: Teaser++ downsamples clouds above 5000 points and equalizes source and target sizes when necessary because of memory and input-size constraints.The method first samples 5000 points, then reduces the larger cloud to match the other cloud’s size.
  • Learning-based baselines: DCP is trained on 10000 3DMatch training pairs and uses 1024-point clouds, while DGR uses released models trained on 3DMatch and KITTI.DGR applies the KITTI-based model to specified outdoor and mixed ETH datasets and the 3DMatch model elsewhere.
  • Iteration accounting: Iteration counts in Fig. 1 correspond to method-specific update operations, including EM iterations for CPD and corresponding-point updates for other methods.GMM-Reg counts objective constructions and Teaser++ counts rotation-matrix calculations.
Loading 2007.07627v3…