1153

Simultaneou 46. Simultaneous Localization

Multimedia Contents

and Mapping

Part E | 46
Cyrill Stachniss, John J. Leonard, Sebastian Thrun

46.1 SLAM: Problem Definition ..................... 1154

This chapter provides a comprehensive intro-
46.1.1 Mathematical Basis ................... 1154
duction in to the simultaneous localization and
46.1.2 Example: SLAM
mapping problem, better known in its abbreviated in Landmark Worlds .................. 1155
form as SLAM. SLAM addresses the main percep- 46.1.3 Taxonomy of the SLAM Problem .. 1156
tion problem of a robot navigating an unknown
environment. While navigating the environment, 46.2 The Three Main SLAM Paradigms ........... 1157
the robot seeks to acquire a map thereof, and 46.2.1 Extended Kalman Filters ............ 1157
at the same time it wishes to localize itself us-
46.2.2 Particle Methods ....................... 1159
46.2.3 Graph-Based
ing its map. The use of SLAM problems can be
Optimization Techniques............ 1162
motivated in two different ways: one might be in-
46.2.4 Relation of Paradigms................ 1166
terested in detailed environment models, or one
might seek to maintain an accurate sense of a mo- 46.3 Visual and RGB-D SLAM ........................ 1166
bile robot’s location. SLAM serves both of these 46.4 Conclusion and Future Challenges ........ 1169
purposes.
Video-References......................................... 1170
We review the three major paradigms from
which many published methods for SLAM are de- References................................................... 1171
rived: (1) the extended Kalman filter (EKF); (2)
particle filtering; and (3) graph optimization. We
also review recent work in three-dimensional (3-D) tance-sensors (RGB-D), and close with a discussion
SLAM using visual and red green blue dis- of open research problems in robotic mapping.

This chapter provides a comprehensive introduction tured, and of limited size. Robustly mapping unstruc-
into one of the key enabling technologies of mobile tured, dynamic, and large-scale environments in an on-
robot navigation: simultaneous localization and map- line fashion remains largely an open research problem.
ping, or in short SLAM. SLAM addresses the problem The historical roots of methods that can be applied
of acquiring a spatial map of an environment while si- to address the SLAM problem can be traced back to
multaneously localizing the robot relative to this model. Gauss [46.1], who is largely credited for inventing the
The SLAM problem is generally regarded as one of the least squares method. In the Twentieth Century, a num-
most important problems in the pursuit of building truly ber of fields outside robotics have studied the making
autonomous mobile robots. It is of great practical im- of environment models from a moving sensor platform,
portance; if a robust, general-purpose solution to SLAM most notably in photogrammetry [46.2–4] and com-
can be found, then many new applications of mobile puter vision [46.5]. Strongly related problems in these
robotics will become possible. fields are bundle adjustment and structure from mo-
While the problem is deceptively easy to state, it tion. SLAM builds on this work, often extending the
presents many challenges, despite significant progress basic paradigms into more scalable algorithms. Mod-
made in this area. At present, we have robust methods ern SLAM systems often view the estimation problem
for mapping environments that are mainly static, struc- as solving a sparse graph of constraints and applying
1154 Part E Moving in the Environment

nonlinear optimization to compute the map and the tra- chosen by the practitioner will depend on a number of
jectory of the robot. As we strive to enable long-lived factors, such as the desired map resolution, the update
autonomous robots, an emerging challenge is to handle time, and the nature of the features in the map, and so
massive sensor data streams. on. Nevertheless, the three methods discussed in this
This chapter begins with a definition of the SLAM chapter cover the major paradigms in this field.
problem, which shall include a brief taxonomy of dif- For more a detailed treatment of SLAM, we refer
ferent versions of the problem. The centerpiece of this the reader to Durrant-Whyte and Bailey [46.6, 7], who
Part E | 46.1

chapter is a layman introduction into the three major provide an in-depth tutorial for SLAM, Grisetti et al.
paradigms in this field, and the various extensions that for a tutorial on graph-based SLAM [46.8], and Thrun
exist. As the reader will quickly recognize, there is no et al., which dedicates a number of chapters to the topic
single best solution to the SLAM method. The method of SLAM [46.9].

46.1 SLAM: Problem Definition

The SLAM problem is defined as follows: A mobile surements are noisy, and path integration techniques
robot roams an unknown environment, starting at an inevitably diverge from the truth.
initial location x0 . Its motion is uncertain, making it Finally, the robot senses objects in the environment.
gradually more difficult to determine its current pose in Let m denote the true map of the environment. The
global coordinates. As it roams, the robot can sense its environment may be comprised of landmarks, objects,
environment with a noisy sensor. The SLAM problem surfaces, etc., and m describes their locations. The en-
is the problem of building a map of the environment vironment map m is often assumed to be time-invariant,
while simultaneously determining the robot’s position i. e., static.
relative to this map given noisy data. The robot measurements establish information be-
tween features in m and the robot location x t . If we,
46.1.1 Mathematical Basis without loss of generality, assume that the robot takes
exactly one measurement at each point in time, the se-
Formally, SLAM is best described in probabilistic ter- quence of measurements is given as
minology. Let us denote time by t, and the robot
location by x t . For mobile robots on a flat ground, x t is
ZT D fz1 ; z2 ; z3 ; : : : ; zT g : (46.3)
usually a three-dimensional vector, comprising its two-
dimensional (2-D) coordinate in the plane plus a single
rotational value for its orientation. The sequence of lo- Figure 46.1 illustrates the variables involved in the
cations, or path, is then given as SLAM problem. It shows the sequence of locations and
sensor measurements, and the causal relationships be-
XT D fx0 ; x1 ; x2 ; : : : ; xT g : (46.1) tween these variables. This diagram represents a graph-

Here T is some terminal time (T might be 1). The

initial location x0 often serves as a point of reference x t –1 xt x t+1
for the estimation algorithm; other positions cannot be
sensed.
Odometry provides relative information between ut –1 ut ut+1
two consecutive locations. Let u t denote the odometry
that characterized the motion between time t  1 and z t –1 zt z t+1
time t; such data might be obtained from the robot’s
wheel encoders or from the controls given to those mo-
tors. Then the sequence
m

UT D fu1 ; u2 ; u3 : : : ; uT g (46.2)
Fig. 46.1 Graphical model of the SLAM problem. Arcs in-
characterizes the relative motion of the robot. For noise- dicate causal relationships, and shaded nodes are directly
free motion, UT would be sufficient to recover the poses observable to the robot. In SLAM, the robot seeks to re-
from the initial location x0 . However, odometry mea- cover the unobservable variables
 Simultaneous Localization and Mapping 46.1 SLAM: Problem Definition 1155

ical model for SLAM. It is useful in understanding the When building 2-D maps, point-landmarks may cor-
dependencies in the problem at hand. respond to door posts and corners of rooms, which,
The SLAM problem is now the problem of recov- when projected into a 2-D map are characterized by
ering a model of the world m and the sequence of a point coordinate. In a 2-D world, each point-landmark
robot locations XT from the odometry and measurement is characterized by two coordinate values. Hence the
data. The literature distinguishes two main forms of the world is a vector of size 2N, where N is the number
SLAM problem, which are both of equal practical im- of point-landmarks in the world. In a commonly stud-

Part E | 46.1
portance. One is known as the full SLAM problem: it ied setting, the robot can sense three things: the relative
involves estimating the posterior over the entire robot range to nearby landmarks, their relative bearing, and
path together with the map the identity of these landmarks. The range and bearing
may be noisy, but in the most simple case the identity
p.XT ; m j ZT ; UT / : (46.4) of the sensed landmarks is known perfectly. Determin-
ing the identity of the sensed landmarks is also known
Written in this way, the full SLAM problem is the
as the data association problem. In practice, it is one of
problem of calculating the joint posterior probability
the most difficult problems in SLAM.
over XT and m from the available data. Notice that the
To model the above described setup, one begins
variables right of the conditioning bar are all directly
with defining the exact, noise-free measurement func-
observable to the robot, whereas those on the left are
tion. The measurement function h describes the work-
the ones that we want. As we shall see, algorithms for
ings of the sensors: it accepts as input a description of
the full SLAM problem are often batch, that is, they
the environment m and a robot location x t , and it com-
process all data at the same time.
putes the measurement
The second, equally important SLAM problem is
the online SLAM problem. This problem is defined via
h.x t ; m/ : (46.6)
p.x t ; m j Z t ; U t / : (46.5)
Computing h is straightforward in our simplified land-
Online SLAM seeks to recover the present robot loca- mark setting; it is a simple exercise in trigonometry. The
tion, instead of the entire path. Algorithms that address probabilistic measurement model can be derived from
the online problem are usually incremental and can pro- this measurement function by adding a noise term. It
cess one data item at a time. In the literature, such is a probability distribution that peaks at the noise-free
algorithms are typically called filters. value h.x t ; m/ but allows for measurement noise, for ex-
To solve the SLAM problem, the robot needs to ample,
be endowed with two more models: a mathematical
model that relates odometry measurements u t to robot p.z t j x t ; m/ D N .h.x t ; m/; Q t / : (46.7)
locations x t1 and x t ; and a model that relates measure-
ments z t to the environment m and the robot location x t . Here N denotes the 2-D normal distribution, which is
These models correspond to the arcs in Fig. 46.1. centered at h.x t ; m/. The 2-by-2 matrix Q t is the noise
In SLAM, it is common to think of those mathemat- covariance, indexed by time.
ical models as probability distributions: p.x t j x t1 ; u t / The motion model is derived from a kinematic
characterizes the probability distribution of the location model of robot motion. Given the location vector x t1
x t assuming that a robot started at a known location and the motion u t , textbook kinematics tells us how to
x t1 and measured the odometry data u t . And likewise, calculate x t . Let this function be denoted by g
p.z t j x t ; m/ is the probability for measuring z t if this
measurement is taken at a known location x t in a known g.x t1 ; u t / : (46.8)
environment m. Of course, in the SLAM problem we do
not know the robot location, and neither do we know the The motion model may then be defined by a normal dis-
environment. As we shall see, Bayes rule takes care of tribution centered at g.x t1 ; u t / but subject to Gaussian
this, by transforming these mathematical relationships noise
into a form where we can recover probability distribu-
tions over those latent variables from the measured data. p.x t j x t1 ; u t / D N .g.x t1 ; u t /; R t / : (46.9)

46.1.2 Example: SLAM in Landmark Worlds Here R t is a covariance. It is of size 3-by-3, since the
location is a three-dimensional 3-D vector.
One common setting of SLAM involves an assumption With these definitions, we have all we need to
that the environment is populated by point-landmarks. develop a SLAM algorithm. While in the literature,
1156 Part E Moving in the Environment

point-landmark problems with range-bearing sensing previously observed landmarks in the map. The prob-
are by far the most studied, SLAM algorithms are not lem of estimating the correspondence is known as data
confined to landmark worlds. But no matter what the association problem. It is one of the most difficult prob-
map representation and the sensor modality, any SLAM lems in SLAM.
algorithm needs a similarly crisp definition of the fea-
tures in m, the measurement model p.z t j x t ; m/, and the Static Versus Dynamic
motion model p.x t j x t1 ; u t /. Note that none of those Static SLAM algorithms assume that the environment
Part E | 46.1

distributions has to be restricted to Gaussian noise as does not change over time. Dynamic methods allow
done in the example above. for changes in the environment. The vast literature on
SLAM assumes static environments. Dynamic effects
46.1.3 Taxonomy of the SLAM Problem are often treated just as measurement outliers. Meth-
ods that reason about motion in the environment are
SLAM problems are distinguished along a number of more involved, but they tend to be more robust in most
different dimensions. Most important research papers applications.
identify the type of problems addressed by making the
underlying assumptions explicit. We already encoun- Small Versus Large Uncertainty
tered one such distinction: full versus online. Other SLAM problems are distinguished by the degree of
common distinctions are as follows: location uncertainty that they can handle. The most sim-
ple SLAM algorithms allow only for small errors in
Volumetric Versus Feature-Based the location estimate. They are good for situations in
In volumetric SLAM, the map is sampled at a resolution which a robot goes down a path that does not intersect
high enough to allow for photo-realistic reconstruction itself, and then returns along the same path. In many
of the environment. The map m in volumetric SLAM environments it is possible to reach the same location
is usually quite high-dimensional, with the result that from multiple directions. Here the robot may accrue
the computation can be quite involved. Feature-based a large amount of uncertainty. This problem is known
SLAM extracts sparse features from the sensor stream. as the loop closing problem. When closing a loop, the
The map is then only comprised of features. Our uncertainty may be large. The ability to close loops is
point-landmark example is an instance of feature-based a key characteristic of modern-day SLAM algorithms.
SLAM. Feature-based SLAM techniques tend to be The uncertainty can be reduced if the robot can sense
more efficient, but their results may be inferior to volu- information about its position in some absolute coor-
metric SLAM due to the fact that the extraction of fea- dinate frame, e.g., through the use of a satellite-based
tures discards information in the sensor measurements. global positioning system (GPS) receiver.

Topological Versus Metric Active Versus Passive

Some mapping techniques recover only a qualitative de- In passive SLAM algorithms, some other entity controls
scription of the environment, which characterizes the the robot, and the SLAM algorithm is purely observing.
relation of basic locations. Such methods are known as The vast majority of algorithms are of this type; they
topological. A topological map might be defined over give the robot designer the freedom to implement ar-
a set of distinct places and a set of qualitative rela- bitrary motion controllers, and pursue arbitrary motion
tions between these places (e.g., place A is adjacent to objectives. In active SLAM, the robot actively explores
place B). Metric SLAM methods provide metric infor- its environment in the pursuit of an accurate map. Ac-
mation between the relation of such places. In recent tive SLAM methods tend to yield more accurate maps
years, topological methods have fallen out of fashion, in less time, but they constrain the robot motion. There
despite ample evidence that humans often use topolog- exist hybrid techniques in which the SLAM algorithm
ical information for navigation. controls only the pointing direction of the robot’s sen-
sors, but not the motion direction.
Known Versus Unknown Correspondence
The correspondence problem is the problem of relat- Single-Robot Versus Multi-Robot
ing the identity of sensed things to other sensed things. Most SLAM problems are defined for a single robot
In the landmark example above, we assumed that the platform, although recently the problem of multi-
identity of landmarks is known. Some SLAM algo- robot exploration has gained in popularity. Multi-robot
rithms make such an assumption, others do not. The SLAM problems come in many flavors. In some, robots
ones that do not provide special mechanisms for es- get to observe each other, in others, robots are told their
timating the correspondence of measured features to relative initial locations. Multirobot SLAM problems
 Simultaneous Localization and Mapping 46.2 The Three Main SLAM Paradigms 1157

are also distinguished by the type of communication al- to traditional methods. They enable the robot to com-
lowed between the different robots. In some, the robots pute a solution given the resource constraints of the
can communicate with no latency and infinite band- system. The more resources available, the better the
width. More realistic are setups in which only nearby solution.
robots can communicate, and the communication is As this taxonomy suggests, there exists a flurry of
subject to latency and bandwidth limitations. SLAM algorithms. Most modern-day conferences ded-
icate multiple sessions to SLAM. This chapter focuses

Part E | 46.2
Any-Time and Any-Space on the very basic SLAM setup. In particular it assumes
Robots that do all computations onboard have limited a static environment with a single robot. Extensions are
resources in memory and computation power. Any- discussed towards the end of this chapter, in which the
time and any-space SLAM systems are an alternative relevant literature is discussed.

46.2 The Three Main SLAM Paradigms

This section reviews three basic SLAM paradigms, sentations is a critical issue in SLAM, and intimately
from which most others are derived. The first, known as related to the topics of sensing and world modeling dis-
EKF SLAM, is in robotics historically the earliest but cussed in Chap. 36 and in Part C.
has become less popular due to its limiting computa- The EKF algorithm represents the robot estimate by
tional properties and issues resulting from performing a multivariate Gaussian
single linearizations only. The second approach uses
nonparametric statistical filtering techniques known as p.x t ; m j Z t ; U t / D N . t ; † t / : (46.10)
particle filters. It is a popular method for online SLAM
and provides a perspective on addressing the data asso- The high-dimensional vector  t contains the robot’s
ciation problem in SLAM. The third paradigm is based best estimate of its own current location x t and the
on graphical representations and successfully applies location of the features in the environment. In our
sparse nonlinear optimization methods to the SLAM point-landmark example, the dimension of  t would be
problem. It is the main paradigm for solving the full 3 C 2N, since we need three variables to represent the
SLAM problem and recently also incremental tech- robot location and 2N variables for the N landmarks in
niques are available. the map.
The matrix † t is the covariance of the robot’s as-
46.2.1 Extended Kalman Filters sessment of its expected error in the guess  t . The
matrix † t is of size .3 C 2N/  .3 C 2N/ and it is pos-
Historically, the EKF formulation of SLAM is the earli- itive semi-definite. In SLAM, this matrix is usually
est, and perhaps the most influential, SLAM algorithm. dense. The off-diagonal elements capture the correla-
EKF SLAM was introduced in [46.10, 11] and [46.12, tions in the estimates of different variables. Nonzero
13], which were the first papers to propose the use of correlations come along because the robot’s location is
a single state vector to estimate the locations of the uncertain, and as a result the locations of the landmarks
robot and a set of features in the environment, with an in the maps are uncertain.
associated error covariance matrix representing the un- The EKF SLAM algorithm is easily derived for our
certainty in these estimates, including the correlations point-landmark example. Suppose, for a moment, the
between the vehicle and feature state estimates. As the motion function g and the measurement function h were
robot moves through its environment taking measure- linear in their arguments. Then, the vanilla Kalman fil-
ments, the system state vector and covariance matrix ter, as described in any textbook on Kalman filtering,
are updated using the extended Kalman filter [46.14, would be applicable. EKF SLAM linearizes the func-
15]. As new features are observed, new states are added tions g and h using Taylor series expansion. In its most
to the system state vector; the size of the system covari- basic form and in the absence of any data association
ance matrix grows quadratically. problems, EKF SLAM is basically the application of
This approach assumes a metrical, feature-based the EKF to the online SLAM problem.
environmental representation, in which objects can be Figure 46.2 illustrates the EKF SLAM algorithm for
effectively represented as points in an appropriate pa- an artificial example. The robot navigates from a start
rameter space. The position of the robot and the loca- pose that serves as the origin of its coordinate system.
tions of features form a network of uncertain spatial As it moves, its own pose uncertainty increases, as in-
relationships. The development of appropriate repre- dicated by uncertainty ellipses of growing diameter. It
1158 Part E Moving in the Environment

Fig.46.2a–d EKF applied to the

a) b) online SLAM problem. The robot’s
path is a dotted line, and its estimates
of its own position are shaded ellipses.
Eight distinguishable landmarks of
unknown location are shown as small
dots, and their location estimates are
Part E | 46.2

shown as white ellipses. In (a–c)

the robot’s positional uncertainty is
increasing, as is its uncertainty about
the landmarks it encounters. In (d) the
robot senses the first landmark again,
and the uncertainty of all landmarks
c) d) decreases, as does the uncertainty
of its current pose (image courtesy
of Michael Montemerlo, Stanford
University)

also senses nearby landmarks and maps them with an EKF idea becomes inapplicable. The solution here is
uncertainty that combines the fixed measurement uncer- to reason about the most likely data association when
tainty with the increasing pose uncertainty. As a result, a landmark is observed. This is usually done based
the uncertainty in the landmark locations grows over on proximity: which of the landmarks in the map cor-
time. The interesting transition happens in Fig. 46.2d: responds most likely to the landmark just observed?
Here the robot observes the landmark it saw in the very The proximity calculation considers the measurement
beginning of mapping, and whose location is relatively noise and the actual uncertainty in the poster estimate,
well known. Through this observation, the robot’s pose and the metric used in this calculation is known as
error is reduced, as indicated in Fig. 46.2d – notice a Mahalanobis distance, which is a weighted quadratic
the very small error ellipse for the final robot pose. distance. To minimize the chances of false data asso-
This observation also reduces the uncertainty for other ciations, many implementations use visible features to
landmarks in the map. This phenomenon arises from distinguish individual landmarks and associate groups
a correlation that is expressed in the covariance matrix of landmarks observed simultaneously [46.17, 18], al-
of the Gaussian posterior. Since most of the uncertainty though distinct features can also be computed from
in earlier landmark estimates is caused by the robot laser data [46.19, 20]. Typical implementations also
pose, and since this very uncertainty persists over time, maintain a provisional landmark list and only add
the location estimates of those landmarks are correlated. landmarks to the internal map when they have been
When gaining information on the robot’s pose, this in- observed sufficiently frequently [46.16, 21]. With an
formation spreads to previously observed landmarks. appropriate landmark definition and careful implemen-
This effect is probably the most important characteristic tation of the data association step, EKF SLAM has been
of the SLAM posterior [46.16]. Information that helps applied successfully in a wide range of environments,
localize the robot is propagated through the map, and as using airborne, underwater, indoor, and various other
a result improves the localization of other landmarks in platforms.
the map. The basic formulation of EKF SLAM assumes that
With a few adaptations, EKF SLAM can also be ap- the location of features in the map is fully observable
plied in the presence of uncertain data association. If from a single position of the robot. The method has
the identity of observed features is unknown, the basic been extended to situations with partial observability,
 Simultaneous Localization and Mapping 46.2 The Three Main SLAM Paradigms 1159

with range-only [46.22] or angle-only [46.23, 24] mea- who coined the name fastSLAM (fast simultaneous lo-
surements. The technique has also been utilized using calization and mapping). Let us first explain the basic
a feature-less representation, in which the state consists FastSLAM algorithm on the simplified point-landmark
of current and past robot poses, and measurements take example, and then discuss the justification for this ap-
the form of constraints between the poses (derived for proach.
example from laser scan matching or from camera mea- At any point in time, FastSLAM maintains K parti-
surements) [46.25, 26]. cles of the type

Part E | 46.2
A key concern of the EKF approach to SLAM Œk Œk Œk Œk
lies in the quadratic nature of the covariance matrix. X Œk
t ;  t;1 ; : : : ;  t;N ; † t;1 ; : : : ; † t;N : (46.11)
A number of researchers have proposed extensions to Here Œk is the index of the sample. This expression
the EKF SLAM algorithms that achieve scalability, for states that a particle contains:
example through submap decomposition [46.27–30].
A related family of approaches [46.31–34] employs the  A sample path X Œk
t , and
Extended Information Filter, which operates on the in-  A set of N 2-D Gaussians with means Œk t;n and
Œk
verse of the covariance matrix. A key insight is that variances † t;n ), one for each landmark in the en-
whereas the EKF covariance is densely populated, the vironment.
information matrix is sparse when the full robot tra-
jectory is maintained, leading to the development of Here n is the index of the landmark (with 1 n
efficient algorithms and providing a conceptual link N). From that it follows that K particles possess K path
to the pose graph optimization methods described in samples. It also possesses KN Gaussians, each of which
Sect. 46.2.3. models exactly one landmark for one of the particles.
The issues of consistency and convergence in Initializing FastSLAM is simple: just set each parti-
EKF SLAM have been investigated in [46.35, 36]. cle’s robot location to the starting coordinates, typically
Observability-based rules for designing consistent EKF .0; 0; 0/T , and zero the map. The particle update then
SLAM estimators are presented in [46.37]. proceeds as follows:
 When an odometry reading is received, new loca-
46.2.2 Particle Methods tion variables are generated stochastically, one for
each of the particles. The distribution for generat-
The second principal SLAM paradigm is based on par- ing those location particles is based on the motion
ticle filters. Particle filters can be traced back to [46.38], model
but they have become popular only in the last two
decades. Particle filters represent a posterior through xŒk
t p.x t j xŒk
t1 ; u t / : (46.12)
a set of particles. For the novice in SLAM, each par-
Here xŒk
t1 is the previous location, which is part
ticle is best thought as a concrete guess as to what
of the particle. This probabilistic sampling step is
the true value of the state may be. By collecting many
easily implemented for any robot whose kinematics
such guesses into a set of guesses, or set of particles,
can be computed.
the particle filter approximates the posterior distribu-
tion. Under mild conditions, the particle filter has been
 When a measurement z t is received, two things hap-
pen: first, FastSLAM computes for each particle the
shown to approach the true posterior as the particle set
probability of the new measurement z t . Let the in-
size goes to infinity. It is also a nonparametric repre-
dex of the sensed landmark be n. Then the desired
sentation that represents multimodal distributions with
probability is defined as follows
ease.
The key problem with the particle filter in the con- w tŒk D N .z t j xŒk Œk Œk
t ;  t;n ; † t;n / : (46.13)
text of SLAM is that the space of maps and robot paths
is huge. Suppose we have a map with 100 features. How The factor w tŒk is called the importance weight,
many particles would it take to populate that space? since it measures how important the particle is in
In fact, particle filters scale exponentially with the di- the light of the new sensor measurement. As before,
mension of the underlying state space. Three or four N denotes the normal distribution, but this time it
dimensions are thus acceptable, but 100 dimensions are is calculated for a specific value, z t . The importance
generally not. weights of all particles are then normalized so that
The trick to make particle filters amenable to the they sum to 1.
SLAM problem goes back to [46.39, 40] and is known Next, FastSLAM draws with replacement from the
as Rao–Blackwellization. It has been introduced into set of existing particles a set of new particles. The
the SLAM literature in [46.41], followed by [46.42], probability of drawing a particle is its normalized
1160 Part E Moving in the Environment

importance weight. This step is called resampling. Blackwellization, and it yields better results than sam-
The intuition behind resampling is that particles pling from the joint. FastSLAM applies this technique,
for which the measurement is more plausible have in that it samples from the path posterior p.X Œk t j Ut ; Zt /
a higher chance of surviving the resampling pro- and represents the map p.m j X Œkt ; U t ; Z t / in Gaussian
cess. form.
Finally, FastSLAM updates for the new particle set FastSLAM also breaks down the posterior over
the mean Œk Œk
t;n and covariance † t;n , based on the maps (conditioned on paths) into sequences of low-
Part E | 46.2

measurement z t . This update follows the standard dimensional Gaussians. The justification for this de-
EKF update rules – note that the extended Kalman composition is subtle. It arises from a specific condi-
filters maintained in FastSLAM are, in contrast to tional independence assumption that is native to SLAM.
EKF SLAM, all low-dimensional (typically 2-D). Fig. 46.3 illustrates the concept graphically. In SLAM,
knowledge of the robot path renders all landmark esti-
This all may sound complex, but FastSLAM is mates independent. This is easily shown for the graph-
quite easy to implement. Sampling from the motion ical network in Fig. 46.3: we find that if we remove
model usually involves simple kinematic calculations. the path variables from Fig. 46.3, then the landmark
Computing the importance of a measurement is often variables are all disconnected [46.43]. Thus, in SLAM
straightforward too, especially for Gaussian measure- any dependence between multiple landmark estimates
ment noise. And updating a low-dimensional particle is mediated through the robot path. This subtle but
filter is also not complicated. important observation implies that even if we used
FastSLAM has been shown to approximate the full a large, monolithic Gaussian for the entire map (one
SLAM posterior. The derivation of FastSLAM exploits per particle, of course), the off-diagonal element be-
three techniques: Rao–Blackwellization, conditional in- tween different landmarks would simply remain zero.
dependence, and resampling. Rao–Blackwellization is It is therefore legitimate to implement the map more
the following concept. Suppose we would like to com- efficiently, using N small Gaussians, one for each land-
pute a probability distribution p.a; b/, where a and b mark. This explains the efficient map representation in
are arbitrary random variables. The vanilla particle filter FastSLAM.
would draw particles from the joint distributions, that Figure 46.4 shows results for a point-feature prob-
is, each particle would have a value for a and one for b. lem; here the point features are the centers of tree
However, if the conditional p.b j a/ can be described in trunks as observed by an outdoor robot. The dataset
closed form, it is equally legitimate to just draw parti- used here is known as the Victoria Park dataset [46.44].
cles from p.a/, and attach to each particle a closed-form Fig. 46.4a shows the path of the vehicle obtained by
description of p.b j a/. This trick is known as Rao– integrating the vehicle controls, without perception. As
can be seen, controls are a poor predictor of location for
this vehicle; after 30 min of driving, the estimated po-
xt –1 xt xt +1 xt +2 sition of the vehicle is well over 100 m away from its
GPS position.
The FastSLAM algorithm has a number of inter-
ut –1 ut ut +1 ut +2
esting properties. First, it solves both full and online
SLAM problems. Each particle has a sample of an en-
zt –1 zt zt +1 zt +2
tire path but the actual update equation only uses the
most recent pose. This makes FastSLAM a filter. Sec-
ond, FastSLAM can maintain multiple data association
m1 m2 m3
hypotheses. It is straightforward to make data associa-
tion decisions on a per-particle basis, instead of having
Fig. 46.3 The SLAM problem depicted as Bayes network graph. to adopt the same hypothesis for the entire filter. While
The robot moves from location x t1 to location x tC2 , driven by we will not give any mathematical justification, we
a sequence of controls. At each location x t it observes a nearby note that the resulting FastSLAM algorithm can even
feature in the map m D fm1 ; m2 ; m3 g. This graphical network illus- deal with unknown data association – something that
trates that the location variables separate the individual features in the extended Kalman filter cannot claim. And third,
the map from each other. If the locations are known, there remains FastSLAM can be implemented very efficiently using
no other path involving variables whose value is not known, be- advanced tree methods to represent the map estimates,
tween any two features in the map. This lack of a path renders the the update can be performed in time logarithmic in the
posterior of any two features in the map conditionally independent size of the map N, and linear in the number of parti-
(given the locations) cles M.
 Simultaneous Localization and Mapping 46.2 The Three Main SLAM Paradigms 1161

a) Raw vehicle path b) FastSLAM (solid), GPS path (dashed) c) Path and map with aerial image

Part E | 46.2
Fig. 46.4 (a) Vehicle path predicted by the odometry; (b) True path (dashed line) and FastSLAM 1.0 path (solid line);
(c) Victoria Park results overlaid on aerial imagery with the GPS path in blue (dashed), average FastSLAM 1.0 path in
yellow (solid), and estimated features as yellow dots (data and aerial image courtesy of José Guivant and Eduardo Nebot,
Australian Centre for Field Robotics)

a) b)

Fig. 46.5 Occupancy grid map generated from laser range data and based on pure odometry (image courtesy of Dirk
Hähnel, University of Freiburg)

FastSLAM has been extended in several ways. One whose maps are most consistent with the measure-
set of variants are grid-based versions of FastSLAM, ment. A resulting large-scale map is shown in Fig. 46.5.
in which the Gaussians used to model point landmarks Further extensions can be found in [46.48, 49], whose
are replaced by an occupancy grid map [46.45–47]. The methods are called DP-SLAM and operate on ances-
variant of [46.46] is illustrated in Fig. 46.5. try trees to provide efficient tree update methods for
Figure 46.6 illustrates a simplified situation with grid-based maps. Related to that, approximations to
three particles just before closing a large loop. The three FastSLAM in which particles share their maps have
different particles each stand for different paths, and been proposed [46.50].
they also posses their own local maps. When the loop The works in [46.45, 47, 51] provide ways to in-
is closed importance resampling selects those particles corporate new observations into the location sampling
1162 Part E Moving in the Environment

3 particles and their trajectories

Part E | 46.2

Map of particle 1 Map of particle 1 Map of particle 1

Fig. 46.6 Application of the grid-based variant of the FastSLAM algorithm. Each particle carries its own map and the
importance weights of the particles are computed based on the likelihood of the measurements given the particle’s own
map

process for landmarks and grid maps, based on prior parameter. Second, nested loops combined with exten-
work in [46.52]. This leads to an improved sampling sive re-visits of previously mapped areas can lead to
process particle depletion, which in turn may prevent the system
from estimating a consistent map. Adaptive resampling
p.z t j mŒk Œk
t1 ; x t / p.x t j x t1 ; u t / strategies [46.45], particles sharing maps [46.50], or fil-
xŒk ; (46.14)
p.z t j mŒk Œk
t ter backup approaches [46.53] improve the situation but
t1 ; x t1 ; u t /
cannot eliminate this problem in general.
which incorporates the odometry and the observation
at the same time. Using an improved proposal dis- 46.2.3 Graph-Based
tribution leads to more accurately sampled locations. Optimization Techniques
This in turn leads to more accurate maps and requires
a smaller number of particles compared to approaches A third family of algorithms solves the SLAM problem
using the sampling process given in (46.12). This ex- through nonlinear sparse optimization. They draw their
tension makes FastSLAM and especially its grid-based intuition from a graphical representation of the SLAM
variants robust tools for addressing the SLAM problem. problem and the first working solution in robotics was
Finally, there are approaches that aim to overcome proposed in [46.54]. The graph-based representation
the assumption that the observations show Gaussian used here is closely related to a series of papers [46.55–
characteristics. As shown in [46.47], there are sev- 64]. We note that most of the earlier techniques are
eral situations in which the model is nonGaussian and offline and address the full SLAM problem. In more
also multimodal. A sum of Gaussians model on a per- recent years, new incremental versions that effectively
particle bases, however, can be efficiently considered re-use the previously computed solution have been pro-
in the particle filter and it eliminates this problem in posed such as [46.65–67].
practice without introducing additional computational The basic intuition of graph-based SLAM is a fol-
demands. lows. Landmarks and robot locations can be thought of
The so-far developed particle filters-based SLAM as nodes in a graph. Every consecutive pair of locations
systems suffer from two problems. First, the number of x t1 ; x t is tied together by an edge that represents the
samples that are required to compute consistent maps information conveyed by the odometry reading u t . Fur-
is often set manually by making an educated guess. ther edges exist between the nodes that correspond to
The larger the uncertainty that the filter needs to rep- locations x t and landmarks mi , assuming that at time t
resent during mapping, the more critical becomes this the robot sensed landmark i. Edges in this graph are
 Simultaneous Localization and Mapping 46.2 The Three Main SLAM Paradigms 1163

soft constraints. Relaxing these constraints yields the If we think of the graph as a spring-mass mod-
robot’s best estimate for the map and the full path. el [46.60], computing the SLAM solution is equivalent
The construction of the graph is illustrated in to computing the state of minimal energy of this model.
Fig. 46.7. Suppose at time t D 1, the robot senses To see, we note that the graph corresponds to the log-
landmark m1 . This adds an arc in the (yet highly in- posterior of the full SLAM problem (46.4)
complete) graph between x1 and m1 . When caching
the edges in a matrix format (which happens to cor- log p.XT ; m j ZT ; UT / : (46.15)

Part E | 46.2
respond to a quadratic equation defining the resulting
constraints), a value is added to the elements be- Without derivation, we state that this logarithm is of the
tween x1 and m1 , as shown on the right hand side of form
Fig. 46.7a. log p.XT ; m j ZT ; UT /
Now suppose the robot moves. The odometry read- X
ing u2 leads to an arc between nodes x1 and x2 , as D const C log p.x t j x t1 ; u t /
shown in Fig. 46.7b. Consecutive application of these X
t
two basic steps leads to an graph of increasing size, C log p.z t j x t ; m/ ; (46.16)
as illustrated in Fig. 46.7c. Nevertheless this graph is t
sparse, in that each node is only connected to a small
number of other nodes (assuming a sensor with limited assuming independence between the individual obser-
sensing range). The number of constraints in the graph vations and odometry readings. Each constraint of the
is (at worst) linear in the time elapsed and in the num- form log p.x t j x t1 ; u t / is the result of exactly one robot
ber of nodes in the graph. motion event, and it corresponds to an edge in the graph.
Likewise, each constraint of the form log p.z t j x t ; m/ is
the result of one sensor measurement, to which we can
a) also find a corresponding edge in the graph. The SLAM
x1 m1
x1 problem is then simply to find the mode of this equa-
m1 tion, i. e.,

XT ; m D argmax log p.XT ; m j ZT ; UT / : (46.17)

m1 XT ;m

x1 Without derivation, we note that under the Gaussian

noise assumptions, which was made in the point-
b) landmark example, this expression resolves to the fol-
x1 x2 m1 lowing quadratic form
x1
m1 x2
log p.XT ; m j ZT ; UT / D const
X
C Œx t  g.x t1 ; u t /T R1 Œx t  g.x t1 ; u t /
m1
t
„ ƒ‚
t
…
odometry reading
x1 x2 X
C Œz t  h.x t ; m/T Q1 Œz t  h.x t ; m/ :
t
„ ƒ‚
t
…
c) feature observation
x1 x2 x3 x4 m1 m2 m3 m4 (46.18)
m2 x1
m1 m4 x2
m3 This quadratic form yields a sparse system of equations
x3
x4 and a number of efficient optimization techniques can
m1 be applied. Common choices include direct methods
m2 such as sparse Cholesky and QR decomposition, or iter-
x1 x2 x3 x4 m3
ative ones such as gradient descent, conjugate gradient,
m4
and others. Most SLAM implementations rely on iter-
atively linearizing the functions g and h, in which case
Fig.46.7a–c Illustration of the graph construction. The (a) the objective in (46.18) becomes quadratic in all of its
diagram shows the graph, the (b) the constraints in matrix variables.
form. (a) Observation ls landmark m1 . (b) Robot motion Extensions to support an effective correction of
from x1 to x2 . (c) Several steps later large-scale graphs are hierarchical methods. One of
1164 Part E Moving in the Environment

the first is the ATLAS framework [46.25], which con- mizer [46.77]. There has also been an extension that can
structs a two-level hierarchy combining a Kalman filter deal with multimodal constraints [46.78], proposing
that operates in the lower level and a global optimiza- a max-mixture representation for maintaining efficiency
tion at the higher level. Similar to that, Hierarchical of the log likelihood optimizing in (46.16). As a result
SLAM [46.68] is a technique for using independent of that, the multimodal extension has only little im-
local maps, which are merged in case of re-visiting pact on the runtime and can easily be incorporated in
a place. A fully hierarchical approach has been pre- most optimizers. Also robust cost function are used for
Part E | 46.2

sented in [46.65]. It builds a multilevel pose-graph and SLAM, for example pseudo Huber and several alterna-
employs an incremental, lazy optimization scheme that tives [46.75, 79–81].
allows for optimizing large graphs and at the same time Graphical SLAM methods have the advantage that
can be executed at each step during mapping. An al- they scale to much higher-dimensional maps than EKF
ternative hierarchical approach is [46.69], which recur- SLAM, exploiting the sparsity of the graph. The key
sively partitions the graph into multiple-level submaps limiting factor in EKF SLAM is the covariance matrix,
using the nested dissection algorithm. which takes space (and update time) quadratic in the
When it comes to computing highly accurate envi- size of the map. No such constraint exists in graphical
ronment reconstructions, approaches that do not only methods. The update time of the graph is constant, and
optimize the poses of the robot but also each indi- the amount of memory required is linear (under some
vidual measurement of a dense sensor often provide mild assumptions). A further advantage of graph-based
better results. In the spirit of bundle adjustment [46.4], methods over the EKF is their ability to constantly re-
approaches for laser scanners [46.70] and Kinect cam- linearize the error function which often leads to better
eras [46.71] have been proposed. results. Performing the optimization can be expensive,
The graphical paradigm can be extended to han- however. Technically, finding the optimal data associa-
dle the data association problems as we can integrate tion is suspected to be an NP-hard problem, although in
additional knowledge on data association into (46.18). practice the number of plausible assignments is usually
Suppose some oracle informed us that landmarks mi small. The continuous optimization of the log likeli-
and mj in the map corresponded to one and the same hood function in (46.18) depends among other things
physical landmark in the world. Then, we can ei- on the number and size of loops in the map. Also the
ther remove mj from the graph and attach all adjacent initialization can have a strong impact on the result and
edges to mi , or we can add a soft correspondence con- a good initial guess can simplify the optimization sub-
straint [46.72] of the form stantially [46.8, 82, 83].
We note that the graph-based paradigm is very
.mj  mi /T .mj  mi / : (46.19) closely linked to information theory, in that the soft con-
straints constitute the information the robot has on the
Here is 2-by-2 diagonal matrix whose coefficients de- world (in an information-theoretic sense [46.92]). Most
termine the penalty for not assigning identical locations methods in the field are offline and they optimize for
to two landmarks (hence we want to be large). Since the entire robot path. If the robot path is long, the opti-
graphical methods are usually used for the full SLAM mization may become cumbersome. Over the last five
problem, the optimization can be interleaved with the years, however, incremental optimization techniques
search for the optimal data association. have been proposed that aim at providing a sufficient
Data association errors typically have a strong im- but not necessarily perfect model of the environment
pact in the resulting map estimate. Even a small number at every point in time. This allows a robot to make
of wrong data associations is likely to result in incon- decisions based on the current model, for example, to
sistent map estimates. Recently, novel approaches have determine exploration goals. In this context, incremen-
been proposed that are robust under a certain number of tal variants [46.93, 94] of stochastic gradient descent
false associations. For example, [46.73, 74] propose an techniques [46.8, 91] have been proposed that estimate
iterative procedure that allows for disabling constraints, which part of the graph requires re-optimization given
an action that is associated with a cost. A generalization new sensor data. Incremental methods [46.66, 79, 95] in
of this method introduced in [46.75] formulates [46.74] the smoothing and mapping framework can be execute
as a robust cost function also reducing the compu- at each step of the mapping process and achieve the
tational requirements. Such approaches can deal with performance by variable ordering and selective relin-
a significant number of false associations and still pro- earization. As also evaluated in [46.96] for the SLAM
vide high-quality maps. Consistency checks for loop problem, variable ordering impacts the performance of
closure hypotheses can be found in other approaches the optimization. Others use hierarchical data struc-
as well, both in the front-end [46.76] and in the opti- tures [46.89] and pose-graphs [46.97] combined with
 Simultaneous Localization and Mapping 46.2 The Three Main SLAM Paradigms 1165

Table 46.1 Recent open-source graph-based SLAM implementations

Name Comment
Dynamic covariance Optimization with a robust cost function for dealing with outliers
scaling (DCS) [46.75] Integrated into g2 o
g2 o [46.80] Flexible and easily extendable optimization framework for SLAM
Comes with different optimization approaches and error functions
Supports external plugins

Part E | 46.2
GTSAM2.1 [46.79] Flexible optimization framework for SLAM and SFMstructure from motion
Implements direct and iterative optimization techniques
Implements smoothing and mapping (SAM), iSAM, and iSAM2
Implements bundle adjustment for Visual SLAM and SFM
HOG-Man [46.65] Incremental optimization approach via hierarchical pose graphs and lazy optimization
Requires pose-graphs with full rank constraints
iSAM2 [46.66] General incremental nonlinear optimization with variable elimination
Variable re-ordering to retain sparsity
On-demand re-linearization of selected variables
KinFu (KinectFusion Open source reimplementation of KinectFusion [46.84] within the point cloud library (PCL)
reimplemented) Dense and highly accurate reconstruction using a Kinect camera
Currently limited to medium sized rooms
MaxMixture [46.78] Optimization for multimodal constraints and outliers
Robust to outliers
Plugin for g2 o
Parallel tracking and System for tracking a hand-held monocular camera and observed features
mapping (PTAM) [46.85] Operates on comparably small workspaces
RGBD-SLAM [46.86] Kinect-frontend for HOG-Man and g2 o
Fairly standard combination of SURF matching and RANSAC
ScaViSLAM [46.87] SLAM system for stereo and Kinect-style cameras
Combines local bundle adjustment with sparse global optimization for on-the-fly processing
SLAM6-D [46.88] SLAM system that operates on point clouds from 3-D laser data
Applies iterative closest point algorithm (ICP) and global relaxation
Sparse surface adjustment Optimizes robot poses and proximity sensor data jointly
(SSA) [46.70, 71] Provides smooth surface estimates
Assumes a range sensor (e.g., laser scanner, Kinect, or similar)
TreeMap [46.89] Incremental optimization approach
Update in O .log N/ time
Provides only a mean estimate
TORO [46.90] Optimization approach that extends stochastic gradient descent (SGD) [46.91]
Robust under bad initial guesses
Recovers quickly from large errors but slow convergence at minimum
Assumes that constraints have roughly spherical covariance matrices
Provides only a mean estimate
Vertigo [46.74] Switchable constraints for robust optimization
Plugin for g2 o

a lazy optimization for on-the-fly mapping [46.65]. As on motivations that are not dissimilar to the graphical
an alternative to global methods, relative optimization approach [46.27, 28, 101].
approaches [46.98] aim at computing locally consistent Recently, researchers addressed the problem of
geometric maps but only topological maps on the global long-term operation and frequent revisits of already
scale. Hybrid approaches [46.87] seek to combine the mapped terrain. To avoid densely connected pose-
best of both worlds. graphs that lead to slow convergence behavior, the robot
There also exists a number of cross-overs that ma- can switch between SLAM and localization, can merge
nipulate the graph online so as to factor out past nodes to avoid a growth of the graph [46.90, 102], or
robot location variables. The resulting algorithms are can discard nodes or edges [46.32, 103–105].
filters [46.25, 33, 99, 100], and they tend to be inti- Graphical and optimization-based SLAM algorithm
mately related to information filter methods. Many of are still subject of intense research and the paradigm
the original attempts to decompose EKF SLAM repre- scales to maps large numbers of nodes [46.25, 55, 57,
sentations into smaller submaps to scale up are based 59, 63–65, 89, 90, 106, 107]. Arguably, the graph-based
1166 Part E Moving in the Environment

paradigm has generated some the largest SLAM maps become independent, and hence are decorrelated. As
ever built. Furthermore, the SLAM community started a result, FastSLAM can represent the posterior by
to release flexible optimization frameworks and SLAM a sampled robot pose, and many local, independent
implementations under open source licenses to sup- Gaussians for its landmarks. The particle representation
port further developments and to allow for efficient offers advantages for SLAM as it allows for compu-
comparisons, (Table 46.1). Especially the optimization tationally efficient updates and for sampling over data
frameworks [46.66, 79, 80] are flexible and powerful associations. On the negative side, the number of neces-
Part E | 46.3

state of the art tools for developing graph-based SLAM sary particles can grow very large, especially for robots
systems. They can be either used as a black box or can seeking to map multiple nested loops.
be easily extended though plugins. Graph-based methods address the full SLAM prob-
lem, hence are in the standard formulation not online.
46.2.4 Relation of Paradigms They draw their intuition form the fact that SLAM
can be modeled by a sparse graph of soft constraints,
The three paradigms just discussed cover the vast ma- where each constraint either corresponds to a motion or
jority of work in the field of SLAM. As discussed, a measurement event. Due to the availability of highly
EKF SLAM comes with a computational hurdle that efficient optimization methods for sparse nonlinear op-
poses serious scaling limitations and the linearization timization problems, graph-based SLAM has become
may lead to inconsistent maps. The most promising ex- the method of choice for building large-scale maps.
tensions of EKF SLAM are based on building local Recent developments have brought up several graph-
submaps; however, in many ways the resulting algo- based methods for incremental map building that can
rithms resemble the graph-based approach. be executed at every time step during navigation. Data
Particle filter methods sidestep some of the is- association search can be incorporated into the basic
sues arising from the natural inter-feature correlations mathematical framework and different approaches that
in the map – which hindered the EKF. By sampling are even robust under wrong data associations are avail-
from robot poses, the individual landmarks in the map able today.

46.3 Visual and RGB-D SLAM

A popular and important topic in recent years has been early approaches were hampered by the lack of suffi-
Visual SLAM – the challenge of building maps and cient computational resources to handle massive video
tracking the robot pose in full 6-DOF using data from data streams. Early approaches were typically based
cameras [46.108] or RGB-D (Kinect) sensors [46.86, on extended Kalman filters [46.113–116], but did not
109]. Visual sensors offer a wealth of information that compute the full covariance for the feature poses and
enables the construction of rich 3-D models of the camera trajectory, resulting in a loss of consistency.
world. They also enable difficult issues such as loop- Visual SLAM is closely related to the structure from
closing to be addressed in novel ways using appearance motion (SFM) problem in computer vision [46.4, 5].
information [46.110]. Visual SLAM is anticipated to Historically, SFM was primarily concerned with off-
be a critical area for future research in perception for line batch processing, whereas SLAM seeks to achieve
robotics, as we seek to develop low-cost systems that a solution for online operation, suitable for closed-loop
are capably of intelligent physical interaction with the interaction of a robot or user with its environment. In
world. comparison to laser scanners, cameras provide a fire hy-
Attempting SLAM with monocular, stereo, om- drant of information, making online processing nearly
nidirectional, or RGB-D cameras raises the level-of- impossible until recent increases in computation have
difficulty of many of the SLAM components, such become available.
as data association and computational efficiency, de- Davison was an early pioneer in developing com-
scribed above. A key challenge is robustness. Many plete visual SLAM systems, initially using a real-time
visual SLAM applications of interest, such as aug- active stereo head [46.121] that tracked distinctive vi-
mented reality [46.85], entail handheld camera motions, sual features with a full covariance EKF approach. Sub-
which present greater difficulties for state estimation, sequent work developed the first real-time SLAM sys-
in comparison to the motion of a wheeled robot across tem that operated with a single freely moving camera
a flat floor. as the only data source [46.23, 122]. This system could
Visual navigation and mapping was a key early goal build sparse, room-size maps of indoor scenes at 30 Hz
in the mobile robotics community [46.111, 112], but frame-rate in real-time, a notable historical achievement
 Simultaneous Localization and Mapping 46.3 Visual and RGB-D SLAM 1167

a) b)
– 200 – 140

– 120
– 150
– 100

– 100 – 80

Part E | 46.3
x (m)
– 60
– 50 – 40

– 20
0
0

50 20
–50 0 50 100 150 200 250 –50 0 50 100 150 200 250
y (m) y (m)

Fig. 46.8 (a) A 2-km path and 50 000 frames estimated for the New College Dataset (after [46.117]) using relative
bundle adjustment (after [46.98]). (b) the relative bundle adjustment solution is easily improved by taking FAB-MAP
(after [46.118]) loop-closures into account – this is achieved without global optimization (after [46.119]). Sibley et al.
advocate that relative metric accuracy and topological consistency are the requirements for autonomous navigation,
and these are better achieved using a relative manifold representation instead of using a conventional single Euclidean
representation [46.120]

a) b)

Fig. 46.9 (a) 3-D Model and (b) close-up view of a corridor environment in the Paul G. Allen building at University of
Washington built from Kinect data (after [46.109]; image courtesy of Peter Henry, University of Washington)

in visual SLAM research. A difficulty encountered with A milestone in creating robust visual SLAM sys-
initial monocular SLAM [46.23] was coping with the tems was the introduction of keyframes in parallel
initialization of points that were far away from the cam- tracking and mapping (PTAM) [46.85], which sep-
era, due to nonGaussian distributions of such feature lo- arated the tasks of keyframe mapping and localiza-
cations resulting from poor depth information. This lim- tion into parallel threads, improving robustness and
itation was overcome in [46.24], introducing an inverse performance for online processing. Keyframes are
depth parameterization for monocular SLAM, a key now a mainstream concept for complexity reduc-
development for enabling a unified treatment of initial- tion in visual SLAM systems. Related approaches
ization and tracking of visual features in real-time. using keyframes include [46.87, 123–126]. The work
1168 Part E Moving in the Environment

in [46.108] analyzes the tradeoffs between filtering and provements in mapping and navigation systems for in-
keyframe-based bundle adjustment in visual SLAM, door environments. State-of-the-art RGB-D SLAM sys-
and concluded that keyframe bundle adjustment outper- tems include [46.109] and [46.86]. Figure 46.9 shows
forms filtering, as it provides the most accuracy per unit examples of the output of these systems.
of computing time. Other researchers aim at exploiting the surface
As pointed out by Davison and other researchers, properties of scanned environments to correct for sen-
an appealing aspect of visual SLAM is that camera
Part E | 46.3

measurements can provide odometry information, and

indeed visual odometry is a key component of modern a)
SLAM systems [46.127]. Here, [46.128] and [46.129]
provide an extensive tutorial of techniques for visual
odometry, including feature detection, feature match-
ing, outlier rejection, and constraint estimation, and
trajectory optimization. Finally, a publicly available vi-
sual odometry library [46.130] that is optimized for
efficient operation on small unmanned aerial vehicles
is available today.
Visual information offers a tremendous source of in-
formation for loop closing, not present in the canonical
2-D laser SLAM systems developed in the early 2000s.
The work in [46.110] was one of the first to employ
techniques for visual object recognition [46.131] to lo-
b)
cation recognition. More recently FAB-MAP [46.118,
132] has demonstrated appearance-only place recogni-
tion at large scale, mapping trajectories with a length
of 1000 km. Combining a bag-of-features approach
with a probabilistic place model and Chow–Liu tree
inference leads to place recognition that is robust
against perceptual aliasing while remaining compu-
tationally efficient. Other work on place recognition
includes [46.133], which combines bag-of-words loop
closing with tests of geometrical consistency based on
conditional random fields (Fig. 46.8).
The techniques described above have formed the
basis for a number of notable large-scale SLAM sys-
tems developed in recent years. A 2008 special is- c)
sue of the IEEE Transactions on Robotics provides
a good snapshot of recent state-of-the-art SLAM tech-
niques [46.135]. Other notable recent examples in-
clude [46.98, 119, 126, 136–138]. The idea of employ-
ing relative bundle adjustment [46.98] to compute a full
maximum likelihood solution in an online fashion,
even for loop closures, by employing a manifold rep-
resentation that does not attempt to enforce Euclidean
constraints results in maps that can be computed at
high frame rate (see also Fig. 46.8). Finally, view-based
mapping systems [46.126, 136, 137] aim at large-scale
and/or life-long visual mapping based on the pose
graph optimization techniques described above in Sec-
tion 46.2.3. Fig.46.10a–c Results obtained with KinectFusion (af-
Several compelling 3-D mapping and localization ter [46.134]). (a) A local scene as a normal map and (b) as
have been created in recent years with RGB-D (Kinect) a Phong-shaded rendering. The (c) image depicts a larger
sensors. The combination of direct range measurements scene (image courtesy of Richard Newcombe, Imperial
with dense visual imagery can enable dramatic im- College London)
 Simultaneous Localization and Mapping 46.4 Conclusion and Future Challenges 1169

Fig. 46.11 Spatially ex-

tended KinectFusion output
produced in real-time with
Kintinuous (after [46.139])

Part E | 46.4
sor noise of range sensors such as the Kinect [46.71]. via highly parallelized operations on commodity GPU
They jointly optimize the poses of the sensor and hardware to yield a system that outperforms previ-
the positions of the surface points measured and it- ous methods such as PTAM for challenging camera
eratively refine the structure of the error function trajectories. Dense methods offer an interesting per-
by recomputing the data associations after each opti- spective from which to address long-standing problems,
mization, resulting in accurate smooth models of the such as visual odometry, from a fresh perspective,
environment. without requiring explicit feature detection and match-
An emerging area for future research is the de- ing [46.141]. KinectFusion [46.84, 134] is a dense mod-
velopment of fully dense processing methods that eling system that tracks the 3-D pose of a handheld
exploit recent advances in commodity graphical pro- Kinect while concurrently reconstructing high-quality
cessing unit (GPU) technology. Kinect-based dense scene 3-D models in real-time. See Fig. 46.10 for an
tracking and mapping, a fully-dense method for small- example. KinectFusion has been applied to spatially ex-
scale visual tracking and reconstruction is described tended environments in [46.139, 142]. An example is
in [46.140]. Dense modeling and tracking are achieved shown in Fig. 46.11.

46.4 Conclusion and Future Challenges

This chapter has provided an introduction into SLAM, The following references provide an in-depth tuto-
which is defined as the problem faced by a mobile rial on SLAM and much greater depth of coverage on
platform roaming an unknown environment, and seek- the details of popular SLAM algorithms. Furthermore,
ing to localize itself while concurrently building a map several implementations of popular SLAM systems, in-
of the environment. The chapter discussed three main cluding most of the approaches listed in Table 46.1,
paradigms in SLAM, which are based on the ex- can be found in online resources such as http://www.
tended Kalman filter, particle filters, and graph-based openslam.org or in the references [46.6, 9, 21, 62].
sparse optimization techniques, and then described re- The considerable progress in SLAM in the past
cent progress in Visual/Kinect SLAM. decade is beyond doubt. The core state estimation at
1170 Part E Moving in the Environment

the heart of SLAM is now quite well understood, An ultimate goal is to realize the challenge of per-
and a number of impressive implementations have sistent navigation and mapping – the capability for
been developed, including several widely used open a robot to perform SLAM robustly for days, weeks,
source software implementations and some commer- or months at a time with minimal human supervi-
cial projects. None-the-less, a number of open research sion, in complex and dynamic environments. Taking
challenges remain for the general problem of robotic the limit as t ! 1 poses difficult challenges to most
mapping in complex and dynamic environments over current algorithms; in fact, most robot mapping and
Part E | 46

extended periods of time, including robots sharing, ex- navigation algorithms are doomed to fail with the
tending, and revising previously built models, efficient passage of time, as errors inevitably accrue. Despite
failure recovery, zero user intervention, and operation recent encouraging solutions [46.102, 105], more re-
on resource-constrained systems. Another exciting area search is needed for techniques that can recover from
for the future is the further development of fully dense mistakes and enable robots to deal with changes in
visual mapping systems exploiting the latest advances the environment and enabling a long-term autonomous
in GPU hardware development. existence.

Video-References

VIDEO 439 Deformation-based loop closure for Dense RGB-D SLAM

available from http://handbookofrobotics.org/view-chapter/46/videodetails/439
VIDEO 440 Large-scale SLAM using the Atlas framework
available from http://handbookofrobotics.org/view-chapter/46/videodetails/440
VIDEO 441 Graph-based SLAM
available from http://handbookofrobotics.org/view-chapter/46/videodetails/441
VIDEO 442 Graph-based SLAM
available from http://handbookofrobotics.org/view-chapter/46/videodetails/442
VIDEO 443 Graph-based SLAM
available from http://handbookofrobotics.org/view-chapter/46/videodetails/443
VIDEO 444 Graph-based SLAM
available from http://handbookofrobotics.org/view-chapter/46/videodetails/444
VIDEO 445 Graph-based SLAM
available from http://handbookofrobotics.org/view-chapter/46/videodetails/445
VIDEO 446 Graph-based SLAM using TORO
available from http://handbookofrobotics.org/view-chapter/46/videodetails/446
VIDEO 447 Sparse pose adjustment
available from http://handbookofrobotics.org/view-chapter/46/videodetails/447
VIDEO 449 Pose graph compression for laser-based SLAM
available from http://handbookofrobotics.org/view-chapter/46/videodetails/449
VIDEO 450 Pose graph compression for laser-based SLAM
available from http://handbookofrobotics.org/view-chapter/46/videodetails/450
VIDEO 451 Pose graph compression for laser-based SLAM
available from http://handbookofrobotics.org/view-chapter/46/videodetails/451
VIDEO 452 DTAM: Dense tracking and mapping in real-time
available from http://handbookofrobotics.org/view-chapter/46/videodetails/452
VIDEO 453 MonoSLAM: Real-time single camera SLAM
available from http://handbookofrobotics.org/view-chapter/46/videodetails/453
VIDEO 454 SLAM++: Simultaneous localisation and mapping at the level of objects
available from http://handbookofrobotics.org/view-chapter/46/videodetails/454
VIDEO 455 Extended Kalman filter SLAM
available from http://handbookofrobotics.org/view-chapter/46/videodetails/455
 Simultaneous Localization and Mapping References 1171

References

46.1 C.F. Gauss: Theoria Motus Corporum Coelestium IEEE Trans. Robotics Autom. 17(6), 890–897
(Theory of the Motion of the Heavenly Bodies Mov- (2001)
ing about the Sun in Conic Sections) (Perthes and 46.19 G.D. Tipaldi, M. Braun, K.O. Arras: Flirt: interest
Bessen, Hamburg 1809), Republished in 1857 and regions for 2D range data with applications to
by Dover in 1963 robot navigation, Proc. Int. Symp. Exp. Robotics
46.2 D.C. Brown: The bundle adjustment – Progress (ISER) (2010)

Part E | 46
and prospects, Int. Arch. Photogramm. 21(3), 3:3– 46.20 G.D. Tipaldi, L. Spinello, W. Burgard: Geometrical
3:35 (1976) flirt phrases for large scale place recognition in 2D
46.3 G. Konecny: Geoinformation: Remote Sensing, range data, Proc. IEEE Int. Conf. Robotics Autom.
Photogrammetry and Geographical Information (ICRA) (2013)
Systems (Taylor Francis, London 2002) 46.21 T. Bailey: Mobile Robot Localisation and Mapping
46.4 B. Triggs, P. McLauchlan, R. Hartley, A. Fitzgibbon: in Extensive Outdoor Environments, Ph.D. Thesis
Bundle adjustment – A modern synthesis, Lect. (Univ. of Sydney, Sydney 2002)
Notes Comput. Sci. 62, 298–372 (2000) 46.22 J.J. Leonard, R.R. Rikoski, P.M. Newman, M. Bosse:
46.5 R. Hartley, A. Zisserman: Multiple View Geometry Mapping partially observable features from mul-
in Computer Vision (Cambridge Univ. Press, Cam- tiple uncertain vantage points, Int. J. Robotics
bridge 2003) Res. 21(10), 943–975 (2002)
46.6 T. Bailey, H.F. Durrant-Whyte: Simultaneous lo- 46.23 A.J. Davison: Real-time simultaneous localisation
calisation and mapping (SLAM): Part II, Robotics and mapping with a single camera, Proc. IEEE 9th
Autom. Mag. 13(3), 108–117 (2006) Int. Conf. Comput. Vis. (2003) pp. 1403–1410
46.7 H.F. Durrant-Whyte, T. Bailey: Simultaneous lo- 46.24 J.M.M. Montiel, J. Civera, A.J. Davison: Unified in-
calisation and mapping (SLAM): Part I, Robotics verse depth parametrization for monocular SLAM,
Autom. Mag. 13(2), 99–110 (2006) Robotics Sci. Syst., Vol. 1 (2006)
46.8 G. Grisetti, C. Stachniss, W. Burgard: Nonlinear 46.25 M. Bosse, P.M. Newman, J. Leonard, S. Teller:
constraint network optimization for efficient map Simultaneous localization and map building in
learning, IEEE Trans. Intell. Transp. Syst. 10(3), large-scale cyclic environments using the Atlas
428–439 (2009) Framework, Int. J. Robotics Res. 23(12), 1113–1139
46.9 S. Thrun, W. Burgard, D. Fox: Probabilistic (2004)
Robotics (MIT Press, Cambridge, 2005) 46.26 J. Nieto, T. Bailey, E. Nebot: Scan-SLAM: Combin-
46.10 R. Smith, M. Self, P. Cheeseman: A stochastic ing EKF-SLAM and scan correlation, Proc. IEEE Int.
map for uncertain spatial relationships, Proc. Int. Conf. Robotics Autom. (ICRA) (2005)
Symp. Robotics Res. (ISRR) (MIT Press, Cambridge 46.27 J. Guivant, E. Nebot: Optimization of the si-
1988) pp. 467–474 multaneous localization and map building algo-
46.11 R. Smith, M. Self, P. Cheeseman: Estimating un- rithm for real time implementation, IEEE Trans.
certain spatial relationships in robotics. In: Au- Robotics. Autom. 17(3), 242–257 (2001)
tonomous Robot Vehicles, ed. by I.J. Cox, G.T. Wil- 46.28 J.J. Leonard, H. Feder: A computationally efficient
fong (Springer Verlag, Berlin, Heidelberg 1990) method for large-scale concurrent mapping and
pp. 167–193 localization, Proc. 9th Int. Symp. Robotics Res.
46.12 P. Moutarlier, R. Chatila: Stochastic multisensory (ISRR), ed. by J. Hollerbach, D. Koditschek (1999)
data fusion for mobile robot location and envi- pp. 169–176
ronment modeling, 5th Int. Symp. Robotics Res. 46.29 J.D. Tardós, J. Neira, P.M. Newman, J.J. Leonard:
(ISRR) (1989) pp. 207–216 Robust mapping and localization in indoor envi-
46.13 P. Moutarlier, R. Chatila: An experimental sys- ronments using sonar data, Int. J. Robotics Res.
tem for incremental environment modeling by 21(4), 311–330 (2002)
an autonomous mobile robot, 1st Int. Sym. Exp. 46.30 S.B. Williams, G. Dissanayake, H.F. Durrant-
Robotics (ISER) (1990) Whyte: Towards multi-vehicle simultaneous lo-
46.14 R. Kalman: A new approach to linear filtering and calisation and mapping, Proc. IEEE Int. Conf.
prediction problems, J. Fluids 82, 35–45 (1960) Robotics Autom. (ICRA) (2002) pp. 2743–2748
46.15 A.M. Jazwinsky: Stochastic Processes and Filtering 46.31 R.M. Eustice, H. Singh, J.J. Leonard: Exactly sparse
Theory (Academic, New York 1970) delayed-state filters for view-based SLAM, IEEE
46.16 M.G. Dissanayake, P.M. Newman, S. Clark, Trans. Robotics 22(6), 1100–1114 (2006)
H.F. Durrant-Whyte, M. Csorba: A solution to 46.32 V. Ila, J.M. Porta, J. Andrade-Cetto: Information-
the simultaneous localization and map building based compact pose SLAM, IEEE Trans. Robotics
(SLAM) Problem, IEEE Trans. Robotics Autom. 17(3), 26(1), 78–93 (2010)
229–241 (2001) 46.33 S. Thrun, D. Koller, Z. Ghahramani, H.F. Durrant-
46.17 J. Neira, J. Tardos, J. Castellanos: Linear time Whyte, A.Y. Ng: Simultaneous mapping and lo-
vehicle relocation in SLAM, Proc. IEEE Int. Conf. calization with sparse extended information fil-
Robotics Autom. (ICRA) (2003) pp. 427–433 ters, Proc.5th Int. Workshop Algorithmic Found.
46.18 J. Neira, J.D. Tardos: Data association in stochas- Robotics, ed. by J.-D. Boissonnat, J. Burdick,
tic mapping using the joint compatibility test, K. Goldberg, S. Hutchinson (2002)
1172 Part E Moving in the Environment

46.34 M.R. Walter, R.M. Eustice, J.J. Leonard: Exactly wellized particle filters, J. Robotics Auton. Syst.
sparse extended information filters for feature- 55(1), 30–38 (2007)
based SLAM, Int. J. Robotics Res. 26(4), 335–359 46.51 D. Roller, M. Montemerlo, S. Thrun, B. Wegbreit:
(2007) FastSLAM 2.0: An improved particle filtering algo-
46.35 T. Bailey, J. Nieto, J. Guivant, M. Stevens, E. Nebot: rithm for simultaneous localization and mapping
Consistency of the EKF-SLAM algorithm, Proc. that provably converges, Int. Jt. Conf. Artif. Intell.
IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS) (2006) (IICAI) (Morgan Kaufmann, San Francisco 2003)
pp. 3562–3568 pp. 1151–1156
Part E | 46

46.36 S. Huang, G. Dissanayake: Convergence and con- 46.52 R. van der Merwe, N. de Freitas, A. Doucet, E. Wan:
sistency analysis for extended Kalman filter based The unscented particle filter, Proc. Adv. Neural In-
SLAM, IEEE Trans. Robotics 23(5), 1036–1049 (2007) form. Process. Syst. Conf. (2000) pp. 584–590
46.37 G.P. Huang, A.I. Mourikis, S.I. Roumeliotis: Ob- 46.53 C. Stachniss, G. Grisetti, W. Burgard: Recovering
servability-based Rules for Designing Consistent particle diversity in a Rao-Blackwellized parti-
EKF SLAM Estimators, Int. J. Robotics Res. 29, 502– cle filter for SLAM after actively closing loops,
528 (2010) Proc. IEEE Int. Conf. Robotics Autom. (ICRA) (2005)
46.38 N. Metropolis, S. Ulam: The Monte Carlo method, pp. 655–660
J. Am. Stat. Assoc. 44(247), 335–341 (1949) 46.54 F. Lu, E. Milios: Globally consistent range scan
46.39 D. Blackwell: Conditional expectation and unbi- alignment for environmental mapping, Auton.
ased sequential estimation, Ann. Math. Stat. 18, Robots 4, 333–349 (1997)
105–110 (1947) 46.55 F. Dellaert: Square root SAM, Robotics Sci. Syst.,
46.40 C.R. Rao: Information and accuracy obtainable in ed. by S. Thrun, G. Sukhatme, S. Schaal, O. Brock
estimation of statistical parameters, Bull. Calcutta (MIT Press, Cambridge 2005)
Math. Soc. 37(3), 81–91 (1945) 46.56 T. Duckett, S. Marsland, J. Shapiro: Learning glob-
46.41 K. Murphy, S. Russel: Rao–Blackwellized parti- ally consistent maps by relaxation, Proc. IEEE Int.
cle filtering for dynamic Bayesian networks. In: Conf. Robotics Autom. (ICRA) (2000) pp. 3841–3846
Sequential Monte Carlo Methods in Practice, ed. 46.57 T. Duckett, S. Marsland, J. Shapiro: Fast, on-
by A. Docout, N. de Freitas, N. Gordon (Springer, line learning of globally consistent maps, Auton.
Berlin 2001) pp. 499–516 Robots 12(3), 287–300 (2002)
46.42 M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit: 46.58 J. Folkesson, H.I. Christensen: Graphical SLAM:
FastSLAM: A factored solution to the simultane- A self-correcting map, Proc. IEEE Int. Conf.
ous localization and mapping problem, Proc. AAAI Robotics Autom. (ICRA) (2004) pp. 383–390
Natl. Conf. Artif. Intell. (2002) 46.59 U. Frese, G. Hirzinger: Simultaneous localization
46.43 J. Pearl: Probabilistic Reasoning in Intelligent and mapping – A discussion, Proc. IJCAI Workshop
Systems: Networks of Plausible Inference (Morgan Reason. Uncertain. Robotics (2001) pp. 17–26
Kaufmann, San Mateo 1988) 46.60 M. Golfarelli, D. Maio, S. Rizzi: Elastic correction
46.44 J. Guivant, E. Nebot, S. Baiker: Autonomous nav- of dead-reckoning errors in map building, Proc.
igation and map building using laser range sen- IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS) (1998)
sors in outdoor applications, J. Robotics Syst. pp. 905–911
17(10), 565–583 (2000) 46.61 J. Gutmann, K. Konolige: Incremental mapping of
46.45 G. Grisetti, C. Stachniss, W. Burgard: Improved large cyclic environments, Proc. IEEE Int. Symp.
techniques for grid mapping with Rao-Black- Comput. Intell. Robotics Autom. (CIRA) (2000)
wellized particle filters, IEEE Trans. Robotics 23, pp. 318–325
34–46 (2007) 46.62 G. Grisetti, R. Kümmerle, C. Stachniss, W. Burgard:
46.46 D. Hähnel, D. Fox, W. Burgard, S. Thrun: A highly A Tutorial on Graph-based SLAM, IEEE Trans. Intell.
efficient FastSLAM algorithm for generating cyclic Transp. Syst. Mag. 2, 31–43 (2010)
maps of large-scale environments from raw laser 46.63 K. Konolige: Large-scale map-making, Proc. AAAI
range measurements, Proc. IEEE/RSJ Int. Conf. In- Natl. Conf. Artif. Intell. (MIT Press, Cambridge
tell. Robots Syst. (IROS) (2003) 2004) pp. 457–463
46.47 C. Stachniss, G. Grisetti, W. Burgard, N. Roy: 46.64 M. Montemerlo, S. Thrun: Large-scale robotic 3-D
Evaluation of gaussian proposal distributions for mapping of urban structures, Springer Tract. Adv.
mapping with rao-blackwellized particle filters, Robotics 21, 141–150 (2005)
Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS) 46.65 G. Grisetti, R. Kümmerle, C. Stachniss, U. Frese,
(2007) C. Hertzberg: Hierarchical optimization on mani-
46.48 A. Eliazar, R. Parr: DP-SLAM: Fast, robust simulta- folds for online 2D and 3D mapping, Proc. IEEE Int.
neous localization and mapping without prede- Conf. Robotics Autom. (ICRA) (2010)
termined landmarks, Proc. 16th Int. Jt. Conf. Artif. 46.66 M. Kaess, H. Johannsson, R. Roberts, V. Ila,
Intell. (IJCAI) (2003) pp. 1135–1142 J.J. Leonard, F. Dellaert: iSAM2: Incremental
46.49 A. Eliazar, R. Parr: DP-SLAM 2.0, Proc. IEEE smoothing and mapping using the Bayes tree,
Int. Conf. Robotics Autom. (ICRA), Vol. 2 (2004) Int. J. Robotics Res. 31, 217–236 (2012)
pp. 1314–1320 46.67 M. Kaess, A. Ranganathan, F. Dellaert: iSAM: In-
46.50 G. Grisetti, G.D. Tipaldi, C. Stachniss, W. Burgard, cremental Smoothing and Mapping, IEEE Trans.
D. Nardi: Fast and accurate SLAM with Rao-Black- Robotics 24(6), 1365–1378 (2008)
 Simultaneous Localization and Mapping References 1173

46.68 C. Estrada, J. Neira, J.D. Tardós: Hierarchical 46.85 G. Klein, D. Murray: Parallel tracking and map-
SLAM: Real-time accurate mapping of large en- ping for small AR workspaces, IEEE ACM Int. Symp.
vironments, IEEE Trans. Robotics 21(4), 588–596 Mixed Augment. Real. (ISMAR) (2007) pp. 225–
(2005) 234
46.69 K. Ni, F. Dellaert: Multi-level submap based SLAM 46.86 F. Endres, J. Hess, N. Engelhard, J. Sturm, D. Cre-
using nested dissection, Proc. IEEE/RSJ Int. Conf. mers, W. Burgard: An evaluation of the RGB-D
Intell. Robots Syst. (IROS) (2010) SLAM system, Proc. IEEE Int. Conf. Robotics Autom.
46.70 M. Ruhnke, R. Kümmerle, G. Grisetti, W. Burgard: (ICRA) (2012)

Part E | 46
Highly accurate maximum likelihood laser map- 46.87 H. Strasdat, A.J. Davison, J.M.M. Montiel, K. Kono-
ping by jointly optimizing laser points and robot lige: Double window optimisation for constant
poses, Proc. IEEE Int. Conf. Robotics Autom. (ICRA) time visual SLAM, Int. Conf. Computer Vis. (ICCV)
(2011) (2011)
46.71 M. Ruhnke, R. Kümmerle, G. Grisetti, W. Burgard: 46.88 A. Nüchter: 3D robot mapping, Springer Tract. Adv.
Highly accurate 3D surface models by sparse sur- Robotics 52 (2009)
face adjustment, Proc. IEEE Int. Conf. Robotics 46.89 U. Frese: Treemap: An O.log n/ algorithm for in-
Autom. (ICRA) (2012) door simultaneous localization and mapping,
46.72 Y. Liu, S. Thrun: Results for outdoor-SLAM using Auton. Robots 21(2), 103–122 (2006)
sparse extended information filters, Proc. IEEE Int. 46.90 G. Grisetti, C. Stachniss, S. Grzonka, W. Burgard:
Conf. Robotics Autom. (ICRA) (2003) A tree parameterization for efficiently computing
46.73 N. Sünderhauf, P. Protzel: BRIEF-Gist – Closing the maximum likelihood maps using gradient de-
loop by simple means, Proc. IEEE/RSJ Int. Conf. In- scent, Robotics Sci. Syst. (2007)
tell. Robots Syst. (IROS) (2011) pp. 1234–1241 46.91 E. Olson, J.J. Leonard, S. Teller: Fast iterative
46.74 N. Sünderhauf, P. Protzel: Switchable constraints alignment of pose graphs with poor initial esti-
for robust pose graph SLAM, Proc. IEEE/RSJ Int. mates, Proc. IEEE Int. Conf. Robotics Autom. (ICRA)
Conf. Intell. Robots Syst. (IROS) (2012) (2006) pp. 2262–2269
46.75 P. Agarwal, G.D. Tipaldi, L. Spinello, C. Stach- 46.92 T.M. Cover, J.A. Thomas: Elements of Information
niss, W. Burgard: Robust map optimization using Theory (Wiley, New York 1991)
dynamic covariance scaling, Proc. IEEE Int. Conf. 46.93 E. Olson, J.J. Leonard, S. Teller: Spatially-adap-
Robotics Autom. (ICRA) (2013) tive learning rates for online incremental SLAM,
46.76 E. Olson: Recognizing places using spectrally clus- Robotics Sci. Syst. (2007)
tered local matches, J. Robotics Auton. Syst. 46.94 G. Grisetti, D. Lordi Rizzini, C. Stachniss, E. Olson,
57(12), 1157–1172 (2009) W. Burgard: Online constraint network optimiza-
46.77 Y. Latif, C. Cadena Lerma, J. Neira: Robust loop tion for efficient maximum likelihood map learn-
closing over time, Robotics Sci. Syst. (2012) ing, Proc. IEEE Int. Conf. Robotics Autom. (ICRA)
46.78 E. Olson, P. Agarwal: Inference on networks of (2008)
mixtures for robust robot mapping, Robotics Sci. 46.95 M. Kaess, A. Ranganathan, F. Dellaert: Fast incre-
Syst. (2012) mental square root information smoothing, Int.
46.79 F. Dellaert: Factor graphs and GTSAM: A hands-on Jt. Conf. Artif. Intell. (ISCAI) (2007)
introduction, Tech. Rep. GT-RIM-CP & R-2012-002 46.96 P. Agarwal, E. Olson: Evaluating variable reorder-
(Georgia Tech, Atlanta 2012) ing strategies for SLAM, Proc. IEEE/RSJ Int. Conf.
46.80 R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, Intel Robots Syst. (IROS) (2012)
W. Burgard: G²o: A general framework for graph 46.97 K. Ni, D. Steedly, F. Dellaert: Tectonic SAM: exact,
optimization, Proc. IEEE Int. Conf. Robotics Autom. out-of-core, submap-based SLAM, Proc. IEEE Int.
(ICRA) (2011) Conf. Robotics Autom. (ICRA) (2007) pp. 1678–1685
46.81 D.M. Rosen, M. Kaess, J.J. Leonard: An incre- 46.98 G. Sibley, C. Mei, I. Reid, P. Newman: Adaptive
mental trust-region method for robust online relative bundle adjustment, Robotics Sci. Syst.
sparse least-squares estimation, Proc. IEEE Int. (2009)
Conf. Robotics Autom. (ICRA) (2012) pp. 1262– 46.99 P.M. Newman, J.J. Leonard, R. Rikoski: Towards
1269 constant-time SLAM on an autonomous under-
46.82 L. Carlone, R. Aragues, J. Castellanos, B. Bona: water vehicle using synthetic aperture sonar, 11th
A linear approximation for graph-based simul- Int. Symp. Robotics Res. (2003)
taneous localization and mapping, Robotics Sci. 46.100 M.A. Paskin: Thin junction tree filters for simul-
Syst. (2011) taneous localization and mapping, Int. Jt. Conf.
46.83 G. Grisetti, R. Kümmerle, K. Ni: Robust optimiza- Artif. Intell. (IJCAI) (Morgan Kaufmann, New York
tion of factor graphs by using condensed mea- 2003) pp. 1157–1164
surements, Proc. IEEE/RSJ Int. Conf. Intell. Robots 46.101 S.B. Williams: Efficient Solutions to Autonomous
Syst. (IROS) (2012) Mapping and Navigation Problems, Ph.D. Thesis
46.84 S. Izadi, R.A. Newcombe, D. Kim, O. Hilliges, (ACFR Univ. Sydney, Sydney 2001)
D. Molyneaux, S. Hodges, P. Kohli, J. Shotton, 46.102 H. Johannsson, M. Kaess, M.F. Fallon, J.J. Leonard:
A.J. Davison, A. Fitzgibbon: Kinectfusion: Real- Temporally scalable visual SLAM using a reduced
time dynamic 3D surface reconstruction and in- pose graph, RSS Workshop Long-term Oper. Au-
teraction, ACM SIGGRAPH Talks (2011) p. 23 ton. Robotic Syst. Chang. Environ. (2012)
1174 Part E Moving in the Environment

46.103 N. Carlevaris-Bianco, R.M. Eustice: Generic factor- dle adjustment, Int. J. Robotics Res. 29(8), 958–
based node marginalization and edge sparsifi- 980 (2010)
cation for pose-graph SLAM, Proc. IEEE Int. Conf. 46.121 A. Davison, D. Murray: Mobile robot localisation
Robotics Autom. (ICRA) (2013) using active vision, Eur. Conf. Comput. Vis. (ECCV)
46.104 M. Kaess, F. Dellaert: Covariance recovery from (1998) pp. 809–825
a square root information matrix for data asso- 46.122 A.J. Davison, I. Reid, N. Molton, O. Stasse:
ciation, J. Robotics Auton. Syst. 57(12), 1198–1210 MonoSLAM: Real-time single camera SLAM, IEEE
(2009) Trans., Pattern Anal. Mach. Intell. 29(6), 1052–1067
Part E | 46

46.105 H. Kretzschmar, C. Stachniss: Information-theo- (2007)

retic compression of pose graphs for laser-based 46.123 R.O. Castle, G. Klein, D.W. Murray: Wide-area aug-
SLAM, Int. J. Robotics Res. 31(11), 1219–1230 (2012) mented reality using camera tracking and map-
46.106 U. Frese, L. Schröder: Closing a million-landmarks ping in multiple regions, Comput. Vis. Image Un-
loop, Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. derst. 115(6), 854–867 (2011)
(IROS) (2006) 46.124 E. Eade, T. Drummond: Unified loop closing and
46.107 J. McDonald, M. Kaess, C. Cadena, J. Neira, recovery for real time monocular SLAM, Br. Mach.
J.J. Leonard: Real-time 6-DOF multi-session Vis. Conf. (2008)
visual SLAM over large scale environments, 46.125 E. Eade, P. Fong, M.E. Munich: Monocular
J. Robotics Auton. Syst. 61(10), 1144–1158 (2012) graph SLAM with complexity reduction, Proc.
46.108 H. Strasdat, J.M.M. Montiel, A.J. Davison: Real- IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS) (2010)
time monocular SLAM: Why filter?, Proc. IEEE Int. pp. 3017–3024
Conf. Robotics Autom. (ICRA) (2010) 46.126 K. Konolige, M. Agrawal: FrameSLAM: From bun-
46.109 P. Henry, M. Krainin, E. Herbst, X. Ren, D. Fox: dle adjustment to real-time visual mapping, IEEE
RGB-D mapping: Using depth cameras for dense Trans. Robotics 24(5), 1066–1077 (2008)
3D modeling of indoor environments, Int. 46.127 D. Nister, O. Naroditsky, J. Bergen: Visual odometry
J. Robotics Res. 31(5), 647–663 (2012) for ground vehicle applications, J. Field Robotics
46.110 D. Nister, H. Stewenius: Scalable recognition with 23(1), 3–20 (2006)
a vocabulary tree, Proc. IEEE Int. Conf. Comput. 46.128 D. Scaramuzza, F. Fraundorfer: Visual odometry.
Vis. Pattern Recognit. (ICCVPR) (2006) pp. 2161– Part I: The first 30 years and fundamentals, IEEE
2168 Robotics Autom. Mag. 18(4), 80–92 (2011)
46.111 R.A. Brooks: Aspects of mobile robot visual map 46.129 F. Fraundorfer, D. Scaramuzza: Visual odometry.
making, Proc. Int. Symp. Robotics Res. (ISRR) (MIT Part II: Matching, robustness, optimization, and
Press, Cambridge 1984) pp. 287–293 applications, IEEE Robotics Autom. Mag. 19(2), 78–
46.112 H. Moravec: Obstacle Avoidance and Navigation 90 (2012)
in the Real World by a Seeing Robot Rover, Ph.D. 46.130 A.S. Huang, A. Bachrach, P. Henry, M. Krainin,
Thesis (Stanford Univ., Stanford 1980) D. Maturana, D. Fox, N. Roy: Visual odometry and
46.113 N. Ayache, O. Faugeras: Building, registrating, and mapping for autonomous flight using an RGB-D
fusing noisy visual maps, Int. J. Robotics Res. 7(6), camera, Proc. Int. Symp. Robotics Res. (ISRR) (2011)
45–65 (1988) 46.131 J. Sivic, A. Zisserman: Video Google: A text re-
46.114 D. Kriegman, E. Triendl, T. Binford: Stereo vi- trieval approach to object matching in videos, Int.
sion and navigation in buildings for mobile Conf. Computer Vis. (ICCV) (2003) p. 1470
robots, IEEE Trans. Robotics Autom. 5(6), 792–803 46.132 M. Cummins, P.M. Newman: Probabilistic ap-
(1989) pearance based navigation and loop closing,
46.115 L. Matthies, S. Shafer: Error modeling in stereo Proc. IEEE Int. Conf. Robotics Autom. (ICRA) (2007)
navigation, IEEE J. Robotics Autom. 3(3), 239–248 pp. 2042–2048
(1987) 46.133 C. Cadena, D. Gálvez, F. Ramos, J.D. Tardós,
46.116 S. Pollard, J. Porrill, J. Mayhew: Predictive feed- J. Neira: Robust place recognition with stereo
forward stereo processing, Alvey Vis. Conf. (1989) cameras, Proc. IEEE/RSJ Int. Conf. Intell. Robots
pp. 97–102 Syst. (IROS) (2010)
46.117 M. Smith, I. Baldwin, W. Churchill, R. Paul, 46.134 R.A. Newcombe, A.J. Davison, S. Izadi, P. Kohli,
P. Newman: The new college vision and laser data O. Hilliges, J. Shotton, D. Molyneaux, S. Hodges,
set, Int. J. Robotics Res. 28(5), 595–599 (2009) D. Kim, A. Fitzgibbon: Kinectfusion: Real-time
46.118 M. Cummins, P.M. Newman: Appearance-only dense surface mapping and tracking, IEEE/ACM
SLAM at large scale with FAB-MAP 2.0, Int. Int. Sym. Mixed Augment. Real. (ISMAR) (2011)
J. Robotics Res. 30(9), 1100–1123 (2010) pp. 127–136
46.119 P.M. Newman, G. Sibley, M. Smith, M. Cum- 46.135 J. Neira, A.J. Davison, J.J. Leonard: Guest editorial
mins, A. Harrison, C. Mei, I. Posner, R. Shade, special issue on visual SLAM, IEEE Trans. Robotics
D. Schroter, L. Murphy, W. Churchill, D. Cole, 24(5), 929–931 (2008)
I. Reid: Navigating, recognising and describing 46.136 K. Konolige, J. Bowman: Towards lifelong visual
urban spaces with vision and laser, Int, J. Robotics maps, Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst.
Res. 28, 11–12 (2009) (IROS) (2009) pp. 1156–1163
46.120 G. Sibley, C. Mei, I. Reid, P. Newman: Vast-scale 46.137 K. Konolige, J. Bowman, J.D. Chen, P. Mihe-
outdoor navigation using adaptive relative bun- lich, M. Calonder, V. Lepetit, P. Fua: View-
 Simultaneous Localization and Mapping References 1175

based maps, Int. J. Robotics Res. 29(8), 941–957 46.140 R.A. Newcombe, S.J. Lovegrove, A.J. Davison:
(2010) DTAM: Dense tracking and mapping in real-time,
46.138 G. Sibley, C. Mei, I. Reid, P. Newman: Planes, Int. Conf. Computer Vis. (ICCV) (2011) pp. 2320–
trains and automobiles – Autonomy for the mod- 2327
ern robot, Proc. IEEE Int. Conf. Robotics Autom. 46.141 F. Steinbruecker, J. Sturm, D. Cremers: Real-
(ICRA) (2010) pp. 285–292 time visual odometry from dense RGB-D images,
46.139 T. Whelan, H. Johannsson, M. Kaess, J.J. Leonard, Workshop Live Dense Reconstr. Mov. Cameras Int.
J.B. McDonald: Robust real-time visual odometry Conf. Comput. Vis. (ICCV) (2011)

Part E | 46
for dense RGB-D mapping, IEEE Int. Conf. Robotics 46.142 H. Roth, M. Vona: Moving volume kinectfusion,
Autom. (ICRA (2013) Br. Mach. Vis. Conf. (2012)