icp-SVD

7/24/2019 icp-SVD

1/37

1

Wolfram Burgard, Cyrill Stachniss,

Maren Bennewitz, Kai Arras

Iterative Closest Point

Algorithm

Introduction toMobile Robotics

7/24/2019 icp-SVD

2/37

2

Motivation

Goal: Find local transformation to align points

7/24/2019 icp-SVD

3/37

3

The Problem

!

Given two corresponding point sets:

!

Wanted: Translation t and rotation Rthatminimize the sum of the squared error:

Where

are corresponding pointsand

7/24/2019 icp-SVD

4/37

4

Key Idea

!

If the correct correspondences are known,the correct relative rotation/translation canbe calculated in closed form

7/24/2019 icp-SVD

5/37

5

Center of Mass

and

are the centers of mass of the two point sets

Idea:

! Subtract the corresponding center of massfrom every point in the two point setsbefore calculating the transformation

!

The resulting point sets are:

and

7/24/2019 icp-SVD

6/37

6

Singular Value Decomposition

Let

denote the singular value decomposition (SVD) of Wby:

whereare unitary, and

are the singular values of W

7/24/2019 icp-SVD

7/37

7

SVD

Theorem(without proof):

If rank(W) = 3, the optimal solution of E(R,t) isunique and is given by:

The minimal value of error function at (R,t) is:

7/24/2019 icp-SVD

8/37

8

ICP with Unknown Data Association

!

If the correct correspondences are notknown, it is generally impossible todetermine the optimal relative rotation/translation in one step

7/24/2019 icp-SVD

9/37

9

Iterative Closest Point (ICP)Algorithm

! Idea: Iterate to find alignment

! Iterative Closest Points[Besl & McKay 92]

! Converges if starting positions are

close enough

7/24/2019 icp-SVD

10/37

Basic ICP Algorithm

!

Determine corresponding points! Compute rotation R, translation t via SVD

! Apply Rand tto the points of the set to be

registered

!

Compute the error E(R,t)! If error decreased and error > threshold

! Repeat these steps

!

Stop and output final alignment, otherwise

10

7/24/2019 icp-SVD

11/37

11

ICP Example

7/24/2019 icp-SVD

12/37

12

ICP Variants

Variants on the following stages ofICP have been proposed:

1. Point subsets (from one or both point

sets)2. Weighting the correspondences

3. Data association

4. Rejecting certain (outlier) point pairs

7/24/2019 icp-SVD

13/37

13

Performance of Variants

!

Various aspects of performance:! Speed

! Stability (local minima)

!

Tolerance wrt. noise and outliers

! Basin of convergence(maximum initial misalignment)

7/24/2019 icp-SVD

14/37

14

ICP Variants


sets)

2.

Weighting the correspondences3. Data association


7/24/2019 icp-SVD

15/37

15

Selecting Source Points

!

Use all points! Uniform sub-sampling

! Random sampling

!

Feature based sampling! Normal-space sampling

(Ensure that samples have normals distributed asuniformly as possible)

7/24/2019 icp-SVD

16/37

16

Normal-Space Sampling

uniform sampling normal-space sampling

7/24/2019 icp-SVD

17/37

17

Comparison

!

Normal-space sampling better for mostlysmooth areas with sparse features[Rusinkiewicz et al., 01]

Random sampling Normal-space sampling

7/24/2019 icp-SVD

18/37

18

Comparison

!

Normal-space sampling better for mostlysmooth areas with sparse features[Rusinkiewicz et al., 01]

Random sampling Normal-space sampling

7/24/2019 icp-SVD

19/37

19

Feature-Based Sampling

3D Scan (~200.000 Points) Extracted Features (~5.000 Points)

! Try to find importantpoints

!

Decreases the number of correspondences to find

! Higher efficiency and higher accuracy

! Requires preprocessing

7/24/2019 icp-SVD

20/37

20

ICP Application(With Uniform Sampling)

[Nuechter et al., 04]

7/24/2019 icp-SVD

21/37

21

ICP Variants


sets)

2.



7/24/2019 icp-SVD

22/37

22

Weighting

!

Select a set of points for each set! Match the selected points of the two sets

! Weight the corresponding pairs

!

E.g., assign lower weights for points withhigher point-point distances

! Determine transformation that minimizesthe error function

7/24/2019 icp-SVD

23/37

24

ICP Variants


sets)

2.



7/24/2019 icp-SVD

24/37

25

Data Association

!

Has greatest effect on convergence andspeed

! Matching methods:

! Closest point

!

Normal shooting

! Closest compatible point

! Projection-based

7/24/2019 icp-SVD

25/37

26

Closest-Point Matching

!

Find closest point in other the point set(using kd-trees)

Generally stable, but slow convergence

and requires preprocessing

7/24/2019 icp-SVD

26/37

27

Normal Shooting

!

Project along normal, intersect other pointset

Slightly better convergence results than

closest point for smoothstructures, worse

for noisy or complex structures

7/24/2019 icp-SVD

27/37

28

Closest Compatible Point

!

Improves the two previous variants byconsidering the compatibilityof the points

! Only match compatible points

! Compatibility can be based on

!

Normals!

Colors

! Curvature

! Higher-order derivatives

! Other local features

7/24/2019 icp-SVD

28/37

29

Point-to-Plane Error Metric

!

Minimize the sum of the squared distancebetween a point and the tangent plane at itscorrespondence point[Chen & Medioni 91]

tangentplane

s1

sourcepoint

destination

point

d1

n1

unit

normal

s2

d2

n2

s3

d3

n3

destination

surface

source

surface

l1

l2

l3

image from [Low 04]

7/24/2019 icp-SVD

29/37

30

Point-to-Plane Error Metric

!

Solved using standard nonlinear leastsquares methods (e.g., Levenberg-Marquardt method [Press92]).

! Each iteration generally slower than the

point-to-point version, however, oftensignificantly better convergence rates[Rusinkiewicz01]

! Using point-to-plane distance instead of

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

7/24/2019 icp-SVD

30/37

31

Projection

!

Finding the closest point is the mostexpensive stage of the ICP algorithm

! Idea: Simplified nearest neighbor search

! For range images, one can project the

points according to the view-point [Blais 95]

7/24/2019 icp-SVD

31/37

32

Projection-Based Matching

!

Constant time

! Does not require precomputing a specialdata structure

!

Requires point-to-plane error metric! Slightly worse alignments per iteration

7/24/2019 icp-SVD

32/37

33

ICP Variants


sets)

2.



7/24/2019 icp-SVD

33/37

34

Rejecting (Outlier) Point Pairs

!

Corresponding points with point to pointdistance higher than a given threshold

7/24/2019 icp-SVD

34/37

35


!


! Rejection of pairs that are not consistentwith their neighboring pairs [Dorai 98]

7/24/2019 icp-SVD

35/37

36


!


! Rejection of pairs that are not consistentwith their neighboring pairs [Dorai 98]

!

Sort all correspondences with respect totheir error and delete the worst t%,Trimmed ICP (TrICP) [Chetverikov et al. 02]

! tis used to estimate the overlap

!

Problem: Knowledge about the overlap isnecessary or has to be estimated

7/24/2019 icp-SVD

36/37

Summary: ICP Algorithm

!

Potentially sample Points! Determine corresponding points

! Potentially weight / reject pairs

! Compute rotation R, translation t (e.g. SVD)

!

Apply Rand tto all points of the set to beregistered

! Compute the error E(R,t)

! If error decreased and error > threshold

!

Repeat to determine correspondences etc.

! Stop and output final alignment, otherwise

38

7/24/2019 icp-SVD

37/37

39

ICP Summary

!

ICP is a powerful algorithm for calculatingthe displacement between scans

! The major problem is to determine thecorrect data associations

!

Convergence speed depends on pointmatchings

! Given the correct data associations, thetransformation can be computed efficiently

using SVD

icp-SVD

Documents