AI Robotics
4. Simultaneous Localization and
Mapping (SLAM)
Pose Tracking, Graph-Based SLAM
Alexander Kleiner
Dr. A. Kleiner
AI Robotics - SLAM
1
Outline
• The SLAM problem
• Pose Tracking
– Scan Matching
– Visual Odometry
• Graph-Based SLAM
Dr. A. Kleiner
AI Robotics - SLAM
2
The SLAM Problem
We consider a mobile robot
exploring unknown terrain, such as
a city destroyed by an earth quake
We record robot actions …
e.g., control commands, such as turning
velocities of the right and left track
… and sensor readings
such as laser readings, features from camera etc.
•The SLAM problem is to estimate
– Map of features
– Path of the robot
Dr. A. Kleiner
AI Robotics - SLAM
3
The SLAM Problem
• SLAM is a chicken-or-egg problem
– We need a map for localizing the robot
– We need a good pose estimate for building the map
• We distinguish here two levels of solvers:
– Pose Tracking SLAM (The Front End)
• Estimates most recent pose on the map, e.g., Scan
Matching, Visual Odometry, …
• Typically applied online / incremental
– Graph-Based SLAM (The Back End)
• Uses “Pose Graph” from pose tracking as input
• Estimates the entire path and map by a global
optimizer, e.g., Lu-Milios, Least Squares, …
• Typically applied offline / batch
Dr. A. Kleiner
AI Robotics - SLAM
4
Why not using GPS ?
Off-track
error 5.0
meters
Multi-path
propagation !
Off-track
error 3.5
meters
Dr. A. Kleiner
AI Robotics - SLAM
5
Example Maps POSE-TRACKING SLAM
Freiburg Campus
(Google Maps)
Dr. A. Kleiner
Map from odometry
AI Robotics - SLAM
Scan matching online while
driving with 1.5 m/s
6
Example Maps GRAPH-BASED SLAM
University of Freiburg
3D map of the Stanford parking garage
Hospital of
Freiburg
Dr. A. Kleiner
AI Robotics - SLAM
7
Outline
• The SLAM problem
• Pose Tracking
– Scan Matching
– Visual Odometry
• Graph-Based SLAM
Dr. A. Kleiner
AI Robotics - SLAM
8
Pose Tracking for USAR?
Wheel odometry works well on flat
surfaces
Wheel revolution provides sufficient evidence of
motion. However, slippage might be a problem
How about odometry
from tracks?
“Skid steering” causes very
noisy readings of track
revolutions
Dr. A. Kleiner
AI Robotics - SLAM
9
Scan Matching
• A scan is a set of n beam measurements
• A laser scanner mounted on a robot can
continuously take scans of the surroundings
while the robot moves (~70 Hz)
The goal is to compute the displacement of the robot given
successive scans zt, zt+1 and the control inputs ut
Dr. A. Kleiner
AI Robotics - SLAM
10
Scan Matching
Two successive scans
after a rotation and
translation of the
robot
Two scans aligned by scan
matching ! yielding the
rotational and translational
offset
Better: instead of comparing only two scans, we can also
compare against an accumulation of scans !a grid map !
Dr. A. Kleiner
AI Robotics - SLAM
11
Scan Registration on a grid
1. Maximize the likelihood of the current pose relative
to the previous pose and map:
xˆ t = arg max { p(zt | xt , mt−1 ) ⋅ p(xt | u t−1 , x t−1 )}
xt
Current observation (Scan)
Local map constructed
so far
€
2. Calculate new map
estimated pose xt
Dr. A. Kleiner
ˆt
m
by adding observation zt at
AI Robotics - SLAM
€
Estimated
transformation from
Odometry
12
Scan Matching with Likelihood Fields
• Approach:
– Sample the space of transformations (can be improved by
Gradient Descent) and select the most likely one
– Assumption: single beams of the observation are
conditionally independent:
K
P( z | x, m) = ∏ P( zk | x, m)
k=1
– How to compute
P( zk | x, m) ?
€
€
Dr. A. Kleiner
AI Robotics - SLAM
13
Computing p(zk|x,m) with Likelihood
Fields
• An “ad-hock” method, not really a probabilistic method:
•
Project the end-points of the scan on the map and compute a
score according to the probability of the alignment:
• Given the distance dist to the nearest obstacle, we can
compute:
dist
2σ hit
k
k
max
hit
1
P( z | x, m) =
2 πσ
e
if z < z
• “Distance Map” can be computed similar to Generalized
Voronoi Diagram (GVD)
€
Dr. A. Kleiner
AI Robotics - SLAM
14
Visual Odometry - Another way of pose
tracking
• A sequence of camera images I ( x, y,t )
• Features are detected in subsequent images by a
translation d = (ξ , η)T : I ( x, y,t + τ ) = I ( x − ξ , y − η,t )
€
• A feature tracker minimizes
the squared error ε over a
tracking window:
€
€
2
ε = ∫ [ I ( x − d ) − J ( x)] wdx,
W
with J ( x) = I ( x, y,t + τ ),
I ( x − d ) = I ( x − ξ , y − η,t )
Dr. A. Kleiner
€
AI Robotics - SLAM
15
Feature tracking with KLT features
Turn Left
Rotate Left
Go Forward
Turn Right
Rotate Right
Go Backward
Dr. A. Kleiner
AI Robotics - SLAM
16
Application: Autonomous obstacle
climbing
Dr. A. Kleiner
AI Robotics - SLAM
17
Application: SLAM while overcoming 3D
obstacles
Do we have here the right map representation?
Dr. A. Kleiner
AI Robotics - SLAM
18
Representing Environments in 3D
Building 52
Building 51
Rotating SICK Laser Scanner:
- 360 Deg in 2 seconds (full sphere )
- 50,000 pts/sec
Very accurate representation However,
huge amount of data after a few meters
of driving!
- How to keep in memory?
- How to plan efficiently?
Trees
3D map generated by odometry pose tracking
Dr. A. Kleiner
Signpost
AI Robotics - SLAM
19
Outline
• The SLAM problem
• Pose Tracking
– Scan Matching
– Visual Odometry
• Graph-Based SLAM
Dr. A. Kleiner
AI Robotics - SLAM
20
Graph-based SLAM
• Pose tracking has the problem to diverge with
increasing travel distance!
• However, if we detect loops we can correct this
divergence
– For example, recognizing on a camera image that we are
“again” in front of the Colosseum
– We can build a graph from pose tracking and add “Loop
closure” edges when ever we are certain that two
locations are the same
• Methods of Graph-based SLAM optimize such a
graph with respect to added constraints
Dr. A. Kleiner
AI Robotics - SLAM
21
Building the graph
• The graph represents constraints between locations
– We want these constraints to express our confidence in an
observation
– Covariance matrices from pose tracking exactly do represent our
confidence!
• Construction of graph
from pose tracking:
– Vertices represent points between locally measured displacements,
e.g., every 1m distance and 0.1 RAD orientation change
– Edges represent the observerd displacement with covariance:
where
is a 3x3 covariance matrix
• We add additional edges when “loop closures” have been
detected, i.e., whenever we associate a location
Dr. A. Kleiner
AI Robotics - SLAM
22
Analogy to Spring-Mass System
z12 ,Σ12
(x1,y1)T
(x2,y2)T
2
typically set to (0,0)T
1
z23 ,Σ23
4
(x4,y4)T
z34 ,Σ34
3
(x3,y3)T
Find low energy arrangement of the springs (zij)
and connected masses (visited locations xi,yi)
Dr. A. Kleiner
AI Robotics - SLAM
23
Graph Optimization
• Lu & Milios introduced 1997 a method based on least squares
to iteratively minimize the overall displacement error:
T
ˆij )
W = ∑ ( zij − zˆij ) Σ−1
ij ( zij − z
(i, j )
Where the zij are the observed displacements, the zˆij
are the predictions based on the prior estimate,
−1
and the Σij are the inverted covariance matrices
€
• Recent €
methods utilize the information€matrix rather than
the covariance matrix
– Makes
€ later computations much more efficient due to sparsity
– Maximum likelihood technique !Covariances can only be returned by
expensive matrix inversion
Dr. A. Kleiner
AI Robotics - SLAM
24
RFID-SLAM: Easy Data Association
• How to detect loop closures reliably in Search &
Rescue?
− Laser scans & camera images can look the same at different
locations, particularly when there are smoke & debris
• RFIDs (e.g. passive or active ZigBee nodes)
have a world-wide unique ID that can be readout by robots easily
− They can be released in the environment by drones or humans
Dr. A. Kleiner
AI Robotics - SLAM
25
RFID-SLAM: Hardware Setting Robot &
Human
13.56 MHz RFIDs
Zerg robot
Robot:
- 4WD (four shaft encoders)
- Inertial Measurement Unit
(IMU)
- RFID antenna for detecting tags
lying beneath the robot
- RFID deploy device
Dr. A. Kleiner
Glove for sensing
RFIDs (TZI Bremen)
RFID antenna and
deploy device
RFID:
- 13.56 MHz (short range
below a meter)
- 2048 Bit RAM, programmable
by the user
AI Robotics - SLAM
IMU
Test person
Human:
-IMU for step counting and
angle estimation
- RFID glove
26
RFID-SLAM Corrected Map
Robot (orange) and pedestrian (red) odometry
Corrected track (green) compared to GPS
ground truth (blue)
-Robot driving at 1.58 m/s for 2.5 km
-10 RFIDs
- Optimization time below a second
Dr. A. Kleiner
AI Robotics - SLAM
27
RFID-SLAM Covariance bounds
3σ bounds from slippage-sensitive
odometry
Dr. A. Kleiner
3σ bounds after the optimization
AI Robotics - SLAM
28
ROS Nodes
• GMAPPING
• Hector SLAM
• ...
Dr. A. Kleiner
AI Robotics - SLAM
29
Assistive Mapping: Capabilities such as
“Completeness of Search”
Completeness of search without assistive mapping
capability
Dr. A. Kleiner
Completeness of search with assistive
mapping capability
AI Robotics - SLAM
30
State-Of-The-Art Mapping by First
Responders in 2008
Dr. A. Kleiner
AI Robotics - SLAM
31
SLAM must be operational under real
conditions !
Dr. A. Kleiner
AI Robotics - SLAM
32
DCMapping used by first responders at
Disaster City
Dr. A. Kleiner
AI Robotics - SLAM
33
Results: DCMapping applied in very
different environments
Telerob Company (Stuttgart, DE)
NIST Niki site (Washington D.C., US)
Research Lab TU Graz (AT)
Research Lab TU Darmstadt (DE)
Maze DisasterCity
(Texas, US)
Intel Research Lab
(Seattle, US)
101 Bldg., Univ. of Freiburg (DE)
Dr. A. Kleiner
AI Robotics - SLAM
103 Bldg., Univ. of
Freiburg (DE)
82 Bldg.,
Univ. of
Freiburg
(DE)
34