the ekf solution to simultaneous localization and mappingwebdiis.unizar.es/~neira/5007439/vibot 2008...
TRANSCRIPT
1J. Neira, Vibot 2007-2008
The EKF Solution to
Simultaneous Localization and Mapping
José NeiraUniversidad de Zaragoza
2J. Neira, Vibot 2007-2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
3J. Neira, Vibot 2007-2008
Simultaneous Localizationand Mapping
Is it possible to use a vehicle, starting at an
• unknown initial location, in an
• unknown environment, to
• incrementally
build a map of theenvironment,
• and at the same time
use the map to determine thevehicle location?
Chicken and eggproblem?
(image: Paul Newman)
4J. Neira, Vibot 2007-2008
Simultaneous Localizationand Mapping
• There is a large amount of potential applications• It gives the vehicle real autonomy• A solution is indeed possible
The solution to the SLAM problem is, in many respects, a ‘Holy Grail’ of the autonomous vehicle research community, as the ability to build a map and navigate simultaneously would indeed make a robot ‘autonomous’.
(Newman 1999, Leonard 2000, Thrun 2001)
5J. Neira, Vibot 2007-2008
From Wikipedia:
• Simultaneous localization and mapping (SLAM) is a technique used by robots and autonomous vehicles to build up a map within an unknown environment while at the same time keeping track of its current position.
• This is not as straightforward as it might sound due to inherent uncertainties in discerning the robot's relative movement from its various sensors.
• … Some of the statistical techniques used in SLAM include Kalman filters, particle filters and scan matching of range data.
• Pioneering work in this field was conducted by Hugh F. Durrant-Whyte.
• Much of the SLAM work is based on concepts imported from computer vision…
• SLAM has not yet been fully perfected, but it is starting to be employed in unmanned aerial vehicles, autonomous underwater vehicles, planetary rovers and newly emerging domestic robots.
6J. Neira, Vibot 2007-2008
Mobile Robots
7J. Neira, Vibot 2007-2008
Mobile Robots
8J. Neira, Vibot 2007-2008
Mobile Robots
9J. Neira, Vibot 2007-2008
Mobile Sensors
10J. Neira, Vibot 2007-2008
Origins: the localizationproblem
OtilioTriton
Vision
Laser
Sonar
Wheel Encoders
11J. Neira, Vibot 2007-2008
A Simple Example: MonoRob• A robot moves in a mono-dimensional space:
• Odometry:
• Uncertainty model:
Uncertainty in Robot Position
Measured
Error
True value
12J. Neira, Vibot 2007-2008
A Simple Example
• Gaussianity assumption:
(image: Duda, Hart, Stork)
13J. Neira, Vibot 2007-2008
• Odometry error model:
• Example: move 0.9m
Robot Odometry: Example
14J. Neira, Vibot 2007-2008
Robot Odometry: Example• Robot moves again 0.85m:
• New estimation:
It grows!
15J. Neira, Vibot 2007-2008
Odometry
0
0.1
0.2
0.3
0.4
0.5
0.6
1 3 5 7 9
11 13 15 17 19 21 23 25 27 29
Odometry error grows unbounded(with the square root of n)
16J. Neira, Vibot 2007-2008
Vehicle motion in 2D
Composition:
Inversion:
17J. Neira, Vibot 2007-2008
Odometry in 2D
Composition:Odometry model:
18J. Neira, Vibot 2007-2008
Odometry in 2D
Jacobians:
19J. Neira, Vibot 2007-2008
Odometry in 2D
How can we avoid drift?
20J. Neira, Vibot 2007-2008
The need for SLAM
21J. Neira, Vibot 2007-2008
Odometry in 2D
(image: Thrun, Burgard, Fox)
For different rotation and translational errors
22J. Neira, Vibot 2007-2008
Odometry in 2D
(image: Thrun, Burgard, Fox)
We are linearizing errors!
23J. Neira, Vibot 2007-2008
Map-based Localization• Cumulative odometry
errors• A priori Map + Perception
Laser
Vision
J. Neira, J.D. Tardós, J. Horn and G. Schmidt: Fusing Range and Intensity Images for Mobile Robot Localization, IEEE Trans. Robotics and Automation, Vol. 15, No. 1,Feb 1999, pp 76-84.
24J. Neira, Vibot 2007-2008
Multisensor Robot Localization• Results (uncertainty x30)
VisionLaser
Laser + vision
25J. Neira, Vibot 2007-2008
• In many applications the environment is unknown • A priori maps usually are:
–Costly to obtain–Inaccurate–Incomplete–Out of date
The need for SLAM
Map Building
26J. Neira, Vibot 2007-2008
Localization and Mapping elements
(image: Durrant-Whyte)
• M: environment features• U: control inputs• X: vehicle locations• Z: sensor measurements
27J. Neira, Vibot 2007-2008
Map-based localization
(image: Durrant-Whyte)
• Given M, U, Z• Compute X
28J. Neira, Vibot 2007-2008
Mapping
(image: Durrant-Whyte)
• Given X, Z• Compute M
29J. Neira, Vibot 2007-2008
SLAM
(image: Durrant-Whyte)
• Given U, Z• Compute X, M
30J. Neira, Vibot 2007-2008
The need for SLAM
• Stochastic map + Extended Kalman Filter:
• First proposed by:R. Smith and P. Cheeseman, “On the Representation and Estimation of
Spatial Uncertainty”, Int. J. Robotics Research 5(4), pp. 56-68, 1986.R. Smith, M. Self and P. Cheeseman, “A Stochastic Map for Uncertain
Spatial Relationships”, In O. Faugeras and G. Giralt (eds.), Robotics Research, The Fourth Int. Symp., pp. 467-474. The MIT Press, 1988
31J. Neira, Vibot 2007-2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
32J. Neira, Vibot 2007-2008
EKF-SLAM: approach• Environment information related to a set
of elements:
• represented by a stochastic map:
B
Fn
F2
RF1
xFnB
xF1B
xF2B
xRB
33J. Neira, Vibot 2007-2008
Map Features in 2D
Points:
Lines:
34J. Neira, Vibot 2007-2008
EKF-SLAM
35J. Neira, Vibot 2007-2008
Example: SLAM in a cloister
• Red dots: environment features (columns)• Black line: robot trajetory
36J. Neira, Vibot 2007-2008
Sensor measurements• In polar coordinates: • In Cartesian coordinates:
37J. Neira, Vibot 2007-2008
The basic EKF SLAM Algorithms
Sensor measurements
38J. Neira, Vibot 2007-2008
EKF-SLAM: add new features
Linearization:
Where:
39J. Neira, Vibot 2007-2008
EKF-SLAM: add new features
40J. Neira, Vibot 2007-2008
EKF-SLAM: add new features
41J. Neira, Vibot 2007-2008
EKF-SLAM: compute robot motion
42J. Neira, Vibot 2007-2008
EKF-SLAM: compute robot motion
Odometry model (white noise):
EKF prediction:
43J. Neira, Vibot 2007-2008
EKF-SLAM: compute robot motion
44J. Neira, Vibot 2007-2008
EKF-SLAM: Observations
45J. Neira, Vibot 2007-2008
EKF-SLAM: Data association
Predicted map .vs. measurements
46J. Neira, Vibot 2007-2008
EKF-SLAM: Observations
Sensor model (white noise):Measurement equation:
Observations at instant k:
Association Hypothesis (obs. i with map feature ji) :
47J. Neira, Vibot 2007-2008
EKF-SLAM: Observations
Linearization:
48J. Neira, Vibot 2007-2008
Data association
Innovation:
Mahalanobis distance:
Hypothesis test:
49J. Neira, Vibot 2007-2008
EKF-SLAM: map update
State update:
Covariance update:
Filter gain:
50J. Neira, Vibot 2007-2008
EKF-SLAM: map update
51J. Neira, Vibot 2007-2008
Why we do SLAM
Uncertainty still grows!
52J. Neira, Vibot 2007-2008
Dead-reckoning, movingforward
0 50 100 150-1
-0.5
0
0.5
1Vehicle error in x (m)
0 50 100 150-4
-2
0
2
4Vehicle error in y (m)
0 50 100 150-4
-2
0
2
4Vehicle error in theta (deg)
0 50 100 1500
0.5
1
1.5
2x 10
-3 sqrt(det (P))
53J. Neira, Vibot 2007-2008
Good news!
Loop closing reduces uncertainty!
54J. Neira, Vibot 2007-2008
Loop closing in EKF-SLAM
Loop closing reduces uncertainty!
0 20 40 60-1
-0.5
0
0.5
1Vehicle error in x (m)
0 20 40 60-1
-0.5
0
0.5
1Vehicle error in y (m)
0 20 40 60-10
-5
0
5
10Vehicle error in theta (deg)
0 20 40 600
2
4
6
8x 10
-4 sqrt(det(P))
55J. Neira, Vibot 2007-2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
56J. Neira, Vibot 2007-2008
Feature extraction: Laser
• Obtain line segments from a laser scan:
– Segmentation– Line estimation
Split and merge:1. Recursive Split:
1. Obtain the line passing by the two extreme points
2. Obtain the point more distant to the line
3. If distance > error_max, split and repeat with the left and right sub-scan
2. Merge:1. If two consecutive
segments are close enough, obtain the common line and the more distant point
2. If distance <= error_max, merge both segments
3. Prune short segments4. Estimate line equation
57J. Neira, Vibot 2007-2008
Split and Merge
Split
Split
Split
No more Splits
Merge
58J. Neira, Vibot 2007-2008
....
Less than 8cm
Split and Merge
59J. Neira, Vibot 2007-2008
Between-segments distance < 10cm
Less than 6 pixels
Split and Merge• Elimination of small segments:
• Segment fusion:
60J. Neira, Vibot 2007-2008
• Problematic case:
• Split and merge: only the extreme points are used• Alternatives:
– Linear regression– RANSAC– Hough transform
Split and Merge
61J. Neira, Vibot 2007-2008
Split and Merge
Not robust to complex and/or spurious data
62J. Neira, Vibot 2007-2008
RANSAC• Given a model that requires n data points to
compute a solution and a set of data points P, with #(P) > n :–Randomly select a subset S1 of n data points
and compute the model M1–Determine the consensus set S1* of points is
P compatible with M1 (within some error tolerance)
–If #(S1*) > th, use S1* to compute (maybe using least squares) a new model M1*
–If #(S1*) < th, randomly select another subset S2 and repeat
–If, after t trials there is no consensus set with th points, return with failure
63J. Neira, Vibot 2007-2008
RANSAC
p no. of pointsn points to build
modelw probability that
a point is good
z acceptable probabilityof failure
w 0,5n 2z 0,05t 11
0 0.2 0.4 0.6 0.8 1
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
t tries ¿?
64J. Neira, Vibot 2007-2008
RANSAC
S & MTLS RANSAC RANSAC+TLS
65J. Neira, Vibot 2007-2008
RANSAC
Robust statistics deal with spuriousness
66J. Neira, Vibot 2007-2008
RANSAC for 3D planes
P.M. Newman, J.J. Leonard, J. Neira and J.D. Tardós: Explore and Return: Experimental Validation of Real Time Concurrent Mapping and Localization. IEEE Int. Conf. Robotics and Automation, May, 2002
67J. Neira, Vibot 2007-2008
Sonar
−4 −2 0 2 4 6 8 10−4
−2
0
2
4
6
8
10Laser scan Sonar returns
Very sparse and noisy data
68J. Neira, Vibot 2007-2008
Move and build a local map
Use a good sensor modelExploit redundancy
−2 −1 0 1 2 3 4 5−3
−2
−1
0
1
2
3Sonar data
−2 −1 0 1 2 3 4 5−3
−2
−1
0
1
2
3Sonar data
Wall!
Corner!
People!
69J. Neira, Vibot 2007-2008
Sj
θkSjρS
j
βρB
i
θkBi ySj
BixSj
Bi
Apparent Sonar Return
Possible Point
Bi
Hough Votingfor i in 1..n_positions
for j in 1..n_sensorsCompute xBi
Sjfor θSj
k in -β/2..β/2 step δCompute θBj
k ρBjk
Vote(θBjk, ρBj
k)end
endend
Sonar Model for Points
70J. Neira, Vibot 2007-2008
0
5
10
15
20
25
30
35
40
45
50
Ro (m)
Thet
a (d
eg)
Hough Table for Points
1
2
0 1 2 3 4 5
-150
-100
-50
0
50
100
150
Hough Transform: Corners• Sonar returns vote for points• Look for local maxima
−2 −1 0 1 2 3 4 5−3
−2
−1
0
1
2
3Point Groups
1
2
71J. Neira, Vibot 2007-2008
Apparent Sonar Return
Sj
θkSj
ρSj
β
ρBi
θkBi ySj
BixSj
Bi
Possible Line
Bi
Hough Votingfor i in 1..n_positions
for j in 1..n_sensorsCompute xBi
Sjfor θSj
k in -β/2..β/2 step δCompute θBj
k ρBjk
Vote(θBjk, ρBj
k)end
endend
Sonar Model for Lines
72J. Neira, Vibot 2007-2008
0
5
10
15
20
25
30
35
40
45
50
Ro (m)
Thet
a (d
eg)
Hough Table for Lines
1
2
0 1 2 3 4 5
-150
-100
-50
0
50
100
150
Hough Transform: Lines
• Sonar returns vote for lines• Look for local maxima
−2 −1 0 1 2 3 4 5−3
−2
−1
0
1
2
3Line Groups
1
2
The Hough gives robust local data associations
73J. Neira, Vibot 2007-2008
Hough Transform• Imaging sonar
74J. Neira, Vibot 2007-2008
Hough Transform
D. Ribas, P. Ridao, J. Neira, J.D. Tardós, SLAM using an Imaging Sonar for Partially Structured Underwater Environments, The 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.
1J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
2J. Neira, VIBOT, 2008
EKF-SLAM
3J. Neira, VIBOT, 2008
» Red dots: environment features (columns)» Black line: robot trajectory» Black semicircle: sensor range
Example: SLAM in a cloister
4J. Neira, VIBOT, 2008
The Data Association Problem
• n map features:
• m sensor measurements:
• Data association should return a hypothesis that associates each observation Ei with a feature Fji
• Non matched observations:
5J. Neira, VIBOT, 2008
The Correspondence Space
Interpretation tree (Grimson et al. 87):
......
...... ......
......
......
......
possible hypotheses
Green points: measurementsBlue Points: predicted features
6J. Neira, VIBOT, 2008
When data association is difficult• Low sensor error • High sensor error
7J. Neira, VIBOT, 2008
When data association is difficult• Low odometry error • High odometry error
8J. Neira, VIBOT, 2008
When data association is difficult• Low feature density • High feature density
9J. Neira, VIBOT, 2008
How important is data association?
A good algorithm A bad algorithm
10J. Neira, VIBOT, 2008
Why it’s difficult?
A good algorithm A bad algorithm
11J. Neira, VIBOT, 2008
Importance of Data Association• EKF update:
• If the association of Ei with feature Fj is…..
error:
covariance:
Divergence!Consistency
correct: spurious:
Values that depend on
12J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
13J. Neira, VIBOT, 2008
Individual Compatibility• Measurement equation for observation Ei and
feature Fj
• Ei and Fj are compatible if:
d = length(zi)
14J. Neira, VIBOT, 2008
Nearest Neighbor
......
...... ......
......
......
Greedy algorithm: O(mn)
15J. Neira, VIBOT, 2008
The Fallacy of the Nearest Neighbor
Unrobust
16J. Neira, VIBOT, 2008
Joint Compatibility
• Given a hypothesis• Joint measurement equation
• The joint hypothesis is compatible if:
d = length(z)
17J. Neira, VIBOT, 2008
Joint Compatibility Branch and Bound
• Find the largest hypothesis with jointly consistent pairings
Selects the largest set of pairingswhere there is consensus
18J. Neira, VIBOT, 2008
The Fallacy of the Nearest Neighbor
A
B
J. Neira, J.D. Tardós. Data Association in Stochastic Mapping usingthe Joint Compatibility Test IEEE Trans. Robotics and Automation, Vol. 17, No. 6, Dec 2001, pp 890 –897
19J. Neira, VIBOT, 2008
Nearest neighbor .vs. Joint Compatibility
NN JCBB
20J. Neira, VIBOT, 2008
SLAM without odometry• No estimation of the
vehicle motion• Segments in the
environment
21J. Neira, VIBOT, 2008
SLAM without odometry
x
y
x
y
x
y
x
y
x
y
x
y
x
y
x
y
Assuming small motions
22J. Neira, VIBOT, 2008
SLAM without odometry
x
y
x
y
x
y
x
y
x
y
x
y
x
y
x
y
Data association using Joint Compatibility
23J. Neira, VIBOT, 2008
SLAM without odometry
x
y
x
y
x
y
x
y
x
y
x
y
x
y
Vehicle motion estimation
24J. Neira, VIBOT, 2008
SLAM without odometry
25J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
26J. Neira, VIBOT, 2008
Loop closing in mosaicinguse first
Joint work with R. García, University of Girona
27J. Neira, VIBOT, 2008
Loop closing in mosaicing:use Last
Sequential mosaicing is a form of odometry
28J. Neira, VIBOT, 2008
The loop closing problem• Loop beginning • Loop end
29J. Neira, VIBOT, 2008
The loop closing problem
30J. Neira, VIBOT, 2008
The loop closing problem
-100 -50 0 50 100 150 200 250 300 350
-150
-100
-50
0
50
100
150
200
Measurements (red) and predicted features (blue)
31J. Neira, VIBOT, 2008
The loop closing problem
• Individual compatibility • Joint Compatibility
0 20 40 60 80 100 120 140 160 180
-50
0
50
100
0 20 40 60 80 100 120 140 160 180
-50
0
50
100
32J. Neira, VIBOT, 2008
The loop closing problem
33J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
34J. Neira, VIBOT, 2008
Global Localization• Robot placed in a previously mapped environment
http://www.acfr.usyd.edu.au/homepages/academic/enebot/victoria_park.htm
35J. Neira, VIBOT, 2008
Problem Definition • On-board sensor obtains m measurements:
http://www.acfr.usyd.edu.au/homepages/academic/enebot/victoria_park.htm
36J. Neira, VIBOT, 2008
Problem Definition
• Twofold question:– Is the vehicle in
the map?– If so, where?
http://www.acfr.usyd.edu.au/homepages/academic/enebot/victoria_park.htm
37J. Neira, VIBOT, 2008
Global Localization algorithms
• Correspondence space– Consider consistent combinations of measurement-
feature pairings.» Branch and Bound (Grimson, 1990)» Maximum Clique (Bailey et. al. 2000)» Random Sampling (Neira et. al. 2003)
• Configuration space– Consider different vehicle location hypotheses.
» Monte Carlo Localization (Fox et. al. 1999)» Markov Localization (Fox et. al. 1998)
38J. Neira, VIBOT, 2008
In correspondence space
39J. Neira, VIBOT, 2008
No vehicle location• Unary constraints:
depend on a single matching (size, color,…)
–Trees: trunk diameter
–Walls: length, corners: angle....
OK
61 714 354 176 000 valid hypotheses
40J. Neira, VIBOT, 2008
No vehicle location• Binary constraints:
distances betweenpoints:
0 100 200 300 400 500 600
100
200
300
400
500
600
700
800
900
41J. Neira, VIBOT, 2008
Locality• Features Fi and Fj may belong to the same
hypotesis iff they are ‘close enough’.
xyxyxyxyxyxy
xy
xyS1
S2S3
S4
S5
S6S7xy
xyS1
S2S3
S4
S5
S6S7
0 10 20 30 40 50 60 70
0
10
20
30
40
50
60
70
nz = 755
Covisibilitymatrix
Limit search in the map to subsets of covisible featuresLocality makes search linear with the global map size
42J. Neira, VIBOT, 2008
Algorithm 1: Geometric Constraints Branch and Bound (Grimson, 1990)
......
...... ......
......
......
......
X X
XX
43J. Neira, VIBOT, 2008
Algorithm 2: Maximum Clique
• All unary and binary constraints can be precomputed
• Build a compatibility graph where:– Nodes represent unary compatible pairings– Arcs represent pairs of binary compatible pairings
• Carrahan, Pardalos (1990)
13
22
32
41
44J. Neira, VIBOT, 2008
Algorithm 3: Generation Verification
......
...... ......
......
......
......
X X
XX
Verification
Generation
45J. Neira, VIBOT, 2008
Algorithm 4: RANSAC
......
...... ......
......
......
......
X X
X X
46J. Neira, VIBOT, 2008
mean time .vs. m
Experiments1. No significant difference in effectiveness of the
considered algorithms2. All algorithms are made linear with the size of
the map3. Efficiency when the vehicle is in the map
47J. Neira, VIBOT, 2008
Experiments4. When the vehicle is NOT in the map:
random measurements, 100 for each m=3..30
mean time .vs. m
J. Neira, J.D. Tardós, J.A. Castellanos, Linear time vehicle relocation in SLAM.IEEE Int. Conf. Robotics and Automation, Taipei, Taiwan, May, 2003
48J. Neira, VIBOT, 2008
In configuration space
49J. Neira, VIBOT, 2008
In Configuration Space:RANDOM sampling
• Consider s randomly chosen vehicle locationshypotheses:
• Monte CarloLocalization
50J. Neira, VIBOT, 2008
Alternative 1: location-driven
• Consider each alternative location hypothesis in turn
51J. Neira, VIBOT, 2008
In Configuration Space:GRID sampling
• Uniformly tessellate the space in s = nx ny nφ gridcells:
• MarkovLocalization
52J. Neira, VIBOT, 2008
Voting in configuration space• Each measurement-feature pairing constrains the
set of possible vehicle locations:
53J. Neira, VIBOT, 2008
Alternative 2: pairing-driven
• Consider each measurement-feature pairing in turn
2
54J. Neira, VIBOT, 2008
Results– Resolution: 1.5m for x and y, and 1º for theta
55J. Neira, VIBOT, 2008
Computational Complexity
• How do they compare?
• Pair_driven is better in proportion to the density of features in the environment.
• Victoria Park: 18321 sq. m.,sample every 1.5m, expect pair_driven to be 82 times faster.
56J. Neira, VIBOT, 2008
Voting for map basedlocalization
Ribas D., Palomeras, N. Hernandez, E., Ridao, P., Carreras, M. ICTINEU AUV Wins the First SAUC-E Competition2007 IEEE International Conference on Robotics and Automation, Rome, Italy.
57J. Neira, VIBOT, 2008
Conclusions
• A solution to SLAM is indeed possible• Data association: algorithms based on some form
of consensus provide the best results
»Joint Compatibility»RANSAC»Hough Transform
• What happens at a large scale?
But…
1J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
2J. Neira, VIBOT, 2008
The EKF SLAM algorithm
3J. Neira, VIBOT, 2008
Without loss of generality...• Environment to be mapped has more or less
uniform density of features
4J. Neira, VIBOT, 2008
Without loss of generality...• Enviroment to be mapped has more or less
uniform density of features
5J. Neira, VIBOT, 2008
Without loss of generality...• Onboard range and bearing sensor obtains m
measurements
6J. Neira, VIBOT, 2008
Without loss of generality...• Vehicle performs an exploratory trajectory, re-
observing r features, and seeing s = m – r new features.
7J. Neira, VIBOT, 2008
The prediction step
8J. Neira, VIBOT, 2008
EKF SLAM prediction
9J. Neira, VIBOT, 2008
EKF SLAM: prediction
EKF prediction is O(n)
O(1)
O(n)
10J. Neira, VIBOT, 2008
Adding new features
11J. Neira, VIBOT, 2008
EKF SLAM: add new features
Linearization:
Where:
12J. Neira, VIBOT, 2008
EKF SLAM: add new features
Adding new features is O(n) O(s)
O(n)
13J. Neira, VIBOT, 2008
The update step
14J. Neira, VIBOT, 2008
EKF SLAM: map updatem observations:
State update:Covariance update:
Filter gain:Innovation Matrix:
15J. Neira, VIBOT, 2008
The innovation matrix
rxr rxn nxn nxr rxr
O(n²) ?
rn² r²n r²
rxn
rxr
rxr
( )
16J. Neira, VIBOT, 2008
The innovation matrix
O(rn)
rxr rxc nxn cxr rxr( )
17J. Neira, VIBOT, 2008
The Kalman gain matrix
nxnnxr rxrnxc
O(r²n)
18J. Neira, VIBOT, 2008
The covariance matrix
nxnnxr cxn
…..…..
…..…..
EKF update step is O(n²)
19J. Neira, VIBOT, 2008
The mixed blessing of Covariance
• Covariance provides data association• But the covariance matrix is full
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
20J. Neira, VIBOT, 2008
EKF-SLAM updates are O(n2)
50 100 150 200 2500
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
Steps
Tim
e (s
eg)
Mean Time by Local Maps
1 local maps
21J. Neira, VIBOT, 2008
Total EKF SLAM is O(n3)
• The total cost of computing a map os size n, withm observations per step, with s=m-r new features per step:
22J. Neira, VIBOT, 2008
Total cost of EKF-SLAM
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
Total SLAM is O(n3)
23J. Neira, VIBOT, 2008
Continuous data association
Individual compatibility is O(nm) = O(n)
24J. Neira, VIBOT, 2008
Map tessellation
Individual compatibility can be O(1)
25J. Neira, VIBOT, 2008
Global localization:Computational Complexity
Global localization is O(nm) = O(n)
26J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
27J. Neira, VIBOT, 2008
Consistency of EKF-SLAM
• Nice “convergence” properties of(Dissanayake et al. 2001):– Landmark covariance decreases monotonically– In the limit, landmarks become fully
correlated– In the limit, landmark covariance reaches a
lower bound related to the initial vehicle covariance
• But SLAM is a non-linear problem– The inherent approximations due to
linearizations can lead to divergence (inconsistency) of the EKF » see for example (Jazwinski, 1970)
28J. Neira, VIBOT, 2008
EKF-SLAM: Robot Motion
Odometry model (white noise):
EKF prediction:
Linearization
29J. Neira, VIBOT, 2008
EKF-SLAM: Map Update
Feature observations:
EKF map update:
Linearization
30J. Neira, VIBOT, 2008
The Consistency Problem
• An estimator is consistent if:
– Pessimistic covariance is OK (but not too pessimistic)– Optimistic covariance = Inconsistency = Filter
divergence
True map valueEKF-SLAMestimation
UnbiasedThe Mean SquareError matches the fil-ter computed Cova-riance
31J. Neira, VIBOT, 2008
1. Normalized Estimation Error Squared NEES
2. Innovation test (observation i map feature j)
Consistency Testing
True map requiredSimulations
Critical whenclosing big
loops
32J. Neira, VIBOT, 2008
EKF-SLAM: Real Example
33J. Neira, VIBOT, 2008
EKF-SLAM: Real Example
Inconsistent! Standard data associationcannot close a 250m loop
Computational complexity was not a problem here!
34J. Neira, VIBOT, 2008
EKF-SLAM: Simulation
• Simulation conditions– Perfect data association– Ideal odometry and measurement noise
» white, Gaussian, known covariance
• Advantages of simulation:– Consistency can be tested against the true
map– A simulation with noise=0 gives the
theoretical map covariance (without linearization errors)
35J. Neira, VIBOT, 2008
EKF-SLAM: SimulationPerfect data associationand noise model
36J. Neira, VIBOT, 2008
EKF-SLAM: Covariance
Initial uncertainty = 0 Initial uncertainty > 0
Optimistic Violates lower bound
J.A. Castellanos, J. Neira, J.D. Tardós, Limits to the Consistency of EKF-based SLAM, 5th IFAC Symposium on Intelligent Autonomous Vehicles, Lisbon, July 2004
37J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
38J. Neira, VIBOT, 2008
Local submaps
39J. Neira, VIBOT, 2008
Local map building• Periodically, the robot
starts a new map, relative to its currentlocation:
• Given measurements:
• EKF approximates theconditional mean:
-2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3 3.5-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3
t= 33.700s
x
y
x
y
x
y
x y
P1
xy P2
x
y
S3
x
y
S4
B1 R1
40J. Neira, VIBOT, 2008
Local map building• Second map:
• No information isshared:
• Common reference:
-2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3 3.5-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3 t= 48.492s
xy
x
yx
y
x
yS1
x
y
S2
B2 R2 Maps are uncorrelated
41J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
42J. Neira, VIBOT, 2008
Map Joining: Example
-2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3 3.5-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3 t= 48.492s
xy
x
y
xy
x y
P1
xyP2
x
yS3
x
y
S4B1 R2
-2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3 3.5-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3t= 33.700s
x
yx
y
x
y
x y
P1
xy P2
x
yS3
x
y
S4B1
R1
-2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3 3.5-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3 t= 48.492s
xy
x
y xy
x
yS1
x
y
S2
B2 R2
-2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3 3.5-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3 t= 48.492s
x
y
x y
P1
xy
x
y
x
y xy
x
y
xy
x y
P1
xy P2
x
y
x
y xy
x
y
xy
x
y
x
y xy
x
y
xy
x
y
S5
x
y
S6
xy
x
y
xy
x
y
S3
x
yS4B1 R2
Joined map After matching and fusionJ.D. Tardós, J. Neira, P.M. Newman and J.J. Leonard, Robust Mapping and Localization in Indoor Environments using Sonar Data, The Int. Journal of Robotics Research, Vol. 21, No. 4, April, 2002, pp 311 –330
43J. Neira, VIBOT, 2008
Map Joining
44J. Neira, VIBOT, 2008
Map Joining Step• New state vector:
• New covariance matrix:
Map joining is O(n²2) on the maps
n1xn1
n2xn2
nxn
n = n1+n2
45J. Neira, VIBOT, 2008
Matching and Fusion Step• Matching function:
• Joint matching function for the hypothesis:
• Map update using EKF:
46J. Neira, VIBOT, 2008
The innovation matrix
O(rn)
rxr rxc nxn cxr
47J. Neira, VIBOT, 2008
The gain matrix
nxr cxr rxrnxn
O(r²n)
48J. Neira, VIBOT, 2008
The update
nxr rxc nxn
…..…..
…..…..
Matching and fusion is O(n2)
49J. Neira, VIBOT, 2008
Cost of Map Joining per step
50 100 150 200 2500
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
Tim
e (s
eg)
Mean Time by Local Maps
1 local maps
8 local maps
50J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
51J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
52J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
53J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
54J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
55J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
56J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
57J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
There is an optimal submap size
58J. Neira, VIBOT, 2008
Optimal local map size
0 50 100 150 200 2500
2
4
6
8
10
12
14
16
Local Map Size p
Tim
e (s
eg)
Mean Final Time
p = 270N = 1
p = 141N = 2
p = 77N = 4
p = 44N = 8
p = 28N = 16
p = 20N = 32
p = 16N = 64
p = 14 N = 128 Local Map Size p
59J. Neira, VIBOT, 2008
EKF-SLAM: remember?
Inconsistent!
60J. Neira, VIBOT, 2008
Map Joining closes the loop!
-60 -40 -20 0 20 40 60 80
-90
-80
-70
-60
-50
-40
-30
-20
-10
0
10
t= 0.000s
xyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxy
xy
Speedx50
61J. Neira, VIBOT, 2008
Montecarlo Simulations
• Simulation conditions– Perfect data association– Ideal odometry and measurement noise
» white, Gaussian, known covariance
• Advantages of simulation:– Consistency can be tested against the true
map– A simulation with noise=0 gives the
theoretical map covariance (without linearization errors)
– Run several algorithms using the same data– Carry out a large amount of runs
62J. Neira, VIBOT, 2008
1. Define the consistency index CI as:
2. When CI < 1, the estimation is consistent with ground truth
3. When CI > 1, the estimation is inconsistent
Consistency Testing
63J. Neira, VIBOT, 2008
Mean map consistency
0 50 100 150 200 2500.94
0.96
0.98
1
1.02
1.04
1.06
1.08
1.1
1.12
p = 270N = 1
p = 141N = 2
p = 77N = 4 p = 44
N = 8
p = 28N = 16
p = 20N = 32
p = 16N = 64
p = 14 N = 128
Local Map Size p
Inde
x va
lue
Map final Consistency index by local maps
Map final ConsistencyBound
64J. Neira, VIBOT, 2008
Mean feature location
65J. Neira, VIBOT, 2008
Mean feature location
66J. Neira, VIBOT, 2008
Map Joining is a ’more for less’
algorithm:
• It improves the consistency of the resulting map• It reduces the computational cost
Limitations:• Map joining is still O(n2)• Environment size must be known
Overcoming limitations:• Can we have EKF-SLAM updates in O(n)?
67J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction4. Data Association
2. The Data Association Problem1. Continuous Data Association2. The Loop Closing Problem3. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
68J. Neira, VIBOT, 2008
D&C: Map Hierarchy• Tree of local maps of half the size their ancestors• Leaves: m=n/P local maps of limited size P
n
n/2 n/2
n/4 n/4n/4 n/4
2p
p p
4p
2p 2p
p pp p m local maps
m/2 joins
4 joins
2 joins
1 join, 1 resulting map
69J. Neira, VIBOT, 2008
Map Hierarchy
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
Number of Maps : 2
x position(m)
y po
sitio
n(m
)
70J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 1
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
71J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 2
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
72J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 3
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
73J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 2
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
74J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 1
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
75J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 2
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
76J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 3
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
77J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 2
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
78J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 3
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
79J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 4
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
80J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 3
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
81J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 2
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
82J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 1
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
83J. Neira, VIBOT, 2008
Divide and Conquer SLAM• m maps of size P, cost O(P3) each• joined into m/2 maps of size 2P, cost O((2P)2)• joined into m/4 maps of size 4P, cost O((4P)2)• … joined into 1 local map of size n, cost O(n2)
84J. Neira, VIBOT, 2008
Divide and Conquer SLAM• This is a geometric progression of the type:
• Thus in this case:
85J. Neira, VIBOT, 2008
Total cost of D&C SLAM
86J. Neira, VIBOT, 2008
Computational cost per step
50 100 150 200 2500
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
Step
Tim
e (s
)EKFD&C
O(n2)
O(n2)
87J. Neira, VIBOT, 2008
Computational cost per step
Step: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 …
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 …
22 22 22 22 22 22 22 22 …
42 42 42 42 …
82 82 …
162 …
88J. Neira, VIBOT, 2008
Computational cost per step
Not needed until…
Step: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 …
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 …
22 22 22 22 22 22 22 22 …
42 42 42 42 …
82 82 …
162 …
89J. Neira, VIBOT, 2008
Computational cost per step
Step: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 …
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 …
22 22 22 22 22 22 22 22 …
42 42 42 42 …
82 82 …
162 …
90J. Neira, VIBOT, 2008
Amortized cost per step
Step: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 …
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 …
2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 …
4 4 4 4 4 4 4 4 4 4 4 4 4 …
8 8 8 8 8 8 8 8 8 …
16
• First idea:
91J. Neira, VIBOT, 2008
Amortized cost per step
Step: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 …
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 …
2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 …
4 4 4 4 4 4 4 4 4 4 4 4 4 …
8 8 8 8 8 8 8 8 8 …
16
• First idea:
92J. Neira, VIBOT, 2008
Amortized cost per step
Step: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 …
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 …
4 4 4 4 4 4 4 …
8 8 8 8 8 8 …
16 16 16 16 …
• Better idea:
93J. Neira, VIBOT, 2008
• The full map can be recovered at any time in a single O(n2)step
• But no part of the algorithm or process requires it
50 100 150 200 2500
0.1
0.2
0.3
0.4
0.5
0.6
Step
Tim
e (s
)
Amortized cost per step
D&C SLAM is O(n) per step
EKFD&C
O(n2)
O(n)
94J. Neira, VIBOT, 2008
Consistency: LOOP
95J. Neira, VIBOT, 2008
Consistency: LAWN
96J. Neira, VIBOT, 2008
Consistency : SNAIL
97J. Neira, VIBOT, 2008
Consistency: Montecarlo runsmean consistency
D&C SLAM is always more consistent
50 100 150 200 2500
0.5
1
1.5
2
2.5
3
3.5
4
steps
Inde
x
EKFD&CBound
98J. Neira, VIBOT, 2008
6DOF SLAM with stereo
L. Paz, P. Piníes, J. Neira and J.D. Tardós Large Scale 6DOF SLAM with Stereo-in-Hand. Conditionally accepted, IEE Transactions on Robotics, 2008.
99J. Neira, VIBOT, 2008
6DOF SLAM with stereo
L. Paz, P. Piníes, J. Neira and J.D. Tardós Large Scale 6DOF SLAM with Stereo-in-Hand. Conditionally accepted, IEE Transactions on Robotics, 2008.
100J. Neira, VIBOT, 2008
6Dof Stereo SLAM, outdoors
150 m loop
101J. Neira, VIBOT, 2008
6DOF SLAM with stereo
102J. Neira, VIBOT, 2008
6Dof Stereo SLAM, indoors
180 m loop
103J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction4. Data Association
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
104J. Neira, VIBOT, 2008
Hierarchical SLAM• Global level: adjacency graph and relative
stochastic map
• Local level: statistically independent local maps
B0
B1
B3B2X1 X2
X3
B0
B1
B2B3
X1 X2
X3
105J. Neira, VIBOT, 2008
Hierarchical SLAM• Local maps:
• Global relative map:
Block diagonal
106J. Neira, VIBOT, 2008
Hierarchical SLAM• Global relative map before loop closing:
• After loop closing:
107J. Neira, VIBOT, 2008
-60 -40 -20 0 20 40 60-100
-80
-60
-40
-20
0
Imposing loop constraints
108J. Neira, VIBOT, 2008
Nonlinear constrainedoptimization
• Minimize corrections to the global map, subject tothe loop constraint:
• Sequential Quadratic Programming (SQP) :
» Iterate until convergence
109J. Neira, VIBOT, 2008
Nonlinear constrainedoptimization
• A more efficient version:
» Iterate until convergence
• Complexity:– P0 is block diagonal– Hi is sparse with nonzeros only for the maps in the loop– The iteration is linear with the number of maps in the
loop
• Convergence:– Converges in 2 or 3 iterations (for loops around 300m)– For bigger errors, may it converge to a local minimum ??
110J. Neira, VIBOT, 2008
Nonlinear constrainedoptimization
• Generalization to closing several loopssimultaneously:
111J. Neira, VIBOT, 2008
Iterated Extended KalmanFilter
• Jacobian of the measurement function:
• Iterated EKF equations:
• With exact loop constraint, z = 0 and Pz = 0, IEKFis equivalent to nonlinear optimization with SQP
C. Estrada, J. Neira, J.D. Tardós, Hierarchical SLAM: real-time accurate map-ping of large environments. To appear in the IEEE Transactions on Robotics
112J. Neira, VIBOT, 2008
Experiment
L. Clemente, A. Davison, I. Reid, J. Neira and J.D. TardósMapping Large Loops with a Single Hand-Held Camera2007 Robotics: Science and Systems, June 27-30, Atlanta, USA
113J. Neira, VIBOT, 2008
Keble College, Oxford
114J. Neira, VIBOT, 2008
115J. Neira, VIBOT, 2008
Sequence of local maps
116J. Neira, VIBOT, 2008
Sequence of local maps
117J. Neira, VIBOT, 2008
Loop closing
118J. Neira, VIBOT, 2008
Keble College, Oxford (290m)
119J. Neira, VIBOT, 2008
Results• Local Map building in real-time @30Hz
– 60 features per map using inverse depth– Bigger maps if converted to (x,y,z)
• Joint Compatibility search adds only 2ms in the worst case
• Map-to-map matching in 1s (in Matlab)– With a new algorithm based on graph theory
• Loop optimization takes 800ms (6 iterations)
• The scale drifts along the map
120J. Neira, VIBOT, 2008
Conclusions
future workexcellentoverkillHierarchical SLAMRelocation
---weakexcellentMap JoiningJoint Compatib.
------inconsistent
very goodEKF-SLAMJoint Compatib.
------inconsistent
weakEKF-SLAMNearest Neighbor
Longer loopLoop 300mLoop 30m
121J. Neira, VIBOT, 2008
Recommended Readings• J.D. Tardós, J. Neira, P. Newman, and J. Leonard. Robust Mapping and
Localization in Indoor Environments using Sonar Data, Int. J. RoboticsResearch, Vol. 21, No. 4, April 2002, pp 311 –330
• J.A. Castellanos, J. Neira, J.D. Tardós, Limits to the Consistency of EKF-basedSLAM, 5th IFAC Symposium on Intelligent Autonomous Vehicles, Lisbon, July 2004.
• C. Estrada, J. Neira, J.D. Tardós, Hierarchical SLAM: real-time accurate mappingof large environments. To appear in the IEEE Transactions on Robotics
• Grimson, W. E. L., Object Recognition by Computer: The Role of Geometric Constraints, The MIT Press, Cambridge, Mass., 1990
• José A. Castellanos and Juan D. Tardós, Mobile Robot Localization and Map Building: A Multisensor Fusion Approach, Kluwer Ac. Pub., Boston, 1999
• J. Neira and J.D. Tardós, Data Association in Stochastic Mapping Using the Joint Compatibility Test, IEEE Trans. Robotics and Automation, vol. 17, no. 6, pp. 890-897, Dec 2001.
• P.M. Newman, J.J. Leonard, J. Neira and J.D. Tardós: Explore and Return: Experimental Validation of Real Time Concurrent Mapping and Localization. IEEE Int. Conf. Robotics and Automation, May, 2002.
• J. Neira, J.D. Tardós, J.A. Castellanos, Linear time vehicle relocation in SLAM. 2003 IEEE Int. Conf. Robotics and Automation, Taipei, Taiwan, May, 2003.
• D. Ortín, J. Neira, J.M.M. Montiel, Relocation using Laser and Vision. 2004 IEEE Int. Conf. Robotics and Automation, New Orleans, USA, April, 2004.
122J. Neira, VIBOT, 2008
INFORMATION
1st Summer School 2002, Stockholm
http://www.cas.kth.se/SLAM/
2nd Summer School 2004, Toulouse
http://www2.laas.fr/SLAM/
3rd Summer Schhol 2006, Oxford
http://www.robots.ox.ac.uk/~SSS06/
123J. Neira, VIBOT, 2008
BIBLIOGRAPHY