Iterative Closest Point

1
Iterative Closest Point

• Ronen Gvili

2
The Problem

• Align two partially-overlapping meshesgiven
  initial guessfor relative transform

3
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)))

4
Motivation

• Shape inspection
• Motion estimation
• Appearance analysis
• Texture Mapping
• Tracking

5
Motivation

• Range images
• registration

6
Motivation

• Range images registration

7
Range Scanners
8
Aligning 3D Data
9
Corresponding Point Set Alignment

• Let M be a model point set.
• Let S be a scene point set.
• We assume
• NM NS.
• Each point Si correspond to Mi .

10
Corresponding Point Set Alignment

• The MSE objective function
• The alignment is

11
Aligning 3D Data

• If correct correspondences are known, can find
  correct relative rotation/translation

12
Aligning 3D Data

• How to find correspondences User input? Feature
  detection? Signatures?
• Alternative assume closest points correspond

13
Aligning 3D Data

• How to find correspondences User input? Feature
  detection? Signatures?
• Alternative assume closest points correspond

14
Aligning 3D Data

• Converges if starting position close enough

15
Closest Point

• Given 2 points r1 and r2 , the Euclidean distance
  is
• Given a point r1 and set of points A , the
  Euclidean distance is

16
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

17
Finding Matches
C the closest point operator Y the set of
closest points to S
18
Finding Matches

• Finding each match is performed in O(NM) worst
  case.
• Given Y we can calculate alignment
• S is updated to be

19
The Algorithm
Init the error to 8
Y CP(M,S),e
Calculate correspondence
(rot,trans,d)
Calculate alignment
S rot(S)trans
Apply alignment
d d
Update error
If error gt threshold
20
The Algorithm

• function ICP(Scene,Model)
• begin
• E ? 8
• (Rot,Trans) ? In Initialize-Alignment(Scene,Model)
  
• repeat
• E ? E
• Aligned-Scene ? Apply-Alignment(Scene,Rot,Tran
  s)
• Pairs ? Return-Closest-Pairs(Aligned-Scene,Mod
  el)
• (Rot,Trans,E) ? Update-Alignment(Scene,Model,
  Pairs,Rot,Trans)
• Until E- E lt Threshold
• return (Rot,Trans)
• end

21
Convergence Theorem

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

22
Convergence Theorem

• Correspondence error
• Alignment error

23
Convergence Theorem
Ek1
Calculate correspondence
Dk1
Calculate alignment
24
Convergence Theorem

• Proof

25
Convergence Theorem

• Proof
• If not - the identity transform would yield a
  smaller MSE than the least square alignment.
• Apply the alignmentqk on S0 ? Sk1 .
• Assuming the correspondences are maintained
  the MSE is still dk.

26
Convergence Theorem

• Proof
• After the last alignment, the closest point
  operator is applied
• It is clear that
• Thus

27
Time analysis

• Each iteration includes 3 main steps
• A. Finding the closest points
• O(NM) per each point
• O(NMNS) total.
• B. Calculating the alignment O(NS)
• C. Updating the scene O(NS)

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

29
Optimizing the Algorithm

• K-D Tree
• Construction time O(knlogn)
• Space O(n)
• Region Query O(n1-1/kk )

30
(No Transcript)
31
(No Transcript)
32
Optimizing the Algorithm

• The Bounds Overlap Ball query
• Function BOP
• Local d,sum
• begin
• For d 1 to K
• if Xqd lt B-d
• sum dist(d,Xqd,B-d)
• else if Xqd gt Bd
• sum dist(d,Xqd,Bd)
• if sum gt PQ0 return false
• return true

33
Optimizing the Algorithm

• Function SEARCH(node)
• Local p,d,temp
• Begin
• if node leaf
• if ball within bounds update PQ
• end
• d ? Discnode , p ? Partnode
• If Xqd lt p
• Begin Checking the left son
• temp ? Bd Bd ? p
• SEARCH(leftson(node))
• Bd ? temp
• end
• else Begin Checking the right son
• temp ? B-d B-d ? p
• SEARCH(rightson(node))
• B-d ? temp
• end

• If Xqd lt p
• begin
• temp ? B-d B-d ? p
• if BOP SEARCH(rightson(node))
• B-d ? temp
• else begin
• temp ? Bd Bd ? p
• if BOP SEARCH(leftson(node))
• Bd ? temp
• end

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

35
Optimizing the Algorithm
36
Optimizing the Algorithm
37
Optimizing the Algorithm
38
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.

39
Optimizing the Algorithm

• The Optimized K-D Tree
• Construction time
• Tn 2Tn/2kN O(KNlogN)
• Search time
• O(logN) Expected.

40
Optimizing the Algorithm

• The Optimized K-D Tree
• The algorithm can use the m-closest points to
  cache potentially closest points.

41
Optimizing the Algorithm

• As the ICP algorithm proceeds a sequence of
  vectors is generated
• q1, q2, q3, q4

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

43
Time analysis

• Each iteration includes 3 main steps
• A. Finding the closest points
• O(NM) per each point
• O(NMlogNS) total.
• B. Calculating the alignment O(NS)
• C. Updating the scene O(NS)

44
ICP Variants

• Variants on the following stages of ICPhave 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

45
Performance of Variants

• Can analyze various aspects of performance
• Speed
• Stability
• Tolerance of noise and/or outliers
• Maximum initial misalignment

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

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

48
Selection of points
Uniform Sampling
Normal-Space Sampling
49
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.

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

51
Points matching

• Closest point

52
Points matching

• Normal Shooting

53
Points matching

• Projection (reverse calibration)
• Project the sample point onto the destination
  mesh , from the point of view of the destination
  meshs camera.

54
Points matching
55
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.

56
Weighting of pairs

• Constant weight.
• Assigning lower weights to pairs with greater
  point-to-point distance
• Weighting based on compatibility of normals
• Scanner uncertainty

57
Weighting of pairs
The rectangles and circles indicate the scanner
reflectance value.
58
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.

59
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 ns.
• Rejection of pairs that are not consistent with
  their neighboring pairs Dorai 98
• (p1,q1) , (p2,q2) are inconsistent iff

60
Rejecting Pairs

• Distance thresholding

61
Rejecting Pairs

• Points on end vertices

62
Rejecting Pairs

• Inconsistent Pairs

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

64
Error metric and minimization

• Sum of squared distances between corresponding
  points .
• There exist closed form solutions for rigid body
  transformation
• SVD
• Quaternions
• Orthonoraml matrices
• Dual quaternions.

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

66
Error metric and minimization

• Using point-to-plane distance instead of
  point-to-point lets flat regions slide along each
  other Chen Medioni 91

67
Error metric and minimization
Closest Point
68
Error metric and minimization
Point to plane
69
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.

70
Real Time ICP
71
Robust Simultaneous Alignment of Multiple Range
Images
72
Motivation
73
Motivation
74
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.

75
Motivation
76
Motivation
77
Registering multiple Images

• Sequential
• Less memory is needed.
• Cheap computation cost.
• Each alignment step is not affected by number
  of images.
• Less accurate.

78
Registering multiple Images

• Sequential
• as we progress in the alignment the accumulated
  error is noticeable.

79
Registering multiple Images

• Simultaneous
• Diffusively distribute the alignment
  error over all overlaps of each range images.
• Large Computational cost.

80
Registering multiple Images

• Simultaneous
• The total alignment error is diffusively
  distributed among all pairs.

81
The Algorithm

• Array KDTrees, Scenes, PointMates, Transforms
• foreach r in AllRangeImages
• foreach s in AllRangeImage-r
• Sceness s
• foreach i in Pointsof(r)
• foreach s in Scenes
• PointMatesi CorrespondenceSearch(i,KDTree
  s)
• Transformsr TransformationStep(PointMates)
  
• TransformAll(AllRangeImages, Transforms)

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

83
Outliers Rejection

• Outlier thresholding
• s standard deviation of the error.
• Eliminate matches with error gt ks
• Some valid points might be classified as
  outliers, and some outliers might be classified
  as valid points.

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

85
Outliers Rejection

• M-Estimators
• Instead of minimizing the sum of square
  residuals , the square residuals are replaced by
  ?(ri).
• Each point is assigned with a likelihood
  probability (weight) and after each iteration the
  probability is updated with respect to the
  residual.

86
The End