Robot Mapping
SLAM Front-Ends
Gian Diego Tipaldi, Wolfram Burgard
1
Graph-Based SLAM
Measurements connect the nodes
through odometry and observations
Robot pose Measurement 2
Graph-Based SLAM
Measurements connect the nodes
through odometry and observations
How to obtain the measurements?
Robot pose Measurement 3
Interplay between Front-End
and Back-End
node positions
Graph Graph
raw Construction Optimization
data (Front-End) graph (Back-End)
(nodes & edges)
today
4
Measurements From Matching
Measurements can be obtained by
matching observations
Popular approaches
Dense matching
Point-to-point matching
Feature-based matching
5
Where to Search for Matches?
Consider uncertainty of the nodes with
respect to the current one
Positional
sensor uncertainty
ranges with respect
to A
B1 B2
Views may Views cannot
Courtesy: E. Olson overlap overlap 6
Note on the Uncertainty
In graph-based SLAM, computing the
uncertainty relative to A requires
inverting the Hessian H
Fast approximation by Dijkstra
expansion (“propagate uncertainty
along the shortest path in the graph”)
Conservative estimate
7
Do you Recall Scan Matching?
Maximize the likelihood of the current
pose relative to the previous pose and
map
current measurement robot motion
map constructed so far
8
Sensor Matching as Front-End
Estimate uncertainty of nodes relative
to the current pose
Get previous observations in the
relevant area
Match the current observations with
the previous ones
Evaluate match
Accept match based on a threshold
9
Correlative Matching
Courtesy: E. Olson
10
Correlative Matching
Courtesy: E. Olson
11
Problems
Many matching to be performed
Might be slow if many candidate
locations
Accuracy up to discretizations
Uncertainties slow to compute
12
Point-to-Point Matching (ICP)
Estimate uncertainty of nodes relative
to the current pose
Sample poses in relevant area
Apply Iterative Closest Point algorithm
Evaluate match
Accept match based on a threshold
13
Point-to-Point Matching (ICP)
Given two corresponding point sets:
Wanted: Translation t and rotation R
that minimize:
Here, and are corresponding points
14
Key Idea
If the correct correspondences are
known, the correct rotation/translation
can be calculated in closed form
15
Center of Mass
and
are the centers of mass of the two sets
Idea:
Subtract the center of mass from every
point in the two point sets
and
16
Singular Value Decomposition
Let , we denote the
singular value decomposition (SVD) of
W by:
Where are orthogonal, and
are the singular values
17
SVD
Theorem (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 is:
18
ICP with Unknown Data
Association
If the correct correspondences are not
known, it is generally impossible to
determine the optimal relative rotation
and translation in one step
19
Iterative Closest Point (ICP)
Algorithm
Idea: Iterate to find alignment
Iterative Closest Points
[Besl & McKay 92]
Converges if starting positions are
“close enough”
20
Basic ICP Algorithm
Determine corresponding points
Compute R and t via SVD
Apply R and t to the points of the set
to be registered
Compute the error E(R,t)
If error decreased and > threshold
Repeat these steps
Stop and output final alignment,
otherwise
21
Problems
ICP is sensitive to the initial guess
Local minima
Ambiguities in the environment
22
Feature-Based Matching
Environment abstraction
Indoor (fr-079) Outdoor (Victoria park)
[Courtesy of G. Grisetti] [Courtesy of M. Kaess]
Sensor abstraction
Laser Camera
[Courtesy of K. Mikolajczyk] 23
Feature-Based Matching
Detect salient locations in the data
Describe them with local information
Match the set of features considering
their appearance
Features available
Laser: FLIRT, SHOT, NARF,…
Camera: SIFT, SURF, BRISK, FAST,…
24
Feature Matching (RANSAC)
Matching algorithm robust to outliers
Iteratively perform:
1. Sample a minimal solution set
2. Compute the transformation
3. Compute the inlier set
4. If inlier set > than previous, update
The number of iterations depends on
the dimension of the minimal set
28
RANSAC Iterations
Let q be the probability of an inlier
29
RANSAC Iterations
Let q be the probability of an inlier
30
RANSAC Iterations
Let q be the probability of an inlier
The probability of outliers in the MSS
31
RANSAC Iterations
Let q be the probability of an inlier
The probability of outliers in the MSS
32
RANSAC Iterations
Let q be the probability of an inlier
The probability of outliers in the MSS
The number of iterations is given by
33
Problems
Local minima
Ambiguities in the environment
34
Problems
Local minima
Ambiguities in the environment
Dealing with ambiguous areas in an
environment is essential for robustly
operating robots
35
Ambiguities - Global Ambiguity
B is inside the uncertainty ellipse of A
Are A and B the same place?
B
A
A’s
uncertainty
Courtesy: E. Olson 57
Ambiguities - Global Ambiguity
B is inside the uncertainty ellipse of A
A and B might not be the same place
B
A
?
?
?
A’s
uncertainty
Courtesy: E. Olson 58
Ambiguities - Global Ambiguity
B is inside the uncertainty ellipse of A
A and B are not the same place
B
A
A’s
uncertainty
Courtesy: E. Olson 59
Ambiguities - Global Sufficiency
B is inside the uncertainty ellipse of A
The is no other possibility for a match
B
A
A’s
uncertainty
Courtesy: E. Olson 60
Ambiguities - Local Ambiguity
“Picket Fence Problem”: largely
overlapping local matches
A’s
Courtesy: E. Olson uncertainty 61
Global Match Criteria
1. Global Sufficiency: There is no possible
disjoint match (“A is not somewhere else
entirely”)
2. Local unambiguity: There are no
overlapping matches (“A is either here or
somewhere else entirely”)
Both need to be satisfied for a match
B
A
A’s
Courtesy: E. Olson uncertainty 62
Olson’s Proposal
Prior
Pose Matcher
Pose-to-Pose rigid-
body transformations
Topological
Grouping
SLAM Sets of pose-to-pose
Backend matches relating
nearby poses
Criterion
SCGP 2
Globally consistent and Locally consistent and
unambiguous matches unambiguous matches
(“Loop Closures”)
Global Criterion
1
Ambiguity Test
Courtesy: E. Olson 63
Topological Grouping
Group together topologically-related pose-
to-pose matches to form local matches
Each group asks a “topological” question:
Do two local maps match?
Local Match Group 1
Courtesy: E. Olson Local Match Group 2 64
Locally Unambiguous Matches
Goal:
Unfiltered Local Match Locally consistent and
(set of pose-to-pose matches) unambiguous local match
(set of pose-to-pose matches)
Courtesy: E. Olson 65
Locally Consistent Matches
Correct pose-to-pose hypotheses
must agree with each other
Incorrect pose-to-pose hypotheses
tend to disagree with each other
Find subset of self-consistent of
hypotheses
Multiple self-consistent subsets, are
an indicator for a “picket fence”!
66
Do Two Hypotheses Agree?
Consider two hypotheses i and j in the set:
hi hj
Form a loop using edges from the prior
graph
hi hj
Rigid-body transformation around the
loop should be the identity matrix 67
Idea of Olson’s Method
Form pair-wise consistency matrix A
Hypothesis Set
j
A= i
hi hj
Courtesy: E. Olson 68
Single Cluster Graph
Partitioning
Idea: Identify the subset of consistent
hypotheses
Find the best indicator vector
(represents a subset of the
hypotheses)
Indicator vector v
i j vi = 1 if hi is correct,
0 if hi is incorrect
Courtesy: E. Olson 69
Single Cluster Graph
Partitioning
Identify the subset of hypotheses that
is maximally self-consistent
Which subset v has the greatest
average pair-wise consistency λ?
Sum of all pair-wise consistencies
between hypotheses in v
Number of hypotheses in v
Gallo et al 1989
Densest subgraph problem
Courtesy: E. Olson 70
Consistent Local Matches
We want find v that maximizes λ(v)
Treat as continuous problem
Derive and set to zero
Which leads to (for symmetric A)
71
Consistent Local Matches
: Eigenvalue/vector problem
The dominant eigenvector v1
maximizes
The hypothesis represented by v1
is maximally self-consistent subset
If λ1/λ2 is large (e.g., λ1/λ2>2) then v1
is regarded as locally unambiguous
Discretize v1 after maximization 72
Global Consistency
Correct method: Can two copies of A be
arranged so that they both fit inside the
covariance ellipse?
Approximation: Is the dimension of A at
least half the length of the dominant axis of
the covariance ellipse?
Potential failures for narrow local matches
B
A
Courtesy: E. Olson 73
Conclusions
Matching local observations is used to
generate pose-to-pose hypotheses
Local matches assembled from pose-
to-pose hypotheses
Local ambiguity (“picket fence”) can
be resolved via SCGP’s confidence
metric
Positional uncertainty: more
uncertainty requires more evidence
76
Literature
FLIRT Features
Tipaldi, Arras: “FLIRT -- Interest
Regions for 2D Range Data”
Spectral Clustering
Olson: “Recognizing Places using
Spectrally Clustered Local Matches”
77
Slide Information
These slides have been created by Cyrill Stachniss as part of
the robot mapping course taught in 2012/13 and 2013/14. I
created this set of slides partially extending existing material
of Edwin Olson, Giorgio Grisetti, Bastian Steder, Rainer
Kümmerle, Patrick Pfaff, and myself.
I tried to acknowledge all people that contributed image or
video material. In case I missed something, please let me
know. If you adapt this course material, please make sure
you keep the acknowledgements.
Feel free to use and change the slides. If you use them, I
would appreciate an acknowledgement as well. To satisfy my
own curiosity, I appreciate a short email notice in case you
use the material in your course.
My video recordings are available through YouTube:
http://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_&feature=g-list
Cyrill Stachniss, 2014
[email protected] bonn.de78
