Introduction to
Mobile Robotics
Iterative Closest Point
Algorithm
Slides adopted from: Wolfram Burgard, Cyrill Stachniss,
Maren Bennewitz, Kai Arras and Probabilistic Robotics Book
1
Motivation
2
The Problem
§ Given: two corresponding point sets:
§ Wanted: translation t and rotation R that
minimizes the sum of the squared error:
Where and are corresponding points.
3
Key Idea
§ If the correct correspondences are known,
the correct relative rotation/translation can
be calculated in closed form.
4
Center of Mass
and
are the centers of mass of the two point sets.
Idea:
§ Subtract the corresponding center of mass
from every point in the two point sets
before calculating the transformation.
§ The resulting point sets are:
and
5
SVD
Let
denote the singular value decomposition (SVD) of W
by:
where are unitary, and
are the singular values of W.
6
SVD
Theorem (without proof):
If rank(W) = 3, the optimal solution of E(R,t) is
unique and is given by:
The minimal value of error function at (R,t) is:
7
Proof
8
ICP with Unknown Data Association
§ If correct correspondences are not known, it
is generally impossible to determine the
optimal relative rotation/translation in one
step
9
ICP-Algorithm
§ Idea: iterate to find alignment
§ Iterated Closest Points (ICP)
[Besl & McKay 92]
§ Converges if starting positions are
close enough
10
Iteration-Example
11
ICP-Variants
§ Variants on the following stages of ICP
have been proposed:
1. Point subsets (from one or both point
sets)
2. Weighting the correspondences
3. Data association
4. Rejecting certain (outlier) point pairs
12
Performance of Variants
§ Various aspects of performance:
§ Speed
§ Stability (local minima)
§ Tolerance wrt. noise and/or outliers
§ Basin of convergence
(maximum initial misalignment)
§ Here: properties of these variants
13
ICP Variants
1. Point subsets (from one or both point
sets)
2. Weighting the correspondences
3. Data association
4. Rejecting certain (outlier) point pairs
14
Selecting Source Points
§ Use all points
§ Uniform sub-sampling
§ Random sampling
§ Feature based Sampling
§ Normal-space sampling
§ Ensure that samples have normals distributed as
uniformly as possible
15
Normal-Space Sampling
uniform sampling normal-space sampling
16
Comparison
§ Normal-space sampling better for mostly-
smooth areas with sparse features
[Rusinkiewicz et al.]
Random sampling Normal-space sampling 17
Feature-Based Sampling
§ try to find important points
§ decrease the number of correspondences
§ higher efficiency and higher accuracy
§ requires preprocessing
3D Scan (~200.000 Points) Extracted Features (~5.000 Points)
18
Application
[Nuechter et al., 04] 19
ICP Variants
1. Point subsets (from one or both point
sets)
2. Weighting the correspondences
3. Data association
4. Rejecting certain (outlier) point pairs
20
Selection vs. Weighting
§ Could achieve same effect with weighting
§ Hard to guarantee that enough samples of
important features except at high sampling
rates
§ Weighting strategies turned out to be
dependent on the data.
§ Preprocessing / run-time cost tradeoff (how
to find the correct weights?)
21
ICP Variants
1. Point subsets (from one or both point
sets)
2. Weighting the correspondences
3. Data association
4. Rejecting certain (outlier) point pairs
22
Data Association
§ has greatest effect on convergence and
speed
§ Closest point
§ Normal shooting
§ Closest compatible point
§ Projection
§ Using kd-trees or oc-trees
23
Closest-Point Matching
§ Find closest point in other the point set
Closest-point matching generally stable,
but slow and requires preprocessing
24
Normal Shooting
§ Project along normal, intersect other point
set
Slightly better than closest point for smooth
structures, worse for noisy or complex
structures 25
Point-to-Plane Error Metric
§ Using point-to-plane distance instead of
point-to-point lets flat regions slide along
each other [Chen & Medioni 91]
26
Projection
§ Finding the closest point is the most
expensive stage of the ICP algorithm
§ Idea: simplified nearest neighbor search
§ For range images, one can project the
points according to the view-point [Blais 95]
27
Projection-Based Matching
§ Slightly worse alignments per iteration
§ Each iteration is one to two orders of
magnitude faster than closest-point
§ Requires point-to-plane error metric
28
Closest Compatible Point
§ Improves the previous two variants by
considering the compatibility of the points
§ Compatibility can be based on normals,
colors, etc.
§ In the limit, degenerates to feature
matching
29
ICP Variants
1. Point subsets (from one or both point
sets)
2. Weighting the correspondences
3. Nearest neighbor search
4. Rejecting certain (outlier) point pairs
30
Rejecting (outlier) point pairs
§ sorting all correspondences with respect to
there error and deleting the worst t%,
Trimmed ICP (TrICP) [Chetverikov et al.
2002]
§ t is to Estimate with respect to the Overlap
Problem: Knowledge about the
overlap is necessary or has to
be estimated
31
ICP-Summary
§ ICP is a powerful algorithm for calculating
the displacement between scans.
§ The major problem is to determine the
correct data associations.
§ Given the correct data associations, the
transformation can be computed efficiently
using SVD.
32
