Iterative Closest Point

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.
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

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 z2 ) 2
Convergence Theorem

The ICP algorithm always converges monotonically to a local minimum with respect to the MSE distance objective function.
Convergence Theorem


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.
Snew rot(S ) trans
The Algorithm
Init the error to ∞
Y = CP(M,S),e (rot,trans,d)
S`= rot(S)+trans
Calculate correspondence
Calculate alignment
Apply alignment Update error
d` = d
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
Motivation

Shape inspection Motion estimation Appearance analysis Texture Mapping Tracking
Motivation

Range images registration
Motivation

Range images registration
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.
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
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 :
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)))

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 :

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 …
1 dk NM
y
i 1
NM
ik
Sik
2
Convergence Theorem

Proof : After the last alignment, the closest point operator is applied : Yk 1 C(M , Sk 1 ) It is clear that:
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

i 1
NS
mi Rot( si ) Trans
2
2

i 1
NS
mi R(qR ) si qT
The alignment is : (rot, trans, dmse ) (M , S )

Correspondence error :
1 ek NS

y
i 1
NS
ik
sik
2
Alignment error:
1 dk NS
y
i 1
NS
ik
Rotk (sio ) Transk
2
Convergence Theorem
Ek Dk
S`= rot(S)+trans
Calculate correspondence

i 1
NS
yik sik