Rigid ICP (Iterative Closest Point)
We recall the notations introduced in our explanation of rigid deformations: a rigid deformation model parameterized by the pair \((R,t)\) transforms a source shape \(A = \{a_i\}\) into a target-aligned shape \(A_{\theta}\), such that:
Here, we describe the classic Rigid ICP algorithm, designed to estimate the optimal rigid transformation parameters \((R,t)\) aligning the source points \(A\) with target points \(B = \{b_j\}\).
Parameters
Rotation matrix \(R \in SO(3)\).
Translation vector \(t \in \mathbb{R}^3\).
Weighted ICP Objective
Let \(\sigma\) represent tentative correspondences, mapping each source point \(a_i\) to a matched target point \(b_{\sigma(i)}\). Each source point \(a_i\) is assigned:
A positive-definite local metric \(L_i \succ 0\), computed from the corresponding point \(b_{\sigma(i)}\) encoding how source points can be transformed.
A scalar weight \(w_i > 0\).
Optionally, we include a Gaussian prior on the parameters via a diagonal precision matrix \(\Lambda = \lambda I\).
Iteratively Reweighted Least Squares (IRLS)
At each IRLS iteration, given the residuals from the previous iteration, we fix scalar weights \(\{w_i\}\) (based on a robust estimator). The weighted least-squares cost we minimize is:
where:
\(\lVert v \rVert_{L_i}^{2} = v^{\top} \,L_i\, v\) is the Mahalanobis norm squared.
\(\lVert \theta \rVert_{\Lambda}^{2}\) represents the prior regularization term.
Robust M-estimator Interpretation
To robustify ICP against outliers, we express our true objective using a robust M-estimator:
where \(\rho: [0, \infty) \to [0, \infty)\) is a robust loss function (e.g. Huber or Tukey).
At each iteration, we linearize \(\rho\) around current residuals \(e_i\), defining influence and weights:
If \(e_i = 0\), one typically sets \(w_i = \psi'(0)\), assuming \(\psi'(0)\) is finite.
We solve the weighted least-squares problem above iteratively until convergence, yielding a local minimizer of \(C_{\mathrm{M}}\).
Choice of \(\rho\) (and corresponding $w_i)
\(L_1\) (absolute value) – zero parameter, works “out of the box”
\[\rho(e) = |e|, \qquad w(e) = \frac{1}{|e| + \varepsilon}\]Variable Trimming (Var-Trim) – hard rejection with adaptive overlap
\[\begin{split}\rho(e;\tau) = \begin{cases} e^2 & \text{if } e \leq \tau,\\ \tau^2 & \text{otherwise}, \end{cases} \qquad w(e;\tau) = \begin{cases} 1 & \text{if } e \leq \tau,\\ 0 & \text{otherwise}. \end{cases}\end{split}\]Here \(\tau\) is recomputed at each iteration so that we keep a fixed proportion (typically 70–90 %) of the smallest residuals.
Cauchy (fixed scale) – single break-point \(k\)
\[\rho(e;k) = \frac{k^2}{2}\,\log\!\bigl(1 + (e/k)^2\bigr), \qquad w(e;k) = \frac{1}{1 + (e/k)^2}\]Cauchy MAD (auto-scaled) – robust, almost parameter-free
At each IRLS outer loop compute the median absolute deviation
\[s = 1.4826 \,\text{MAD}(e),\]then normalise residuals \(\tilde e = e / s\) and reuse the Cauchy form with \(k \approx 1\):
\[\rho(\tilde e) = \frac{k^2}{2}\,\log\!\bigl(1 + (\tilde e/k)^2\bigr), \qquad w(\tilde e) = \frac{1}{1 + (\tilde e/k)^2}\]
Alternating Optimization
Rigid ICP alternates between two steps:
Correspondence step: Identify nearest target points for each source point under the current transformation:
\[\sigma(i) = \mathrm{argmin}_j \;\lVert R\,a_i + t - b_j \rVert_{L_i}^2.\]Compute local metrics \(L_i\) based on the current correspondences \(\sigma(i)\).
Registration step: Solve the weighted Procrustes problem given fixed correspondences:
\[(R, t) = \underset{R \in SO(3),\,t \in \mathbb{R}^3}{\mathrm{argmin}} \sum_i w_i \lVert R\,a_i + t - b_{\sigma(i)} \rVert_{L_i}^2 + \lVert \theta \rVert_{\Lambda}^2.\]Closed-form solution:
- Compute weighted centroids:
- \[\bar{a} = \frac{\sum_i w_i \,a_i}{\sum_i w_i}, \quad \bar{b} = \frac{\sum_i w_i \,b_{\sigma(i)}}{\sum_i w_i}.\]
- Construct weighted cross-covariance:
- \[H \;=\; \sum_i w_i\, (a_i - \bar{a})\, (b_{\sigma(i)} - \bar{b})^{\top}.\]
Perform SVD: \(H = U \,\Sigma\, V^{\top}\), set \(R = V\,U^{\top}\) (correct for \(\det R < 0\)), then \(t = \bar{b} - R\,\bar{a}\).
Iterate until changes in cost \(C\) become negligible or a maximum iteration count is reached.
Initialization of \(R\) and \(t\)
FPFH + RANSAC: Compute local FPFH descriptor, sample minimal sets, and keep the transform that maximises the inlier count. When to use: medium overlap (≈ 40–80 %), unknown rotation, outliers < 90 %.
PCA alignment: Aligns the first (or first two) principal axes of the clouds before refinement. When to use: strongly elongated or planar geometry (pipes, façades, trunks), clouds already fairly close.
Centroid + SVD: Solves Procrustes on a handful of coarse matches, then refines. When to use: high overlap (> 60 %) with some reliable correspondences.
Centroid shift only: Simply translates the source cloud onto the target barycentre. When to use: nearly overlapping captures (multi-frame of the same sensor), initial error < 10 cm and < 10°.
Choice of \(L_i\)
In many 3D-registration problems, the most reliable constraint at each point \(a_i\) is how far it lies from the tangent plane of the underlying surface. When we have normals information, we then use a point-to-plane local metric that is defined as
\[L_i = \alpha_i \, n_{\sigma(i)} \, n_{\sigma(i)}^{\top} + \beta (I - n_{\sigma(i)} n_{\sigma(i)}^{\top}),\]where \(\alpha\;\gg\; \beta > 0\) are scalar weights, and \(n_{\sigma(i)}\) is the normal vector at point \(b_{\sigma(i)}\) corresponding to \(a_i\). With this metric, displacements along \(n_{\sigma(i)}\) are penalized more than displacements in the tangent plane.
In the absence of normal information, a point-to-point (isotropic) metric can be used instead:
\[L_i = I,\]If both source and target have normals of good quality, we can use the plane-to-plane metric as an alternative:
\[L_i + L'_{\sigma(i)}, \quad L_i = \alpha_i\, n_i\, n_i^\top, \quad L'_{\sigma(i)} = \alpha'_{\sigma(i)}\, n'_{\sigma(i)}\, {n'_{\sigma(i)}}^\top.\]Finally, for more refined capture of the local geometry of the shapes, one can use the local covariance at the cost of inverting matrices:
\[L_i = (\Sigma_i + \Sigma’_{\sigma(i)})^{-1}.\]where \(\Sigma_i\) is the local covariance matrix at point \(a_i\), and \(\Sigma’_{\sigma(i)}\) is the local covariance matrix at point \(b_{\sigma(i)}\).