Iterative Closest Point

Ronen Gvili

The Problem

Align two partiallyoverlapping

meshes
given initial guess
for relative
transform

Data Types

Point sets
Line segment sets (polylines)
Implicit curves : f(x,y,z) = 0
Parametric curves : (x(u),y(u),z(u))
Triangle sets (meshes)
Implicit surfaces : s(x,y,z) = 0
Parametric surfaces
(x(u,v),y(u,v),z(u,v)))

Motivation

Shape inspection
Motion estimation
Appearance analysis
Texture Mapping
Tracking

Motivation

Range images
registration

Motivation

Range images registration

Range Scanners

Aligning 3D Data

Corresponding Point Set

Alignment

Let M be a model point set.

Let S be a scene point set.

We assume :
1.
NM = NS.
2.

Each point Si correspond to Mi .

Corresponding Point Set

Alignment
The MSE objective function :
1
f ( R, T )
NS
1
f (q)
NS

NS

i 1

mi Rot ( si ) Trans

NS

m R(q
i 1

) si qT

The alignment is :
(rot , trans, d mse ) ( M , S )

Aligning 3D Data

If correct correspondences are known,

can find correct relative
rotation/translation

Aligning 3D Data

How to find correspondences: User

input? Feature detection? Signatures?
Alternative: assume closest points
correspond

Aligning 3D Data

How to find correspondences: User

input? Feature detection? Signatures?
Alternative: assume closest points
correspond

Aligning 3D Data

Converges if starting position close

enough

Closest Point

Given 2 points r1 and r2 , the

Euclidean distance is:

d (r1 , r2 ) r1 r2 ( x1 x2 ) 2 ( y1 y2 ) 2 ( z1 z 2 ) 2

Given a point r1 and set of points

A , the Euclidean distance is:

d (r1 , A) min d (r1 , ai )

i1..n

Finding Matches

The scene shape S is aligned to

be in the best alignment with
the model shape M.
The distance of each point s of
the scene from the model is :

d ( s, M ) min d m s
mM

Finding Matches
d ( s, M ) min d m s d ( s, y )
mM

yM
Y C (S , M )
Y M
C the closest point operator
Y the set of closest points to S

Finding Matches

Finding each match is performed in

O(NM) worst case.

Given Y we can calculate alignment

(rot , trans, d ) ( S , Y )

S is updated to be :

S new rot ( S ) trans

The Algorithm
Init the error to
Y = CP(M,S),e
(rot,trans,d)
S`= rot(S)
+trans

d` = d

Calculate
correspondence
Calculate alignment
Apply alignment
Update error
If error > threshold

The Algorithm
function ICP(Scene,Model)
begin
E` + ;
(Rot,Trans) In Initialize-Alignment(Scene,Model);
repeat
E E`;
Aligned-Scene Apply-Alignment(Scene,Rot,Trans);
Pairs Return-Closest-Pairs(Aligned-Scene,Model);
(Rot,Trans,E`) Update-Alignment(Scene,Model,Pairs,Rot,Trans);
Until |E`- E| < Threshold
return (Rot,Trans);
end

Convergence Theorem

The ICP algorithm always

converges monotonically to a local
minimum with respect to the MSE
distance objective function.

Convergence Theorem

Correspondence error :
1
ek
NS

NS

i 1

yik sik

Alignment error:
1
dk
NS

NS

i 1

yik Rot k ( sio ) Trans k

Convergence Theorem
Ek
Dk
S`= rot(S)
+trans

Ek+1
Dk+1

Calculate
correspondence
Calculate alignment
Apply alignment
Calculate
correspondence
Calculate alignment

Convergence Theorem

Proof :
S k Rot k ( S 0 ) Trans k
Yk C ( M , sk )
NS

1
ek
NS

1
dk
NS

NS

i 1

i 1

yik sik

yik Rot k ( sio ) Trans k

Convergence Theorem

Proof : d k ek
If not - the identity transform would
yield a smaller MSE than the least
square alignment.
Apply the alignmentqk on S0 Sk+1 .
Assuming the correspondences are
maintained : the MSE is still dk.
1 NM
2
dk
yik Sik

N M i 1

Convergence Theorem

Proof :
After the last alignment, the closest
point operator is applied : Y C ( M , S )
k 1
k 1
It is clear that:

yi ,k 1 Si ,k 1 yik Si ,k 1
ek 1 d k
Thus :

0 d k 1 ek 1 d k ek

Time analysis
Each iteration includes 3 main steps
A. Finding the closest points :
O(NM) per each point
O(NM*NS) total.
B. Calculating the alignment: O(NS)
C. Updating the scene: O(NS)

Optimizing the Algorithm

The best match/nearest neighbor
problem :
Given N records each described by K
real values (attributes) , and a
dissimilarity measure D , find the m
records closest to a query record.

Optimizing the Algorithm

K-D Tree :
Construction time: O(knlogn)
Space: O(n)
Region Query : O(n1-1/k+k )

Optimizing the Algorithm

Optimizing the K-D Tree :

Motivation: In each internal node we

can exclude the sub K-D tree if the
distance to the partition is greater than
the ball radius .
Adjusting the discriminating number,
the partition value , and the number of
records in each bucket.

Optimizing the Algorithm

Optimizing the Algorithm

Optimizing the Algorithm

Optimizing the Algorithm

Optimizing the K-D Tree :

We choose in each internal node the key
with the largest spread values as the
discriminator and the median as the
partition value.

Optimizing the Algorithm

The Optimized K-D Tree :

Construction time :
Tn = 2Tn/2+kN = O(KNlogN)
Search time:
O(logN) Expected.

Optimizing the Algorithm

The Optimized K-D Tree :

The algorithm can use the m-closest
points to cache potentially closest
points.

Optimizing the Algorithm

As the ICP algorithm proceeds a

sequence of vectors is generated :
q1, q2, q3, q4

qk qk qk 1

qk qk 1
1
k cos

qk qk 1

Optimizing the Algorithm

Let be a small angular tolerance.

Suppose :

k & k 1

vk 0, vk 1 qk , vk 2 qk 1 vk 1

Instead of 50 iterations in the ICP ,

this accelerated variant converges in
less than 20 iterations.

Time analysis
Each iteration includes 3 main steps
A. Finding the closest points :
O(NM) per each point
O(NMlogNS) total.
B. Calculating the alignment: O(NS)
C. Updating the scene: O(NS)

ICP Variants

Variants on the following stages of ICP

have been proposed:
1.
2.
3.
4.
5.
6.

Selecting sample points (from one or both mesh

Matching to points in the other mesh
Weighting the correspondences
Rejecting certain (outlier) point pairs
Assigning an error metric to the current transform
Minimizing the error metric w.r.t. transformation

Performance of Variants

Can analyze various aspects of

performance:
Speed
Stability
Tolerance of noise and/or outliers
Maximum initial misalignment

ICP Variants
1. Selecting sample points (from one or
both meshes).
2. Matching to points in the other mesh.
3. Weighting the correspondences.
4. Rejecting certain (outlier) point pairs.
5. Assigning an error metric to the
current transform.
6. Minimizing the error metric w.r.t.
transformation.

Selection of points

Use all available points [Besl 92].

Uniform subsampling [Turk 94].
Random sampling in each iteration
[Masuda 96].
Ensure that samples have normals
distributed as uniformly as possible
[Rusinkiewicz 01].

Selection of points

Uniform Sampling

Normal-Space Sampling

ICP Variants
1. Selecting sample points (from one or
both meshes).
2. Matching to points in the other mesh.
3. Weighting the correspondences.
4. Rejecting certain (outlier) point pairs.
5. Assigning an error metric to the
current transform.
6. Minimizing the error metric w.r.t.
transformation.

Points matching

Closest point in the other mesh [Besl

92].
Normal shooting [Chen 91].
Reverse calibration [Blais 95].
Restricting matches to compatible
points (color, intensity , normals ,
curvature ..) [Pulli 99].

Points matching

Closest point :

Points matching

Normal Shooting

Points matching

Projection (reverse calibration)

Project the sample point onto the
destination mesh , from the point of view
of the destination meshs camera.

Points matching

ICP Variants
1. Selecting sample points (from one or
both meshes).
2. Matching to points in the other mesh.
3. Weighting the correspondences.
4. Rejecting certain (outlier) point pairs.
5. Assigning an error metric to the
current transform.
6. Minimizing the error metric w.r.t.
transformation.

Weighting of pairs

Constant weight.
Assigning lower weights to pairs
with greater point-to-point distance :
Weight 1

Dist ( p1 , p2 )
Dist max

Weighting based on compatibility of

normals :Weight n1 n2
Scanner uncertainty

Weighting of pairs
The rectangles and circles
indicate the scanner
reflectance value.

ICP Variants
1. Selecting sample points (from one or
both meshes).
2. Matching to points in the other mesh.
3. Weighting the correspondences.
4. Rejecting certain (outlier) point pairs.
5. Assigning an error metric to the
current transform.
6. Minimizing the error metric w.r.t.
transformation.

Rejecting Pairs

Corresponding points with point to point

distance higher than a given threshold.
Rejection of worst n% pairs based on some
metric.
Pairs containing points on end vertices.
Rejection of pairs whose point to point distance
is higher than n*.
Rejection of pairs that are not consistent with
their neighboring pairs [Dorai 98] :
(p
) are
if
1,q(1)
Dist
p1,, (p
p22),q
2Dist
(qinconsistent
1 , q2 ) threshold

Rejecting Pairs
Distance thresholding

Rejecting Pairs
Points on end vertices

Rejecting Pairs
Inconsistent Pairs
q1
q2

p1

p2

ICP Variants
1. Selecting sample points (from one or
both meshes).
2. Matching to points in the other mesh.
3. Weighting the correspondences.
4. Rejecting certain (outlier) point pairs.
5. Assigning an error metric to the
current transform.
6. Minimizing the error metric w.r.t.
transformation.

Error metric and

minimization

Sum of squared distances between

corresponding points .
There exist closed form solutions for
rigid body transformation :
1.
2.
3.
4.

SVD
Quaternions
Orthonoraml matrices
Dual quaternions.

Error metric and

minimization

Sum of squared distances from each

sample point to the plane containing
the destination point (Point to Plane)
[Chen 91].
No closed form solution available.

Error metric and

minimization

Using point-to-plane distance instead of

point-to-point lets flat regions slide
along each other [Chen & Medioni 91]

Error metric and

minimization

Closest Point

Error metric and

minimization

Point to plane

Error metric and

minimization
Search for alignment :

Repeatedly generating set of

corresponding points using the current
transformations and finding new
transformations that minimizes the
error metric [Chen 91].

The above method combined with

extrapolation in transform space [Besl
92].

Real Time ICP

Robust Simultaneous
Alignment of Multiple Range
Images

Motivation

Motivation

Motivation
Graph of the twentyseven registered scans
of the Cathedral data
set. The nodes
correspond to the
individual range scans.
The edges show pair
wise alignments.
The directed edges
show the paths from
each scan to the pivot
scan that is used as an
anchor.

Motivation

Motivation

Registering multiple
Images

1.
2.
3.

4.

Sequential :
Less memory is needed.
Cheap computation cost.
Each alignment step is not afected
by
number of images.
Less accurate.

Registering multiple
Images

Sequential :
as we progress in the alignment
the accumulated error is
noticeable.

Registering multiple
Images

1.

2.

Simultaneous
Difusively distribute the alignment
error over all overlaps of each range
images.
Large Computational cost.

Registering multiple
Images

Simultaneous
The total alignment error is
difusively distributed among all
pairs.

The Algorithm
Array KDTrees, Scenes, PointMates, Transforms
foreach r in AllRangeImages
foreach s in AllRangeImage-r
Scenes[s] = s
foreach i in Pointsof(r)
foreach s in Scenes
PointMates[i] += CorrespondenceSearch(i,KDTree[s])
Transforms[r] = TransformationStep(PointMates)
TransformAll(AllRangeImages, Transforms)

Speeding Up

During the first iterations it is more

important to bring the sets of points
closer to each other than to accurately
calculate the transform .
Random sub sampling of the points.

Outliers Rejection

Outlier thresholding:
standard deviation of the error.
Eliminate matches with error > |k|
Some valid points might be classified as
outliers, and some outliers might be
classified as valid points.

Outliers Rejection

Median/Rank estimation:
Calculate the median, which is (almost
guaranteed) valid point and use its error
as estimation, i.e. Least Median of
Squares.
Requires exhaustive search.

Outliers Rejection

M-Estimators
Instead of minimizing the sum of square
residuals , the square residuals are
replaced by (ri).
Each point is assigned with a likelihood
probability (weight) and after each
iteration the probability is updated with
respect to the residual.

The End