Iterative Closest Point
Table of Contents:
 The 3D Registration Problem
 Optimal Transformation for Point Clouds: Derivation
 PointCloud Registration with Scale Estimation
 Toy Example and Implementation of Optimization over Sim(3)
The 3D Registration Problem
The 3D Registration Problem is defined as follows: given two shapes \(A\) and \(B\) which partially overlap, use only rigid transforms to register \(B\) against \(A\) by minimizing a measure of distance \(\delta\) between A and B. We assume \(A\) and \(B\) are positioned close to each other. The optimization problem can be phrased as:
\[\min\limits_T \delta(A, T(B))\]Degrees of Freedom: Transform estimation
A rigid motion has 6 degrees of freedom (3 for translation and 3 for rotation). We typically estimate the motion using many more pairs of corresponding points, so the problem is overdetermined (which is good, given noise, outliers, etc – use least squares approaches).
Key Challenges of the Hard Optimization Problem
 We must estimate correspondences. This gives rise to combinatorial searches.
 We must estimate the aligning transform. Transforms may be nonlinear.
Fortunately, the lowdimensionality of some transforms helps.
Optimal Transformation for Point Clouds: Derivation
When given correspondences, the problem is formulated as:
Given two sets points: \(\{a_i \}_{i=1}^n, \{ b_i \}_{i=1}^n\) in . Find the rigid transform \(\mathbf{R} \in SO(3), t \in R^3\) that minimizes
\[\underset{\mathbf{R}, t}{\mbox{minimize }} \sum\limits_{i=1}^N \ \mathbf{R}x_i + t  y_i \_2^2\] \[\sum\limits_{i=1}^n \Ra_i − t − b_i \_2^2\]We can define the centroid of A as:
\[\bar{a} = \frac{1}{A} \sum\limits_i a_i\]Similarly for B,
\[\bar{b} = \frac{1}{B} \sum\limits_i b_i\]Defining the centroids as above will help simplify the minimization problem. Let us now define the following:
\[a_i^{\prime} = a_i − \bar{a}\] \[b_i^{\prime} = b_i − \bar{b}\]We can rewrite our original points as:
\[a_i = a_i^{\prime} + \bar{a}, \hspace{10mm} b_i = b_i^{\prime} + \bar{b}\]Plugging these back into our original objective function:
\[= \sum\limits_{i=1}^n \R(a_i^{\prime} + \bar{a}) − t − (b_i^{\prime} + \bar{b})\_2^2\] \[= \sum\limits_{i=1}^n \Ra_i^{\prime} − b_i^{\prime} + (R\bar{a} − \bar{b} − t)\_2^2\]Let
\[t = R\bar{a} − \bar{b}\]We’ll see that our original optimization problem over 2 variables (\(\mathbf{R},t\)) on \(\Big\{(a_i,b_i)\Big\}_{i=1}^n\) becomes an optimization problem over just a single variable \(\mathbf{R}\) on \(\Big\{(a_i^\prime,b_i^\prime)\Big\}_{i=1}^n\), which is far more tractable and easier to deal with in closedform.
\[\sum\limits_{i=1}^n \Ra_i − t − b_i \_2^2 = \sum\limits_{i=1}^n \Ra_i^{\prime} − b_i^{\prime}\_2^2\] \[tr(RN)\]from typing import Tuple
import numpy as np
def find_rigid_alignment(A: np.ndarray, B: np.ndarray) > Tuple[np.ndarray,np.ndarray]:
"""
2D or 3D registration with known correspondences.
Registration occurs in the zero centered coordinate system, and then
must be transported back.
Args:
A: Array of shape (N,D)  Reference Point Cloud (target)
B: Array of shape (N,D)  Point Cloud to Align (source)
Returns:
R: optimal rotation (3,3)
t: optimal translation (3,)
"""
num_pts = A.shape[0]
dim = A.shape[1]
a_mean = np.mean(A, axis=0)
b_mean = np.mean(B, axis=0)
# Zerocenter the point clouds
A = a_mean
B = b_mean
N = np.zeros((dim, dim))
for i in range(num_pts):
N += A[i].reshape(dim,1) @ B[i].reshape(1,dim)
N = A.T @ B
U, D, V_T = np.linalg.svd(N)
S = np.eye(dim)
det = np.linalg.det(U) * np.linalg.det(V_T.T)
# Check for reflection case
if not np.isclose(det,1.):
S[dim1,dim1] = 1
R = U @ S @ V_T
t = R @ b_mean.reshape(dim,1)  a_mean.reshape(dim,1)
return R, t.squeeze()
Similar code named the Umeyama Transform after[4] ships with Eigen as Umeyama. The Open3D library utilizes the Umeyama method also (source code here).
Roland Siegwart’s group at ETH Zurich has an efficient opensource C++ ICP implementation named libpointmatcher.
Local Methods
Iterated Closest Pair (ICP) [3]

Align the \(A\) points to their closest \(B\) neighbors, then repeat.

Converges, if starting positions are “close enough”.
Variants
Below we discuss two of many ICP variants: ExhaustiveSearch ICP and Generalized ICP. A discussion of more variants can be found in [7].
Exhaustive Search
As mentioned above, ICP relies upon a strong assumption: the scans (point clouds) \(A\) and \(B\) are positioned close to each other, i.e. we have a good initial alignment estimate. When it is clear that this assumption does not hold, one solution is to use more computation and to sample the space of possible initial alignments. Here is a short summary of several strengths and weaknesses of the Exhaustive Search method:
 Compare (ideally) all alignments
 Correspondence is determined by the alignment at which models are closest.
 Provides optimal result
 Can be unnecessarily slow
 Does not generalize well to nonrigid deformations
Chen et al. describe more details of a RANSACbased DARCES (dataaligned rigidityconstrained exhaustive search) method in [6]. Their goal is to check all possible data alignments of two given 3D data sets in an efficient way. They employ RANSAC to ensure that the model fitting is not influenced my outliers (robust estimation).
Generalized ICP
Segal et al. [5] introduce a method called Generalized ICP …
PointCloud Registration with Scale Estimation
At times, it may be desirable to align not just two point clouds, but instead to align two pose graphs, perhaps when evaluating an SFM result with a ground truth model. However, there may be a gauge ambiguity (unknown scale) between the two models. Instead of finding the best SE(3) transformation, we’ll need to compute the best Sim(3) transformation, i.e. we must also estimate a scale factor \(s\) (see here) [8].
\[\underset{\mathbf{R}, t, s}{\mbox{minimize }} \sum\limits_{i=1}^N \ b_i  s\mathbf{R}a_i  t \_2^2\]Perhaps more accurately, we can write the “to” and “from” indices on our rotations and translations:
\[\underset{\mathbf{R}, t, s}{\mbox{minimize }} \sum\limits_{i=1}^N \ b_i  s({}^b\mathbf{R}_aa_i)  {}^bt_a \_2^2\]In this scenario, we can estimate the relative rotation \({}^b \mathbf{R}_a\) first, as the Karcher mean between noisy \({}^b \mathbf{R}_a\) estimates from each posepair.
Now, assuming that \({}^b \mathbf{R}_a\) has been computed, then the scale factor can become the only unknown in the minimization problem above via a special trick below. We are now working with the optimization problem below:
\[\begin{aligned} \underset{t, s}{\mbox{minimize }} & \sum\limits_{i=1}^N \ b_i  s({}^b\mathbf{R}_aa_i)  {}^bt_a \_2^2 \\ \end{aligned}\]Suppose we compute centroids \(\bar{a}\) and \(\bar{b}\) as we did before. Suppose we set \(t = \bar{b}  s R \bar{a}\). Then
\[\begin{aligned} \underset{t, s}{\mbox{minimize }} & \sum\limits_{i=1}^N \ b_i  s(R^\star a_i)  (\bar{b}  s R^\star \bar{a}) \_2^2 \\ & \sum\limits_{i=1}^N \ b_i  s(R^\star a_i)  \bar{b} + s R^\star \bar{a} \_2^2 \\ & \sum\limits_{i=1}^N \ b_i  \bar{b}  s(R^\star a_i) + s R^\star \bar{a} \_2^2 \\ & \sum\limits_{i=1}^N \ b_i  \bar{b}  s(R^\star a_i)  s R^\star (\bar{a}) \_2^2 \\ & \sum\limits_{i=1}^N \ (b_i  \bar{b})  sR^\star (a_i  \bar{a}) \_2^2 \\ \end{aligned}\]We’ll define new variables Using \(\tilde{a_i} = R^\star(a_i  \bar{a})\) and \(\tilde{b_i} = b_i  \bar{b}\) as we used earlier, then we get a optimization problem over a single unknown:
\[\begin{aligned} \underset{s}{\mbox{minimize }} & \sum\limits_{i=1}^N \ \tilde{b_i}  s\tilde{a_i}\_2^2 \\ & \sum\limits_{i=1}^N \Big(\tilde{b_i}  s \tilde{a_i} \Big)^T \Big(\tilde{b_i}  s\tilde{a_i} \Big) \\ & \sum\limits_{i=1}^N \Big( \tilde{b_i}^T  s \tilde{a_i}^T \Big) \Big(\tilde{b_i}  s\tilde{a_i} \Big) \\ & \sum\limits_{i=1}^N \tilde{b_i}^T\tilde{b_i}  \sum\limits_{i=1}^N 2 s \tilde{a_i}^T \tilde{b_i} + \sum\limits_{i=1}^N s^2 \tilde{a_i}^T \tilde{a_i} \\ \end{aligned}\]Now, we can set the gradient equal to zero, and optimize:
\[\begin{aligned} 0 &= \nabla_s \Bigg( \sum\limits_{i=1}^N \tilde{b_i}^T\tilde{b_i}  \sum\limits_{i=1}^N 2 s \tilde{a_i}^T \tilde{b_i} + \sum\limits_{i=1}^N s^2 \tilde{a_i}^T \tilde{a_i} \Bigg) \\ 0 &= \nabla_s \Bigg(\sum\limits_{i=1}^N 2 s \tilde{a_i}^T \tilde{b_i}\Bigg) + \nabla_s \Bigg( \sum\limits_{i=1}^N s^2 \tilde{a_i}^T \tilde{a_i} \Bigg) \\ 0 &= \nabla_s \Bigg(s\sum\limits_{i=1}^N 2 \tilde{a_i}^T \tilde{b_i}\Bigg) + \nabla_s \Bigg( s^2 \sum\limits_{i=1}^N \tilde{a_i}^T \tilde{a_i} \Bigg) \\ 0 &= \sum\limits_{i=1}^N 2 \tilde{a_i}^T \tilde{b_i}  2s \sum\limits_{i=1}^N \tilde{a_i}^T \tilde{a_i} \\ 2s \sum\limits_{i=1}^N \tilde{a_i}^T \tilde{a_i} &= \sum\limits_{i=1}^N 2 \tilde{a_i}^T \tilde{b_i} \\ s^\star = \frac{\sum\limits_{i=1}^N 2 \tilde{a_i}^T \tilde{b_i}}{2\sum\limits_{i=1}^N \tilde{a_i}^T \tilde{a_i}} \end{aligned}\]Toy Example and Implementation of Optimization over Sim(3)
We’ll now try out this method on a realworld example. Suppose we reconstructed a scene with 3 poses along a line. The ground truth also has 3 poses along a line, but at a different scale (2x), and shifted/rotated.
import numpy as np
from gtsam import Rot3, Pose3
# 90 degrees yaw
Rz90 = Rot3.Rz(np.deg2rad(90))
# Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
# Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
eTo0 = Pose3(Rot3(), np.array([1, 0, 0]))
eTo1 = Pose3(Rot3(), np.array([2, 0, 0]))
eTo2 = Pose3(Rot3(), np.array([4, 0, 0]))
eToi_list = [eTo0, eTo1, eTo2]
# Create destination poses
# (same three objects, but instead living in the world/city "w" frame)
wTo0 = Pose3(Rz90, np.array([0, 12, 0]))
wTo1 = Pose3(Rz90, np.array([0, 14, 0]))
wTo2 = Pose3(Rz90, np.array([0, 18, 0]))
wToi_list = [wTo0, wTo1, wTo2]
In GTSAM, the scaleintegrated registration algorithm described above is implemented as Align()
:
we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity3.Align(we_pairs)
for wToi, eToi in zip(wToi_list, eToi_list):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
We can write a simple implementation ourselves, in Numpy. We’ll centralize the points:
A = np.array([ wToi.translation() for wToi in wToi_list ])
B = np.array([ eToi.translation() for eToi in eToi_list ])
a_centroid = A.mean(axis=0)
b_centroid = B.mean(axis=0)
# a_i's are the rows
A = a_centroid
B = b_centroid
Suppose we compute \(R\) as the Karcher mean of aRb
terms. We’ll now estimate the scale s
:
y = 0
x = 0
for i in range(len(wToi_list)):
da = A[i] ## centralized point
db = B[i]
da_prime = wSe.rotation().matrix() @ db
y += da.T @ da_prime
x += da_prime.T @ da_prime
s = y / x
aTb = (a_centroid  s * ( wSe.rotation().matrix() @ b_centroid)) / s
Note that to construct the Sim(3) object, we divide t
by s
. This is required because the registration cost function minimizes \(a  (sRb + t)\), whereas Sim(3) computes \(s(Rb + t)\), i.e. t
is multiplied by the scale again, so we must divide it here to offset this.
References
[1] Leonidas Guibas. Alignments and Correspondences, Geometric Features. CS 233: Geometric and Topological Data Analysis, 9 May 2018.
[2] Shinji Umeyama. Leastsquares estimation of transformation parameters between two point patterns”, PAMI 1991. PDF.
[3] Paul Besl and Neil McKay. A Method for Registration of 3D Shapes. TPAMI, 1992. PDF
[4] Jeff Phillips. Lecture 24: Iterative Closest Point and Earth Mover’s Distance. CPS296.2 Geometric Optimization. 10 April 2007. PDF.
[5] A. Segal, D. Haehnel, and S. Thrun. GeneralizedICP. In Robotics: Science and Systems (RSS), volume 2, page 435, 2009. PDF.
[6] Chen, C., Hung, Y., and Cheng, J. “RANSACBased DARCES: A New Approach to Fast Automatic Registration of Partially Overlapping Range Images,” Trans. PAMI, Vol. 21, No. 11, 1999.
[7]. Szymon Rusinkiewicz and Marc Levoy. Efficient Variants of the ICP Algorithm. PDF.
[8]. Timo Zinsser, Jochen Schimdt, Heinrich Niemann. Point Set Registration with Integrated Scale Estimation. International Conference on Pattern Recognition and Image Processing (PRIP 2005), pages 116119. PDF.