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