Robotics

Estimation and Learning

with Dan Lee

Week 4.
Localization

4.4 Iterative Closest Point (ICP)

Algorithm
Review: EM Algorithm
> ;
1
arg max '()|+) arg max 1 ln 1 67 (8 9 |-7 , Σ7 )
-,/ 5
& 9<= 7<=

Initialize +? Initialize - and Σ

E-step E-step

Find a lower bound D + +? Update E79

M-step M-step
+?@AB = arg max D(+|+? ) Update - and Σ
&

Stop if converged Stop if converged

General EM EM for GMM

Review: 3D Map Representation
Map visualized in 3D

3D point cloud measurement

Implementation Example
ICP Algorithm
• Problem: Register two point sets X and Y.

Measurement (X) Model (Y)

ICP Algorithm
• Problem 1: Rotation and translation?

Measurement (X) Model (Y)

ICP Algorithm
• Problem 2: Correspondences?

Measurement (X) Model (Y)

ICP Algorithm
• Problem 2: Correspondence

Measurement (X) Model (Y)

?
ICP Algorithm

Initialize F and G
E-step

M-step

Stop if converged
ICP Algorithm

Closest point!
I9 = arg min O9 − IQ
Initialize F and G KL ∈N

E-step
Find H O9 I9
M-step

Stop if converged
 ICP Algorithm

Initialize F and G
E-step
Find H
M-step
S
Find better F and G F, G = arg min 1 R(O9 , I9 )
TU ,KU ∈V
Stop if converged

[SOLUTION] K. Arun, T. Huang, and S. Blostein, “Least-squares fitting of two 3D

point set”, IEEE Transactions on Pattern Analysis and Machine Intelligence, 9(5), pp. 698–700, 1987.
ICP Algorithm

Initialize F and G Measurement (X) Model (Y)

E-step
F, G
Update H
M-step
Update F and G

Stop if converged
ICP: Example

Initialize F and G
E-step
Update H
M-step
Update F and G

Stop if converged
ICP: Motion Increment

Raw measurements are Registration gives the motion

in the local coordinate frame. increment of the body w.r.t the model