Iterative Closest Point

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
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.
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
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 :
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
Convergence Theorem

The ICP algorithm always converges monotonically to a local minimum with respect to the MSE distance objective function.
Convergence Theorem
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 .


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.

i 1
NS
yik sik
2

i 1
NS
yik Rotk ( sio ) Transk
2
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.
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

The Optimized K-D Tree :
Construction time : Tn = 2Tn/2+kN = O(KNlogN) Search time: O(logN) Expected.
Optimizing the Algorithm

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 :
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 )
Aligning 3D Data


How to find correspondences: User input? Feature detection? Signatures? Alternative: assume closest points correspond
Aligning 3D Data
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)))

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
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
Calculate alignment
Apply alignment Calculate correspondence Calculate alignment
Ek+1 Dk+1
Convergence Theorem

Proof :
S k Rotk ( S 0 ) Transk Yk C ( M , sk ) 1 ek NS 1 dk NS
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.

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 …
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)
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:
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ຫໍສະໝຸດ Baidu:
Motivation

Shape inspection Motion estimation Appearance analysis Texture Mapping Tracking
Motivation

Range images registration
Motivation

Range images registration