Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest...
Transcript of Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest...
![Page 1: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/1.jpg)
1
Iterative Closest Point Algorithm
Introduction toMobile Robotics
Slides adopted from: Wolfram Burgard, Cyrill Stachniss,
Maren Bennewitz, Kai Arras and Probabilistic Robotics Book
![Page 2: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/2.jpg)
2
Motivation
![Page 3: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/3.jpg)
3
The Problem
§ Given: two corresponding point sets:
§ Wanted: translation t and rotation R that minimizes the sum of the squared error:
Where are corresponding points.and
![Page 4: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/4.jpg)
4
Key Idea§ If the correct correspondences are known,
the correct relative rotation/translation can be calculated in closed form.
![Page 5: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/5.jpg)
5
Center of Mass
and
are the centers of mass of the two point sets.Idea:§ Subtract the corresponding center of mass
from every point in the two point sets before calculating the transformation.
§ The resulting point sets are:
and
![Page 6: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/6.jpg)
6
SVD
Let
denote the singular value decomposition (SVD) of W by:
where are unitary, and
are the singular values of W.
![Page 7: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/7.jpg)
7
SVDTheorem (without proof):
If rank(W) = 3, the optimal solution of E(R,t) is unique and is given by:
The minimal value of error function at (R,t) is:
![Page 8: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/8.jpg)
Proof
8
![Page 9: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/9.jpg)
9
ICP with Unknown Data Association
§ If correct correspondences are not known, it is generally impossible to determine the optimal relative rotation/translation in one step
![Page 10: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/10.jpg)
10
ICP-Algorithm
§ Idea: iterate to find alignment§ Iterated Closest Points (ICP)
[Besl & McKay 92]§ Converges if starting positions are �close enough�
![Page 11: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/11.jpg)
11
Iteration-Example
![Page 12: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/12.jpg)
12
ICP-Variants
§ Variants on the following stages of ICP 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
![Page 13: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/13.jpg)
13
Performance of Variants
§ Various aspects of performance:§ Speed§ Stability (local minima)§ Tolerance wrt. noise and/or outliers§ Basin of convergence
(maximum initial misalignment)
§ Here: properties of these variants
![Page 14: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/14.jpg)
14
ICP Variants
1. Point subsets (from one or both point sets)
2. Weighting the correspondences3. Data association4. Rejecting certain (outlier) point pairs
![Page 15: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/15.jpg)
15
Selecting Source Points§ Use all points§ Uniform sub-sampling§ Random sampling§ Feature based Sampling§ Normal-space sampling
§ Ensure that samples have normals distributed as uniformly as possible
![Page 16: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/16.jpg)
16
Normal-Space Sampling
uniform sampling normal-space sampling
![Page 17: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/17.jpg)
17
Comparison§ Normal-space sampling better for mostly-
smooth areas with sparse features [Rusinkiewicz et al.]
Random sampling Normal-space sampling
![Page 18: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/18.jpg)
18
Feature-Based Sampling
3D Scan (~200.000 Points) Extracted Features (~5.000 Points)
§ try to find �important� points§ decrease the number of correspondences § higher efficiency and higher accuracy § requires preprocessing
![Page 19: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/19.jpg)
19
Application
[Nuechter et al., 04]
![Page 20: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/20.jpg)
20
ICP Variants
1. Point subsets (from one or both point sets)
2. Weighting the correspondences3. Data association 4. Rejecting certain (outlier) point pairs
![Page 21: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/21.jpg)
21
Selection vs. Weighting§ Could achieve same effect with weighting§ Hard to guarantee that enough samples of
important features except at high sampling rates
§ Weighting strategies turned out to be dependent on the data.
§ Preprocessing / run-time cost tradeoff (how to find the correct weights?)
![Page 22: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/22.jpg)
22
ICP Variants
1. Point subsets (from one or both point sets)
2. Weighting the correspondences3. Data association4. Rejecting certain (outlier) point pairs
![Page 23: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/23.jpg)
23
Data Association§ has greatest effect on convergence and
speed§ Closest point§ Normal shooting§ Closest compatible point§ Projection§ Using kd-trees or oc-trees
![Page 24: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/24.jpg)
24
Closest-Point Matching§ Find closest point in other the point set
Closest-point matching generally stable,but slow and requires preprocessing
![Page 25: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/25.jpg)
25
Normal Shooting§ Project along normal, intersect other point
set
Slightly better than closest point for smooth structures, worse for noisy or complex structures
![Page 26: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/26.jpg)
26
Point-to-Plane Error Metric§ Using point-to-plane distance instead of
point-to-point lets flat regions slide along each other [Chen & Medioni 91]
![Page 27: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/27.jpg)
27
Projection§ Finding the closest point is the most
expensive 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]
![Page 28: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/28.jpg)
28
Projection-Based Matching
§ Slightly worse alignments per iteration§ Each iteration is one to two orders of
magnitude faster than closest-point§ Requires point-to-plane error metric
![Page 29: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/29.jpg)
29
Closest Compatible Point§ Improves the previous two variants by
considering the compatibility of the points§ Compatibility can be based on normals,
colors, etc.§ In the limit, degenerates to feature
matching
![Page 30: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/30.jpg)
30
ICP Variants
1. Point subsets (from one or both point sets)
2. Weighting the correspondences3. Nearest neighbor search 4. Rejecting certain (outlier) point pairs
![Page 31: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/31.jpg)
31
Rejecting (outlier) point pairs§ sorting all correspondences with respect to
there error and deleting the worst t%, Trimmed ICP (TrICP) [Chetverikov et al. 2002]
§ t is to Estimate with respect to the Overlap
Problem: Knowledge about the overlap is necessary or has to be estimated
![Page 32: Introduction to Mobile Robotics Iterative Closest …kosecka/cs685/cs685-icp.pdf1 Iterative Closest Point Algorithm Introduction to Mobile Robotics Slides adopted from: Wolfram Burgard,](https://reader033.fdocuments.in/reader033/viewer/2022052408/5f0cf19e7e708231d437e6ab/html5/thumbnails/32.jpg)
32
ICP-Summary§ ICP is a powerful algorithm for calculating
the displacement between scans.§ The major problem is to determine the
correct data associations.§ Given the correct data associations, the
transformation can be computed efficiently using SVD.