You are on page 1of 84

Iterative Closest Point

Ronen Gvili
The Problem
Align two partially-
overlapping 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. N
M
= N
S
.
2. Each point S
i
correspond to M
i
.
Corresponding Point Set Alignment
The MSE objective function :




The alignment is :

=
=
=
=
S
S
N
i
T i R i
S
N
i
i i
S
q s q R m
N
q f
Trans s Rot m
N
T R f
1
2
1
2
) (
1
) (
) (
1
) , (
) , ( ) , , ( S M d trans rot
mse
u =
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 r
1
and r
2
, the Euclidean
distance is:


Given a point r
1
and set of points A ,
the Euclidean distance is:

2
2 1
2
2 1
2
2 1 2 1 2 1
) ( ) ( ) ( ) , ( z z y y x x r r r r d + + = =
) , ( min ) , (
1
.. 1
1 i
n i
a r d A r d
e
=
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 :

s m d M s d
M m
=
e
min ) , (
Finding Matches
M Y
M S C Y
M y
y s d s m d M s d
M m
_
=
e
= =
e
) , (
) , ( min ) , (
C the closest point operator
Y the set of closest points to S
Finding Matches
Finding each match is performed in O(N
M
)
worst case.
Given Y we can calculate alignment


S is updated to be :
) , ( ) , , ( Y S d trans rot u =
trans S rot S
new
+ = ) (
The Algorithm
Init the error to
Calculate correspondence
Calculate alignment
Apply alignment
Update error
If error > threshold
Y = CP(M,S),e
(rot,trans,d)
S`= rot(S)+trans
d` = d
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 :



Alignment error:

=
=
S
N
i
ik ik
S
k
s y
N
e
1
2 1

=
=
S
N
i
k io k ik
S
k
Trans s Rot y
N
d
1
2
) (
1
Convergence Theorem
Calculate correspondence
Calculate alignment
Apply alignment
E
k

D
k
S`= rot(S)+trans
Calculate correspondence
Calculate alignment
E
k+1
D
k+1
Convergence Theorem
Proof :

=
=
=
=
=
+ =
S
S
N
i
k io k ik
S
k
N
i
ik ik
S
k
k k
k k k
Trans s Rot y
N
d
s y
N
e
s M C Y
Trans S Rot S
1
2
1
2
0
) (
1
1
) , (
) (
Convergence Theorem
Proof :
If not - the identity transform would
yield a smaller MSE than the least
square alignment.
Apply the alignmentq
k
on S
0
S
k+1 .

Assuming the correspondences are
maintained

: the MSE is still d
k
.
k k
e d s

=
=
M
N
i
ik ik
M
k
S y
N
d
1
2 1
Convergence Theorem
Proof :
After the last alignment, the closest
point operator is applied :
It is clear that:



Thus :

) , (
1 1 + +
=
k k
S M C Y
k k
k i ik k i k i
d e
S y S y
s
s
+
+ + +
1
1 , 1 , 1 ,
k k k k
e d e d s s s s
+ + 1 1
0
Time analysis
Each iteration includes 3 main steps
A. Finding the closest points :
O(N
M
) per each point
O(N
M
*N
S
) total.
B. Calculating the alignment: O(N
S
)
C. Updating the scene: O(N
S
)
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(n
1-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 :
T
n
= 2T
n/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 :
q
1
, q
2
, q
3
, q
4



|
|
.
|

\
|
A A
A A
=
= A

1
1
1
1
cos
k k
k k
k
k k k
q q
q q
q q q



u
Optimizing the Algorithm
Let be a small angular tolerance.
Suppose :



Instead of 50 iterations in the ICP , this
accelerated variant converges in less
than 20 iterations.


ou
ou u ou u < <
1
&
k k
1 1 2 1
, , 0

+ A = A = =
k k k k k k
v q v q v v

Time analysis
Each iteration includes 3 main steps
A. Finding the closest points :
O(N
M
) per each point
O(N
M
logN
S
) total.
B. Calculating the alignment: O(N
S
)
C. Updating the scene: O(N
S
)
ICP Variants

Variants on the following stages of ICP
have been proposed:
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
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 :


Weighting based on compatibility of
normals :
Scanner uncertainty
max
2 1
) , (
1
Dist
p p Dist
Weight =
2 1
n n Weight - =
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
1
,q
1
) , (p
2
,q
2
) are inconsistent iff

threshold q q Dist p p Dist > ) , ( ) , (
2 1 2 1
Rejecting Pairs
Distance thresholding

Rejecting Pairs
Points on end vertices

Rejecting Pairs
Inconsistent Pairs

p1
p2
q2
q1
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. SVD
2. Quaternions
3. Orthonoraml matrices
4. 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 twenty-
seven 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
Sequential :
1. Less memory is needed.
2. Cheap computation cost.
3. Each alignment step is not affected by
number of images.
4. Less accurate.


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


Registering multiple Images
Simultaneous
1. Diffusively distribute the alignment error
over all overlaps of each range images.
2. Large Computational cost.


Registering multiple Images
Simultaneous
The total alignment error is diffusively
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 (r
i
).
Each point is assigned with a likelihood
probability (weight) and after each iteration
the probability is updated with respect to the
residual.
The End

You might also like