1. Iterative Closest Point
Ronen Gvili

2. The Problem
Align two partially- overlapping meshes given initial guess for 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 ∞ Calculate correspondence
Y = CP(M,S),e
Calculate correspondence
(rot,trans,d)
Calculate alignment
S`= rot(S)+trans
Apply alignment
d` = d
Update error
If error > threshold

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

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 Calculate correspondence Calculate alignment
Apply alignment Ek Dk
S`= rot(S)+trans
Ek+1
Calculate correspondence
Dk+1
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  Sk+1 .
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(NM*NS) 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/k+k )

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

33. Optimizing the Algorithm

34. Optimizing the Algorithm

35. Optimizing the Algorithm

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

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

38. Optimizing the Algorithm
The Optimized K-D Tree :
The algorithm can use the m-closest points to cache potentially closest points.

39. Optimizing the Algorithm
As the ICP algorithm proceeds a sequence of vectors is generated :
q1, q2, q3, q4 …

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

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

42. ICP Variants
Variants on the following stages of ICP have been proposed:
Selecting sample points (from one or both meshes)
Matching to points in the other mesh
Weighting the correspondences
Rejecting certain (outlier) point pairs
Assigning an error metric to the current transform
Minimizing the error metric w.r.t. transformation

43. Performance of Variants
Can analyze various aspects of performance:
Speed
Stability
Tolerance of noise and/or outliers
Maximum initial misalignment

44. ICP Variants Selecting sample points (from one or both meshes).
Matching to points in the other mesh.
Weighting the correspondences.
Rejecting certain (outlier) point pairs.
Assigning an error metric to the current transform.
Minimizing the error metric w.r.t. transformation.

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

46. Normal-Space Sampling
Selection of points
Uniform Sampling
Normal-Space Sampling

47. ICP Variants Selecting sample points (from one or both meshes).
Matching to points in the other mesh.
Weighting the correspondences.
Rejecting certain (outlier) point pairs.
Assigning an error metric to the current transform.
Minimizing the error metric w.r.t. transformation.

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

49. Points matching
Closest point :

50. Points matching
Normal Shooting

51. Points matching Projection (reverse calibration)
Project the sample point onto the destination mesh , from the point of view of the destination mesh’s camera.

52. Points matching

53. ICP Variants Selecting sample points (from one or both meshes).
Matching to points in the other mesh.
Weighting the correspondences.
Rejecting certain (outlier) point pairs.
Assigning an error metric to the current transform.
Minimizing the error metric w.r.t. transformation.

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

55. Weighting of pairs The rectangles and circles indicate the scanner
reflectance value.

56. ICP Variants Selecting sample points (from one or both meshes).
Matching to points in the other mesh.
Weighting the correspondences.
Rejecting certain (outlier) point pairs.
Assigning an error metric to the current transform.
Minimizing the error metric w.r.t. transformation.

57. 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 n*σ.
Rejection of pairs that are not consistent with their neighboring pairs [Dorai 98] :
(p1,q1) , (p2,q2) are inconsistent iff

58. Rejecting Pairs
Distance thresholding

59. Rejecting Pairs
Points on end vertices

60. Rejecting Pairs
Inconsistent Pairs p1 p2 q2 q1

61. ICP Variants Selecting sample points (from one or both meshes).
Matching to points in the other mesh.
Weighting the correspondences.
Rejecting certain (outlier) point pairs.
Assigning an error metric to the current transform.
Minimizing the error metric w.r.t. transformation.

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

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

64. Error metric and minimization
Using point-to-plane distance instead of point-to-point lets flat regions slide along each other [Chen & Medioni 91]

65. Error metric and minimization
Closest Point

66. Error metric and minimization
Point to plane

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

68. Real Time ICP

69. Robust Simultaneous Alignment of Multiple Range Images

70. Motivation

71. Motivation

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

73. Motivation

74. Motivation

75. Registering multiple Images
Sequential :
Less memory is needed.
Cheap computation cost.
Each alignment step is not affected by number of images.
Less accurate.

76. Registering multiple Images
Sequential :
as we progress in the alignment the accumulated error is noticeable.

77. Registering multiple Images
Simultaneous
Diffusively distribute the alignment error over all overlaps of each range images.
Large Computational cost.

78. Registering multiple Images
Simultaneous
The total alignment error is diffusively distributed among all pairs.

79. The Algorithm Array KDTrees, Scenes, PointMates, Transforms
foreach r in AllRangeImages
foreach s in AllRangeImage-r
Scenes[s] = s
foreach i in Pointsof(r)
foreach s in Scenes
PointMates[i] += CorrespondenceSearch(i,KDTree[s])
Transforms[r] = TransformationStep(PointMates)
TransformAll(AllRangeImages, Transforms)

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

81. Outliers Rejection Outlier thresholding:
σ – standard deviation of the error.
Eliminate matches with error > |kσ|
Some valid points might be classified as outliers, and some outliers might be classified as valid points.

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

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

84. The End