-
Algorithms for Simultaneous Localization and Mapping
Yuncong Chen
February 3, 2013
Abstract
Simultaneous Localization and Mapping (SLAM) is theproblem in
which a sensor-enabled mobile robot incre-mentally builds a map for
an unknown environment,while localizing itself within this map.
Efficient andaccurate SLAM is fundamental for any mobile robot
toperform robust navigation. It is also the cornerstone
forhigher-level tasks such as path planning and exploration.In this
talk, I will survey the three major families ofSLAM algorithms:
parametric filter, particle filter andgraph-based smoother. I will
review the representativealgorithms and the state-of-the-art in
each family. I willalso discuss issues including submapping, data
associa-tion and loop closing.
1 Introduction
Simultaneous Localization and Mapping (SLAM)[Thrunet al., 1998;
Leonard and Durrant-Whyte, 1991; Smithet al., 1987; Smith and
Cheeseman, 1986] is the problemin which a sensor-enabled mobile
robot builds a map foran unknown environment, while localizing
itself relativeto this map. Using the sensors the robot is given
mea-surements and often odometries. Efficient and accurateSLAM is a
fundamental capability of any mobile robotthat is excepted to
perform proper navigation. In addition,it is the cornerstone for
higher-level tasks such as pathplanning and exploration.
SLAM intertwines the two problems of localization(i.e. given a
map, estimate ones location based on rela-tive measurements of the
map) and mapping (i.e. givenones exact location, construct the
map). Each prob-lem has applications of its own. An example of
purelocalization is robotic competitions in highly
constrainedenvironment like the RoboCup, where the position
ofmarkers on the field is clearly defined and can be storedin the
robots memory beforehand. This kind of environ-
ment however, rarely exists outside games. Pure mappingexamples
include a coast-surveying UAV equiped withGPS and IMU that
accurately feed back its location, ori-entation and acceleration.
However the quick attenuationof GPS signal by roof, earth or water
makes exact stateinformation unavailable for indoor, underground or
un-derwater exploration tasks. These practical limitationsmake SLAM
as a simultaneous solution to both problemsan important area of
study.
This paper will not discuss topological mapping,SLAM in dynamic
environment, or issues specific to3D visual SLAM, but will review
the general algorithmsthat apply to all metric SLAM approaches.
2 Formulation
There are two versions of the SLAM problem. One isonline SLAM.
In a probabilistic framework, it can beformulated as finding the
most likely current pose st andmap m given the measurements z1:t
and controls u1:t.
arg maxst,m
P (st,m|z1:t,u1:t) (1)
The other version is full SLAM, which estimates theentire
trajectory s1:t = {s1, s2, , st},
arg maxs1:t,m
P (s1:t,m|z1:t,u1:t) (2)
Estimators for online SLAM are usually refer to asfilters, and
those for full SLAM are called smoothers.The subtle difference in
these two formulations actuallyleads to ramifications in the
families of algorithms toapply.
2.1 Variables
In these problems, st = [x, y, ] represents the pose (i.e.x,
y-location and orientation) of the robot at the current
1
-
timestep t. In 3D, the orientation could be parameterizedas
Euler angles or unit quarternions.
u1:t is the driving command issued by the robot con-troller or
the odometry readings obtained from on-boardsensors. Essentially,
it is an external signal that gives onea rough guess for the robots
motion.
z1:t is the history of measurements, from timestep 1 tothe
current timestep t. The form of measurements dependon the sensor
type and the application. For SLAM usinglaser rangefinders, raw
measurements are the distancesfrom the robot to the nearest
obstacle along each lasercast, i.e. zt = {zt,1, zt,2, , zt,n} where
zt,k is the per-ceived distance for laser cast k at time t. The
endpointsof all laser casts form a frame of point cloud.
Algorithmsthat work directly with these point clouds are called
scan-or view-based SLAM.
In many situations, one wants to extract landmarks(i.e. corners,
straight lines, ellipses, etc.) from theraw scan for conservation
of storage and easy represen-tation. In that case, parameters of
the extracted land-marks can be used as measurements. Algorithms
thatwork with landmarks are called landmark-based SLAM.zt =
{zt,ct,1 , zt,ct,2 , , zt,ct,n} is a list of landmarkparameters
based on the observation at timestep t andeach element is the
parameters of one observed land-mark. Here c represents data
association; for example,ct,3 = 2 means the 3rd landmark observed
from the scanat timestep t corresponds to map landmark no.2.
Dataassociations are usually not known, and have to be esti-mated
alongside other variables. This is a big topic inSLAM (see Section
7 for details). For ease of exposition,in the following
derivations, we assume the correspon-dence for all measurements are
available.
m represents the map, typically either a grid map ora landmark
map. In methods that directly integrate rawrange data, the
environment is divided into small squarescalled occuancy grids
(Fig. 1). Each grid is labelled witha probability of it being
occupied (i.e. untraversable).The map is a list that stores these
occupancy probabili-ties, m = [o1, o2, on]. The storage cost is
linear in thearea of the environment, and in the 2D case,
quadraticin the resolution of grids. A more popular alternativeis a
landmark map which stores the parameters of alllandmarks (Fig. 2).
Compared to the dense representa-tion, the sparsity of landmarks in
the environment greatlyreduces the storage cost. In the case of
point landmarks,m = [l1, l2, , ln] is a list of landmark
coordinates.
Figure 1: The grid-based Intel lab dataset, including 943
poses.
Figure 2: The landmark-based victoria park dataset, including
6969 poses(red) and 151 landmarks (yellow). Ellipses represent
estimate uncertainties.[Paz et al., 2008]
2.2 Motion model and measurement model
The motion model and the measurement model govern theSLAM
process, and completely specify the relationshipsbetween all
variables as shown in the graphical model(Fig. 3).
2
-
Figure 3: The graphical model of SLAM. Shaded nodes (controls u
and mea-surements z) are observed. White nodes (poses x and
landmarks l) are theSLAM state variables that are to be estimated.
The directed edges representconditional probabilities defined by
the motion model and the measurementmodel.
1The motion model,
st = f(st1, ut) +N (0,u)
describes the robots current pose st given its previouspose st1
and the driving signal (or odometry) ut1.This function is usually
nonlinear due to the effect oforientation. Uncertainty in motion is
introduced in theform of a Gaussian noise. The motion model has
theMarkov property in the sense that the current pose is
onlyaffected by the preceding pose, not any poses before it.
The measurement model is also a nonlinear function,
zt,i = h(st, lct,i) +N (0,z)
that describes the sensor measurement zt,i of a map land-mark
lct,i given the location of this landmark and therobots current
pose st. The measurement is also as-sumed to be corrupted by a
Gaussian noise.
These two models give rise to the conditonal proba-bilities that
define the edges in the Bayesian network forSLAM:
p(st|st1, ut) = N(f(st1, ut),u
)p(zt,i|st, lct,i) = N
(h(st, lct,i),z
)1for notational simplicity, we assume the models and noise
vari-
ances are time-invariant. We also assume the landmark-based
SLAM.
2.3 Online SLAM
Using Bayes rule and the Markov property of the mo-tion model,
the posterior for online SLAM (1) can becomputed recursively:
p(st,m|z1:t,u1:t) p(zt|st,m)p(st|st1,
ut)p(st1,m|z1:t1,u1:t1)dst1.
(3)
This update of the posterior can be understood as:first the
integral is a marginalization over the previouspose st1, then the
multiplication is conditioning on zt.All filtering approaches to
SLAM essentially performthese two steps. In Section 3, I will
review approachesbased on parametric filters including Extended
KalmanFilter(EKF) and its dual Extended Information Filter(EIF). In
Section 4, I will review algorithms based onRao-blackwellized
Particle Filters.
2.4 Full SLAM
The posterior for full SLAM (2) can be decomposed intoa product
of individual conditional probabilities:
p(s1:t,m|z1:t,u1:t)
=t
=1
p(s |s1, u )p(z |s ,m)
=
t=1
p(s |s1, u )Ni=1
p(z,i|s , lc,i)
exp{
t=1
(f(s1, u ) s2u
+
Ni=1
h(s , lc,i) z,i2z)}
Here x y2 , (x y)T(x y) is the squaredMahalanobis distance
between two vectors with respectto covariance matrix .
Maximizing this posterior is equivalent to minimizingthe
quadratic function:
t=1
(f(s1, u )s2u+
Ni=1
h(s , lc,i)z,i2z)
(4)
3
-
This is also known as nonlinear least squares optimiza-tion, and
is the central problem that most full SLAMalgorithms address. In
Section 5 I will review approachesthat exploit the special struture
of SLAM and use sparselinear algebra and graph-theoretic techniques
to performthe optimization.
3 Parametric Filters
Filters maintain a posterior over map and the current pose.The
posterior is usually represented as a multivariateGaussian.
3.1 Extended Kalman Filter
Kalman Filter(KF) is an standard estimator for dynamiclinear
systems with Gaussian noise. It maintains a statevector t = [st,
l1, l2, , lN ] that concatenates the cur-rent robot pose and the
landmark estimates, where N isthe number of map landmarks; as well
as a covariancematrix, t which is a measure of confidence.
Extended Kalman Filter (EKF) is an extension ofKalman filters
that works with nonlinear models. Ateach timestep, it linearizes
the motion model and mea-surement model around the current state
estimate, andthen update the linearized system with the same rules
ofregular Kalman filters. The filter works in two steps:
Time Update: compute the predicted state t andcovariance matrix
t.
t = f(t1, ut)
t = Att1ATt +GuGT
where At is the Jacobian of the motion model fwith respect to
the pose st evaluated at t. G issimply a projection matrix.
Measurement Update: Solve data association c(assume known for
simplicity), correct the estimatet and t with new measurement. This
requirescomputing the Kalman Gain Kt.
Kt = tCTt (CttC
Tt + z)
1
t = t +Kt(zt h(t, c))t = (I KtCt)t
where Ct is the Jacobian of the measurement modelh with respect
to the pose and all observed land-marks, evaluated at .
These two steps of EKF is an instantiation of the up-date
formula of (3) in the case of Gaussian models. EKFproduces an exact
solution of the posterior, and is easyto implement.
However the computational cost of EKF restricts itsuse on large
scale mapping. When marginalizing outprevious robot poses, the
uncertainty in the pose cor-relates the all landmarks being
observed. As a result,the covariance matrix quickly becomes dense,
requiringstorage that is quadratic in the number of landmarks
(orequivalently in the area of the map). The multiplicationsof the
covariance matrix with sparse Jacobians imposequadratic time
complexity (see [Paz et al., 2008] for adetailed analysis). These
make the scaling of EKF almostimpossible. In practice, EKF can only
handle maps thatcontain a few hundred landmarks.[Thrun, 2004]
Also, the assumption that the posterior is a uni-modelGaussian
distribution is often not valid in the real worldwhere ambiguous
scenes are common.
Most importantly, EKFs are known to give over-confident
covariance estimate, an effect known as in-consistency [Huang and
Dissanayake, 2007; Julier andUhlmann, 2001]. Inconsistency is
caused by evaluatingthe Jacobians at estimated states, rather than
the truestates. In fact, all filtering approaches applied to
non-linear problems suffer from inconsistency, because thewrong
linearization of past poses can not be revoked.This is different
from the smoothing approaches (seeSection 5).
An variant of EKF called Unscented Kalman Filter(UKF) improves
consistency by avoiding linearizing thenonlinear models. It uses
the unscented transform todeterministically sample a set of
representative points(aka. sigma points) from the distribution of
current esti-mate. These points are propagated through the
nonlinearmodels, from which the new mean and covariance
arerecovered. UKF also alleviates part of the computationalburden
as no Jacobians need to be evaluated.
3.2 Sparse Extended Information Filter
Concerned with the computational cost of EKF, re-searchers
turned to the dual of EKF, known as the ex-tended information
filter (EIF). Instead of representingthe Gaussian posterior in
terms of the mean and thecovariance matrix , EIF maintains the
information formof the posterior which uses the information vector
b andthe information matrix H . The two parameterizations
4
-
are equivalent and can be related by H = 1 andb = TH , but their
sparsity pattern is drastically dif-ferent, as is apparent from
Fig. 4. Elements in the in-formation matrix can be thought of as
constraints thateach carries the relation of a certain pair of
landmarks.Note that the small number of strong links exists
onlybetween metrically nearby landmarks, and by removingthe large
number of weak links whose values are nearzero the information
matrix can be made truly sparse.This allows the Sparse EIF
(SEIF)[Thrun, 2004] to usestorage that is linear in the number of
landmarks andperform all updates in constant time.
Figure 4: Typical snapshots of EKFs applied to the SLAM problem.
Shownhere is a map (left panel), a correlation (center panel), and
a normalized infor-mation matrix (right panel). Notice that the
normalized information matrix isnaturally almost sparse, motivating
the approach of using sparse informationmatrices in SLAM. [Thrun,
2004]
The two update steps of EIF is largely parallel to thatof
EKF:
time update
Ht =[(I +At)H
1t1(I +At)
T + SuST]1
bt = (bt1H1t1 + f(t1, ut)T )Ht
where At is the Jacobian of the motion model evalu-ated at t and
S is a projection matrix. Time updatefor EIF is expensive because
of the inversion of adense Ht and the recovery of the mean t.
Constanttime update can be achieved if Ht is sparse and tis
available for the pose and all active landmarks.
measurement update
Ht = Ht + Ct1z C
Tt
bt = bt + (zt zt + CTt t)T1z CTtwhere Ct is the Jacobian of the
measurement modelevaluated at t. Measurement update is essentiallya
conditioning of the posterior, which is complex forEKF, but
requires only constant-time summations
for EIF. It is important to note that C is sparse, withonly
non-zero entries at the blocks correspondingto the pose and the
observed landmarks in this mea-surement. This means at each step a
matrix withonly a constant number of non-zero entries will beadded
to Ht. This property has the intuitive inter-pretation that
information adds up, and the mainbenefit of working with the
information form.
SEIF performs sparsification whenever a measurementupdate or
motion update introduces a new link that wouldviolate the sparsity
bound, hence keeping H sparse. Thesparsification step essentially
imposes conditional inde-pendence between the pose and the active
landmarks. Aresult of this approximation is inconsistency.
The mean is incrementally recovered by formulatingstate recovery
as an optimization problem, and do co-ordinate descent for a
constant number of variables ateach timestep. As the optimization
takes place over mul-tiple timesteps, the recovery is an
approximate, but theamortized constant time map recovery is an
advantage.
Despite being an approximate approach, the result ofSEIF is
comparable to that of EKF, yet at a much im-proved speed. Also
since the information matrix can beformed by adding up terms
corresponding to individualobservations, they naturally lends to
multi-robot SLAM,in which each robot collects measurements about a
sub-region, and periodically merges its partial informationmatrix
with the others [Thrun and Liu, 2005].
3.3 Related Algorithms
The Thin Junction Tree Filter [Paskin, 2002] andTreeMap [Frese
and on; Frese et al., 2006; Frese, 2005]approximate the graphical
model with a sparse tree struc-ture, by essentially breaking weak
links. They are eachcapable of efficient inference in O(n) and
O(log(n))time.
D-SLAM [Wang et al., 2007b] and the Exactly SparseExtended
Information Filter [Walter et al., 2007] focuson the fact that time
update causes the fill-ins in the in-formation matrix. To maintain
sparsity, they marginalizeout the robot pose from the state when
performing timeupdate, and subsequently relocate the robot within
themap. By doing so, their algorithms achieve the speed ofEIF, yet
provide conservative estimates.
5
-
4 Particle Filters
Particle filters, also known as sequential Monte Carlomethods,
represent the posterior by a set of samples (aka.particles). The
samples are updated in iterations to trackthe evolution of the
belief distribution.
Compared with EKFs, particle filters can representmulti-modal
posterior and models with non-Gaussiannoise. As the particles are
propagated and resampled,they automatically concentrate on the more
probableregions of the distribution. The complexity and
effec-tiveness of a particle filter is largely determined by
thenumber of particles, which has to be carefully tunedfor most
applications to avoid both high computationalcost and the depletion
of particles. However an optimalparticle size is often difficult to
prove.
When appied to SLAM, a particle filter maintains par-ticles that
each represents a version of the belief for therobot pose (or
trajectory) and the map. As multiple ver-sions of the map is
stored, mapping is usually more robustagainst erroneous data
associations and does not requireexplicit loop closing
heuristics.
In general, the full SLAM posterior does not have aclosed form
that one can directly sample from. Instead,sampling importance
resampling (SIR) filter allows oneto sample from a proposal
distribution that is tractable,and then weight the samples based on
its likelihood underthe target distribution. The closer the
proposal distribu-tion is to the target distribution, the smaller
the varianceof the filter estimate.
The structure of SLAM makes a technique
calledRao-blackwellization particularly useful. The
Rao-blackwellized particle filter [Doucet et al., 2000;
Murphy,1999] makes use of the following factorization of the
pos-terior:
p(s1:t,m|z1:t,u1:t1)= p(s1:t|z1:t,u1:t1)p(m|s1:t, z1:t)
This factorization allows us to first estimate only
thetrajectory p(s1:t|z1:t,u1:t1), usually using the SIR fil-ter,
and then compute the map p(m|s1:t, z1:t) given thesampled
trajectory.
The Rao-blackwellized SIR filter updates the particlesin the
following steps:
1. Sampling A new generaton of particles {x(i)t } isobtained
from the previous generation {x(i)t1} that
follows the proposal distribution pi:
x(i)1:t pi(|z1:t, u1:t1)
2. Importance weighting An importance weight w(i)tis assigned to
each particle:
w(i)t =
p(x(i)1:t|z1:t, u1:t1)
pi(x(i)1:t|z1:t, u1:t1)
In most SLAM algorithms, the proposal pi is re-stricted to have
the recursive form:
pi(x1:t|z1:t, u1:t1) = pi(xt|x1:t1, z1:t, u1:t1) pi(x1:t1|z1:t1,
u1:t2)
so that the importance weights can be computedrecursively,
w(i)t
p(zt|m(i)t1, x(i)t )p(x(i)t |x(i)t1, ut1)pi(xt|x(i)1:t1, z1:t,
u1:t1)
w(i)t1
3. Resampling Particles are resampled with replace-ment
according to their importance weight. Afterresampling, all
particles have the same weight.
4. Map Estimation For each particle, the correspond-ing map
estimate p(m(i)|x(i)1:t, z1:t) is computed.
Most particle filter SLAM algorithms are based on thisframework.
The major challenge is to reduce the numberof particles while
avoiding particle depletion and main-taining (as resampling can
potentially eliminate correctparticles). This requires one to
choose a proposal dis-tribution that is both accurate and easy to
sample from;also to prevent unnecessary resamplings. Other
varia-tions include the map representation (i.e. landmarks
oroccupancy grids).
4.1 FastSLAM
In FastSLAM[Montemerlo et al., 2002], the map is repre-sented
with landmarks. Using the fact that all landmarksare independent
given the trajectory, the posterior factor-ization is:
p(s1:t,m|z1:t,u1:t1)
= p(s1:t|z1:t,u1:t1)Kk=1
p(k,k|s1:t, z1:t,u1:t1)
6
-
The algorithm uses a particle filter to estimatethe trajectory
p(s1:t|z1:t,u1:t1), and each trajectoryparticle has an associated
set of independent EKFsthat each tracks the parameters of one
landmark,p(k,k|s1:t, z1:t,u1:t1). Because the EKFs are
in-dependent, each update takes constant time, and the costof
updating EKFs associated with all trajetory particlesscales
linearly with the number of landmarks, as well asthe number of
trajetory particles.
The proposal distribution used by FastSLAM is thebelief state
before integrating the current measurement,
pi = p(x1:t|z1:t1, u1:t)
Particles {x(i)1:t1} are updated purely based on the mo-tion
model p(xt|xt1, ut1) to obtain {x(i)1:t}. In this case,the weight
for particle x(i)1:t is:
w(i)t Em
[p(zt|x(i)1:t)
]=
p(zt|x(i)1:t,m)p(m)dm
Here p(zt|x(i)1:t,m) is the measurement model, p(m) isa Gaussian
defined by the estimates {k} and {k} as-sociated with this
particle. If the measurement model islinearized, this integral
results in a Gaussian and hencehas a closed form. Note that the
cost of weights com-putation scales linearly with the number of
samplingiterations, or equivalently, the trajectory length.
The original FastSLAM has poor performance whenthe motion noise
is large (i.e. proposal distribution de-viates markedly from target
distribution), because mostupdated particles are terminated in the
resampling stepdue to low likelihood given the new measurement.
Thiscauses most of the later particles to share a common an-cestor,
therefore losing the power to represent multipletrajectories.
FastSLAM 2.0[Montemerlo et al., 2003]improves the proposal
distribution by taking into accountthe most recent measurements.
With the same number ofparticles, the more efficient use of the
particles results inmuch faster convergence and smaller error.
[Hahnel et al., 2003] extends FastSLAM to a
grid-maprepresentation based on raw laser range measurements.
4.2 DP-SLAM
DP-SLAM [Eliazar, 2003; Estrada et al., 2005; Eliazarand Parr,
2004] uses the same proposal distribution asFastSLAM, but
represents map by accumulating raw
laser range data on grids, resulting in dense grid mapsinstead
of sparse landmarks.
Such representation bypasses the data association prob-lem, but
imposes heavy memory/time cost at the resam-pling step, when the
entire map has to be copied overfrom an old particle to a new one.
This is not an issue forFastSLAM as landmark parameters have far
smaller sizecompared to a grid map.
DP-SLAM maintains a single map shared by all parti-cles. Each
grid of this map stores an observation log thatlists all particles
that have made observation on this gridand their observations. By
associating particles with amap, rather than vise versa, DP-SLAM
essentially giveseach particle its own logical map. When one needs
toknow a particles current estimate for a particular grid(when
computing weight for this particle), one can searchthis grids
observation log for entries made by this par-ticles most recent
ancestor. This requires maintaininga particle ancestry tree in
which all current particles areleaves. This tree is kept minimal by
pruning childlessnodes and collapsing branches with a single child,
sothat the cost of finding ancestors is bounded. Boundedancestor
search and bounded observation lookup resultin efficient update
that does not dependent on the numberof iterations, but only on the
number of particles.
4.3 Related Algorithms
[Grisetti et al., 2007] draws particles in a more effectiveway.
Highly accurate proposal allows them to utilize theeffective sample
size to indicate whether resampling isneeded. This reduces the risk
of particle depletion.
5 Graph/Optimization-based SLAM
Graph- or optimization-based SLAM algorithms exploitthe
graphical model of SLAM, and formulate state es-timation as
optimization problems. They then employtechniques at the
intersection of sparse linear algebra andgraph theory to solve the
optimization problem. Graphbased SLAM is usually related to the
full SLAM, inwhich the goal is to estimate the entire trajectory
history,in addition to the current pose. These estimators arecalled
smoothers, in contrast to filters.
As discussed previously, filtering inevitably is subjectto
inconsistency because linearization error due to wrongestimates of
past poses are perpetuated in the system. In
7
-
contrast, smoothers, which solve the full SLAM problem,can
revise estimates for the entire trajetory based on latestdata and
relinearize the nonlinear model using revisedestimates, therefore
produces more accurate and moreconsistent result.
Recall from section 3, the major computational hurdlefor both
EKFs and EIFs is a dense matrix (the covariancematrix for EKF and
the almost but still exactly sparseinformation matrix for EIF). The
lack of sparsity is in-herent in the filtering approach. For
smoother, however,the information matrix is exactly sparse. This
providesopportunities for using sparse optimization techniques
tofind the most likely trajectory, as well as the map.
Most optimization-based smoothing algorithms con-sists of two
components: a front-end that extracts con-straints between
variables from sensor data, and a back-end that optimizes the sum
of constraints. The front-endneeds to address data association and
bound the searchfor correspondence. The back-end is closely related
tobundle adjustment in the computer vision literature, andthe two
areas largely converge at visual SLAM. The com-mon problem they
face usually boils down to a nonlinearleast-squares optimization
problem.
5.1 Nonlinear least-squares formulation
Based on the nonlinear quadratic function (4) in Section2.4, we
give a more general formulation of the optimiza-tion problem of
full SLAM.
Suppose we have n variables (including pose variablesand
landmark variables) and m constraints (includingmeasurement
constraints that link a pose variable witha landmark variable, and
odometry constraints that linka pose with another pose). To
simplify notation, wedo not distinguish odometry errors and
measurementerrors, but use a generic error term eij(x) , e(xi, xj ,
zij)to represent the residual of the constraint connectingvariable
i and j. The potential of this constraint is definedby the
Mahalanobis distance eij(x)2ij with respect tothe error covariance
matrix ij . The objective functionwe need to minimize is the total
error potential, alsoknown as 2 error:
2 =(i,j)
eij(x)2ij
Nonlinear least squares problems like this are usuallysolved
using the Gauss-Newton method or the Levenberg-Marquardt method.
They form a linear system around the
current estimate, solve to get a new estimate and iterateuntil
convergence.
Given the current state estimate x0, each error term
islinearized at x0 to obtain eij(x) eij,0 + Jij, whereeij,0 ,
eij(x0), Jij is the Jocabian of eij(x) and =x x0. The linearized
potential function is,
2 =(i,j)
Jij + eij,02ij
=(i,j)
12ijJij +
12ijeij,022
=(i,j)
Aij + qij2
= A + q2
where Aij , 12ijJij , qij ,
12ijeij,0. A Rmn is the
Jacobian of constraints and is formed by vertically stack-ing
all Aij . Each row of A corresponds to a constraint,and its only
two non-zero entries are at columns corre-sponding to the two
variables involved in this constraint.Similarly, q Rm is formed by
vertically stacking allqij .
To find the minimizer we solve the normal equation
ATA = AT q (5)
Here ATA , H is the information matrix for fullSLAM. It is
block-wise sparse. The new estimate x isthen given by,
x = x0 + .
5.2 Sparsity of information matrix
It is important to note that the full SLAM informationmatrixH is
exactly block-wise sparse, as shown in Fig. 6.In fact, a constraint
on i and j modifies H only at fourblocks: two on diagonal at (i, i)
and (j, j), and twooff-diagonal at (i, j) and (j, i). In other
words, an off-diagonal block (i, j) in H is non-zero if and only if
thereis a link between variables i and j on the graph.
Thisstructure is exploited by a variety of sparse linear
solversthat work in batch mode or incrementally.
5.3 Direct solvers and variable ordering
The normal equation Eq. (5) can be solved in a varietyof ways.
Explicitly inverting H is not necessary, ratherwe use
factorization. The most common approach is to
8
-
Figure 5: The typical sparsity pattern of a pose-before-landmark
variableordering. Jacobian A (left), information matrix H (top) and
square-root ma-trix R (middle), square-root matrix with a better
varaible ordering and hencefewer fill-ins (bottom)[Dellaert,
2006].
apply the Cholesky decomposition ATA = RTR whereR Rnn is the
upper-triangular square root matrix ofH . To solve RTR = AT q,
first apply a forward sub-stitution on RT y = AT q to recover y,
then a backwardsubstitution on R = y yields .
An alternative is the QR decomposition. It does notrequire
squaring A, and hence also avoids squaring thecondition number of
A, since a large condition numbermeans slower convergence (for
iterative solvers) and nu-merically unstable solutions.
The QR decomposition of A yields:
A = Q
[R0
]
where R Rnn is the square root information matrixas above, and Q
Rmm is an orthogonal matrix. Withthis decomposition the normal
equation can be writtenas: [
R0
] = QT q ,
[d]
The problem is simplified to the full rank linear systemR = d,
which again is easy to solve by back substitu-tion.
In practice, the dense matrix Q is never explicitlyformed. R can
be found by applying Householder re-flection matrices or Givens
rotations to A. d is computedby appending q to A and undergo the
same transforms.
From a graphical model point of view, factorizationis equivalent
to variable elimination, in which nodes areeliminated one by one.
Eliminating one node introducesedges among all its neighbours.
These extra edges corre-spond to fill-ins in one row of the square
root matrix R.Too many fill-ins results in slow factorization, and
theorder in which variables are eliminated may drasticallyaffect
the number of fill-ins. Finding an optimal orderingis NP-complete
[Yannakakis, 1981]. The most widelyused appriximate algorithms
include the minimum de-gree family (e.g. COLAMD[Dellaert, 2006;
Kaess et al.,2008]) and the generalized nested dissection based
ongraph theory [Ni and Dellaert, 2010].
The structure of SLAM can be exploited to design effi-cient
variable ordering. To illustrate the effect of a goodvariable
ordering, consider one common heuristic that allpose variables are
placed before all landmark variables[Kummerle et al., 2011]. The
information matrix canbe partitioned based on this ordering, so
that the normalequation is written as,[
Hpp HplHlp Hll
] [pl
]=
[bpbl
]where p is the pose variables and l is the
landmarkvariables.
An equivalent reduced system on just p can beformed by taking
the Schur complement of H ,
(Hpp HplH1ll Hlp)p = bp HplH1ll bl
Note that computing H1ll is easy since Hll is blockdiagonal (no
constraints between landmarks). After pis obtained, we solve
Hlll = bl +Hlp
p
9
-
for the landmark estimates. Again the structure of Hllsimplifies
the computation. This, in terms of the graph,means to first
eliminate landmark nodes. This causesposes that observe the same
landmark to be clustered, butno extra links are introduced
involving other landmarks.Then we estimate the poses on the
remaining pose graph,and finally compute the landmark estimates
based onpose estimates.
The opposite ordering that all landmarks are beforeposes is not
as good, becauseHpp is block band-diagonalrather than
block-diagonal, hence harder to inverse. Onthe graph, the effect is
that the elimination of poses cre-ates many more extra edges
between landmarks and otherposes, compared to the pose graph in the
former case.
5.4 Graph-based SLAM
The seminal paper of [Lu and Milios, 1997] proposed thegraph
optimization paradigm. The specific problem theyhad is the
view-based SLAM, where measurements arelaser scans, instead of
landmarks. They build a networkof pose nodes called the pose graph.
The constraintsbetween pose nodes are defined by either odometry
ormatching of laser scans, in forms of quadratic functions.They
then optimize the total constraints in the pose graph.This
framework has been used in most graph-based algo-rithms; for
landmark-based SLAM, often the landmarknodes are also included in
the graph. [Gutmann andKonolige, 1999] provides an effective
approach to forconstructing the pose graph and for detecting loop
clo-sures while running an incremental estimation algorithm.
GraphSLAM[Thrun and Montemerlo, 2006] works onthe information
matrix, gradually reducing it by applyingexact transformations to
remove the landmark variables.The effect is to shift the
information between landmarksand poses to information between pairs
of poses. Thereduced optimization problem is then solved using
conju-gate gradient to obtain the trajectory. Finally landmarksare
recovered by solving decoupled small optimizationproblems one for
each landmark, holding the optimizedtrajectory fixed.
SAM [Dellaert, 2006] uses the colamd heuristicto re-order the
varaibles. The incremental versioniSAM[Kaess et al., 2008] performs
fast incrementalupdates to the square root information matrix
usingGivens rotations. To remain efficient and consistent,iSAM
requires periodic batch steps for variable reorder-ing and
relinearization. Similarly, Sparse Pose Adjust-
ment[Konolige et al., 2010] uses Cholesky decompo-sition with
amd ordering under the LM algorithm. Ituses memory-efficient
storage to speed the setup of theinformation matrix, which is the
most costly part inbatch factorization. The most recent iSAM2[Kaess
et al.,2012] uses a novel data structure called Bayes Tree,
thatachieves incremental variable ordering and fluid
relin-earization.
5.5 Iterative Solvers
Iterative solvers gradually move the state to reduce theresidual
A + q until it is sufficiently close to zero.A large family of
algorithms estimates the curvature atthe current state. For large
systems, such methods oftenstuck in local minima and flat regions,
therefore heavilyrely on good initial estimate. The most popular
method ofthis kind is Conjugate Gradient[Thrun and
Montemerlo,2006]. Preconditioning typically leads to markedly
fasterconvergence, and is used by making large maps with 60Kposes
[Konolige, 2004] and the state-of-the-art sparsegraph optimization
library g2o[Kummerle et al., 2011].
Another family of iterative algorithms considers onlya subset of
nodes or edges on the graph. Relaxationmethods iteratively pick a
node and move it to whereits neighbours think it should be.
[Duckett and Mars-land, 2002] uses the Gauss-Seidel relaxation but
suffersfrom slow convergence. [Frese et al., 2005]
significantlyimproves the convergence speed with multigrid
methods.Stochastic Gradient Descent (SGD) randomly selects asingle
edge and follows the gradient with respect to justthat edge. It
avoids local minima, and is robust to poorinitial estimate. [Olson
et al., 2006] applies precondi-tioned SGD to the pose graph that
uses an incrementalpose state space. A hierarchical extension to
this methodis TORO [Grisetti et al., 2009; Grisetti and
Stachniss,2007], which uses a tree-based parameterization. It
re-duces the maximum path length between arbitrary nodes,and hence
provides faster convergence.
6 Submapping
Submapping connects small local maps together to forma global
map. Poses and landmarks are often parameter-ized relative to its
own local map, so that the estimatesare independent across submaps.
Also because a submaphas bounded size, the orientation uncertainty,
which is
10
-
the main causes of inconsistency, can be limited. At theglobal
level, the constraints within each local map aremerged to form a
single synthetic constraint that linksthis local map with the
others. Submapping is oftenused with graph-based approaches, as
submaps can beintuitively interpreted as graph cuts. However,
manyalgorithms that represent posterior in a tree structure,like
the thin-junction tree algorithm[Paskin, 2002], canalso be
interpreted as an implicit submapping methodsince each cluster of
the junction tree can be viewed as asubmap.
Figure 6: Two-level hierarchical SLAM [Estrada et al.,
2005].
The Atlas framework [Bosse et al., 2003; Bosse, 2004]constructs
a two-level hierarchy. The lower level is es-timated with a Kalman
filter, and a global optimizationthen aligns the local maps at the
top level. The Hierar-chical SLAM of [Estrada et al., 2005] also
imposes loopconsistency in the optimization.
For two-level approaches the map joining in the globallevel
tends to become expensive with large maps. [Pazet al., 2008]
improved it by fusing the local maps inan even higher hierarchy.
Optimization then proceedsin a divide-and-conquer manner. Similarly
[Konolige,2004] forms a binary hierarchy over keynodes on the
posegraph, and estimates reduced graphs at different
levels.Synthetic constraints between two nodes are computed by
chaining covariances of lower level nodes. Higher levelnodes are
optimized and fixed before optimizing lowerlevels.
FrameSLAM[Konolige and Agrawal, 2008] ex-tends this idea to 3D
visual mapping, and the constraintsuse relative pose in order to
minimize approximationerrors of synthetic constraints. Tectonic-SAM
[Ni andDellaert, 2010; Ni et al., 2007] smoothes nodes withineach
submap before optimizing boundary nodes. It alsouses relative
parameterization so that the linearizationpoints of submap nodes
can be reused .
Submapping can naturally produce hybrid maps thatare locally
metric but globally topological. Hybrid mapsare effective in many
exploration applications where spa-tial connectivity is critical,
but not the accurate distanceinformation.
7 Data Association and Loop Closing
An observation has to be placed in relationship to theexisting
map, in order to transfer its information intothe map and hence
reduce uncertainty. In the case oflandmark-based maps, whenever a
new measurement isreceived on a landmark, one needs to determine
eitherthis observation corresponds to an existing landmark, orit is
a new landmark that has not been seen before.
The simple nearest neighbour approach, matches anobservation to
the landmark that has the shortest Maha-lanobis distance. KD-tree
is often used to speed up thesearch.
Finding matched landmarks based on Mahalanobis dis-tance
requires the uncertainty of landmarks to be avail-able. For
approaches that explicitly maintain the meanand covariance matrix
for landmarks, like EKFs, this isnot a problem. For EIFs or graph
SLAM, which workswith the information form, recovering the
covariance(and the mean for EIF) involves the expensive inversionof
the information matrix. Various partial state recoverytechniques
have been developed to efficiently computethe marginal covariance
for only a subset of landmarksthat are of interest [Frese et al.,
2006; Konolige andAgrawal, 2008; Kaess et al., 2008].
If the uncertainty of a newly-observed landmark istoo large (due
to accummulated noise) and no decisionscan be made with sufficient
confidence, this observationcan temporarily be placed in a
wait-list and tracked sep-arately. This prevents potentially
incorrect associationsfrom jeopardizing the estimator. A weight is
assigned to
11
-
each wait-listed observation, and is adjusted in each
stepaccording to its likelihood computed from the latest
mea-surements. The weight gets increased if this
wait-listedlandmark is observed consistently, or a loop closure
sig-nificantly decreases its uncertainty. The weight decays ifno
further measurements support the decision. As moremeasurements are
made, the observation either maturesinto the map or is discarded.
This strategy is also usefulwhen there are dynamic objects in the
environment.
Matching observations to their nearest neighboursgreedily often
leads to spurious joint pairings, becauselater steps can not revise
established pairings. It is morerobust to consider associations for
multiple landmarks asa whole. Such batch methods require
maintaining multi-ple hypothesis. Note that multiple hypotheses is
inherentin particle filters, so they do not need do this
explicitly.
A popular batch association algorithm is Random Sam-ple
Consensus ( RANSAC) [Fischler and Bolles, 1981].The idea is to
generate many (possibly inconsistent) can-didate associations,
select a random subset of candidatesto obtain a model, and then
determines the support forthis model from the remaining candidates.
The asso-ciations for the best model with sufficient support
isaccepted, while those with little consensus are
deemedinconsistent and therefore rejected.
Joint compatibility branch and bound algorithm [Neiraand Tardos,
2001; Tardos et al., 2002] computes a jointcompatibility score for
each hypothesis, and uses thisscore to bound the search over a tree
of hypotheses for theone that includes the largest number of
jointly compatiblepairings.
For optimization-based approaches, lazy data associ-ation
incoorperates the association as variables in opti-mization, and
the quality of association is given by the2 error.[Hahnel et al.,
2005]
7.1 Loop Closing
The issue of loop closing is more subtle than just
dataassociation. It refers to the situation when the robotreturns
to a place that it has visited a long time ago,usually after a
lengthy loop. Data association techniquesare required to recognize
such situation, but the efficientpropagation of this information to
the entire map, and inthe case of full SLAM, back down the entire
trajetory,is the real challenge. Loop closing usually involves
themost expensive computations in SLAM, but it is essentialto
correct gradual trajectory warping due to errors in
orientation estimate. (Fig. 7)
Figure 7: The effect of loop closing.
For graph-based approaches, closing a loop meansadding a
constrain between the current pose and an ear-lier pose. This
complicates the connectivity of the posechain, breaking the block
tridiagonal structure of theinformation matrix, and thus makes
subsequent optimiza-tion more difficult.
Similar effects happen to filters. In information filters,the
link between the pose and an early landmark decaysas the robot
moves onwards. Loop closing re-activessuch weak links.
8 Conclusions
This paper reviews common approaches for SLAM inthe classical
formulation. Extended Kalman Filter pro-vides the earliest solution
but is computationally costly.Particle filters are flexible, but
often suffer from particledepletion. The sparsity associated with
the informationform of SLAM posterior is exploited to design both
ap-prximate and exact information filters and smoothers.Insights
are drawn from the intersection of sparse linearalgebra and graph
theory to solve the optimization prob-lem underlying SLAM,
producing the state-of-the-artalgorithms.
Based on these techniques, current SLAM researchconsists of a
wide range of directions, each focusing onspecific goals that make
SLAM more useful in real life.Some exciting areas include:
3D Visual SLAM: using cameras or D-RGB sensorsto estimate states
in 3D. This is closely related toStructure from Motion, but the
span of trajetory isoften much larger. Visual SLAM can work
withpoint clouds or landmarks of 3D shapes, and theoutput can be
photorealistic maps. The appearanceinformation of the environment
gives rise to many
12
-
possibilities for robust data association and loopclosing
[Newman and Ho, 2005; Newman et al.,2006; Eade, 2008; Cummins and
Newman, 2007].
life-long SLAM: requires constant time update andefficient
representations of the map.
active SLAM: automatically choose the best paththat obtains a
map with given accuracy in minimumtime, or maximize the coverage
with a fixed timehorizon and a required map quality.
Reinforcementlearning literature provides insights into the
desiredbalance of exploration and exploitation [Makarenkoet al.,
2002; Stachniss et al., 2004; Sim and Roy,2005]
SLAM in dynamic environment: dynamic path plan-ning, with
explicit modelling of dynamic and staticobjects [Lin and Wang,
2010; Wang et al., 2007a].
References
M Bosse. Simultaneous Localization and Map Buildingin
Large-Scale Cyclic Environments Using the AtlasFramework. The
International Journal of RoboticsResearch, 23(12):11131139,
December 2004. 6
M Bosse, P Newman, J Leonard, M Soika, W Feiten, andS Teller. An
Atlas framework for scalable mapping. InRobotics and Automation,
2003. Proceedings. ICRA03. IEEE International Conference on, pages
18991906, 2003. 6
M Cummins and P Newman. Probabilistic AppearanceBased Navigation
and Loop Closing. In Robotics andAutomation, 2007 IEEE
International Conference on,pages 20422048, 2007. 8
F Dellaert. Square Root SAM: Simultaneous localizationand
mapping via square root information smoothing.The international
Journal of robotics . . . , 2006. 5, 5.3,5.4
A. Doucet, N. De Freitas, K Murphy, and S Russell.
Rao-Blackwellised particle filtering for dynamic Bayesiannetworks.
pages 176183, 2000. 4
T Duckett and S Marsland. Fast, on-line learning ofglobally
consistent maps. Autonomous Robots, 2002.5.5
E Eade. Unified loop closing and recovery for real timemonocular
SLAM. British machine vision conference,pages 110, July 2008. 8
A Eliazar. DP-SLAM: Fast, robust simultaneous localiza-tion and
mapping without predetermined landmarks.International Joint
Conference on Artificial Intelli-gence, 2003. 4.2
A.I Eliazar and R Parr. DP-SLAM 2.0. In Robotics andAutomation,
2004. Proceedings. ICRA 04. 2004 IEEEInternational Conference on,
pages 13141320, 2004.4.2
C Estrada, J Neira, and J D Tardos. Hierarchical SLAM:Real-Time
Accurate Mapping of Large Environments.Robotics, IEEE Transactions
on, 21(4):588596, 2005.4.2, 6
Martin A Fischler and Robert C Bolles. Random sampleconsensus: a
paradigm for model fitting with appli-cations to image analysis and
automated cartography.Communications of the ACM, 24(6):381395,
June1981. 7
U Frese. Treemap: An O (log n) algorithm for simulta-neous
localization and mapping. Spatial Cognition IVReasoning, 2005.
3.3
U Frese, P Larsson, and T Duckett. A multilevel re-laxation
algorithm for simultaneous localization andmapping. Robotics, IEEE
Transactions on, 21(2):196207, 2005. 5.5
U Frese, L Intelligent Robots Schroder, and Systems2006 IEEE RSJ
International Conference on. Closinga Million-Landmarks Loop. In
Intelligent Robots andSystems, 2006 IEEE/RSJ International
Conference on,2006. 3.3, 7
U Robotics Frese and Automation 2007 IEEE Interna-tional
Conference on. Efficient 6-DOF SLAM withTreemap as a Generic
Backend. In Robotics and Au-tomation, 2007 IEEE International
Conference on. 3.3
G Grisetti and C Stachniss. A tree parameterization
forefficiently computing maximum likelihood maps usinggradient
descent. Proc of robotics: . . . , 2007. 5.5
G Grisetti, C Stachniss, and W Burgard. Improved Tech-niques for
Grid Mapping With Rao-Blackwellized Par-
13
-
ticle Filters. Robotics, IEEE Transactions on, 23(1):3446, 2007.
4.3
G Grisetti, C Stachniss, and W Burgard. Nonlinear con-straint
network optimization for efficient map learning.Intelligent
Transportation Systems, IEEE Transactionson, 10(3):428439, 2009.
5.5
J.-S Gutmann and K Konolige. Incremental mapping oflarge cyclic
environments. In Computational Intelli-gence in Robotics and
Automation, 1999. CIRA 99.Proceedings. 1999 IEEE International
Symposium on,pages 318325, 1999. 5.4
D Hahnel, W Burgard, D Fox, and S Thrun. An efficientfastSLAM
algorithm for generating maps of large-scale cyclic environments
from raw laser range mea-surements. In Intelligent Robots and
Systems, 2003.(IROS 2003). Proceedings. 2003 IEEE/RSJ
Interna-tional Conference on, pages 206211, 2003. 4.1
D Hahnel, S Thrun, B Wegbreit, and W Burgard. Towardslazy data
association in SLAM. Robotics Research,pages 421431, 2005. 7
Shoudong Huang and Gamini Dissanayake. Convergenceand
Consistency Analysis for Extended Kalman FilterBased SLAM.
Robotics, IEEE Transactions on, 23(5):10361049, 2007. 3.1
S.J Julier and J.K Uhlmann. A counter example to thetheory of
simultaneous localization and map building.In Robotics and
Automation, 2001. Proceedings 2001ICRA. IEEE International
Conference on, pages 42384243, 2001. 3.1
M Kaess, A Ranganathan, and F Dellaert. iSAM: In-cremental
Smoothing and Mapping. Robotics, IEEETransactions on,
24(6):13651378, 2008. 5.3, 5.4, 7
M Kaess, H Johannsson, and R Roberts. iSAM2: Incre-mental
Smoothing and Mapping Using the Bayes Tree.. . . Journal of
Robotics . . . , 2012. 5.4
K Konolige. Large-scale map-making. pages 457463,2004. 5.5,
6
K Konolige and M Agrawal. FrameSLAM: From bun-dle adjustment to
real-time visual mapping. IEEETransactions on Robotics,
24(5):10661077, 2008. 6,7
K Konolige, G Grisetti, R Kummerle, W Burgard,B Limketkai, and R
Vincent. Efficient Sparse PoseAdjustment for 2D mapping. In
Intelligent Robots andSystems (IROS), 2010 IEEE/RSJ International
Confer-ence on, pages 2229, 2010. 5.4
R Kummerle, G Grisetti, H. Strasdat, K Konolige,W Robotics
Burgard, and Automation ICRA 2011IEEE International Conference on.
G2o: A generalframework for graph optimization. In Robotics and
Au-tomation (ICRA), 2011 IEEE International Conferenceon, 2011.
5.3, 5.5
J.J Leonard and H.F Durrant-Whyte. Simultaneous mapbuilding and
localization for an autonomous mobilerobot. pages 14421447, 1991.
1
Kuen-Han Lin and Chieh-Chih Wang. Stereo-based si-multaneous
localization, mapping and moving objecttracking. In Intelligent
Robots and Systems (IROS),2010 IEEE/RSJ International Conference
on, pages39753980, 2010. 8
F Lu and E Milios. Globally consistent range scan align-ment for
environment mapping. Autonomous Robots,4(4):333349, 1997. 5.4
A.A Makarenko, S.B Williams, F Bourgault, and H.FDurrant-Whyte.
An experiment in integrated ex-ploration. In Intelligent Robots and
Systems, 2002.IEEE/RSJ International Conference on, pages 534539,
2002. 8
M Montemerlo, S Thrun, D Koller, and B Wegbreit. Fast-SLAM 2.0:
An improved particle filtering algorithmfor simultaneous
localization and mapping that prov-ably converges. 18:11511156,
2003. 4.1
Michael Montemerlo, Sebastian Thrun, Daphne Koller,and Ben
Wegbreit. FastSLAM: a factored solutionto the simultaneous
localization and mapping prob-lem. In Eighteenth national
conference on Artificialintelligence. American Association for
Artificial Intel-ligence, July 2002. 4.1
K Murphy. Bayesian map learning in dynamic environ-ments.
Advances in Neural Information ProcessingSystems (NIPS),
12:10151021, 1999. 4
J Neira and J D Tardos. Data association in stochasticmapping
using the joint compatibility test. Robotics
14
-
and Automation, IEEE Transactions on, 17(6):890897, 2001. 7
P Newman and Kin Ho. SLAM-Loop Closing with Vi-sually Salient
Features. In Robotics and Automation,2005. ICRA 2005. Proceedings
of the 2005 IEEE In-ternational Conference on, pages 635642, 2005.
8
P Newman, D Cole, and K Ho. Outdoor SLAM us-ing visual
appearance and laser ranging. In Roboticsand Automation, 2006. ICRA
2006. Proceedings 2006IEEE International Conference on, pages
11801187,2006. 8
Kai Ni and F Dellaert. Multi-level submap based SLAMusing nested
dissection. In Intelligent Robots and Sys-tems (IROS), 2010
IEEE/RSJ International Conferenceon, pages 25582565, 2010. 5.3,
6
Kai Ni, D Steedly, and F Dellaert. Tectonic SAM:
Exact,Out-of-Core, Submap-Based SLAM. In Robotics andAutomation,
2007 IEEE International Conference on,pages 16781685, 2007. 6
E Olson, J Leonard, and S Teller. Fast iterative alignmentof
pose graphs with poor initial estimates. In Roboticsand Automation,
2006. ICRA 2006. Proceedings 2006IEEE International Conference on,
pages 22622269,2006. 5.5
MA Paskin. Thin Junction Tree Filtering for Simultane-ous
Localization and Mapping. 2002. 3.3, 6
L M Paz, J D Tardos, and J Neira. Divide and Conquer:EKF SLAM in
O(n). Robotics, IEEE Transactions on,24(5):11071120, 2008. 2, 3.1,
6
R Sim and N Roy. Global A-Optimal Robot Explorationin SLAM. In
Robotics and Automation, 2005. ICRA2005. Proceedings of the 2005
IEEE InternationalConference on, pages 661666, 2005. 8
R Smith, M Self, P Robotics Cheeseman, and Automa-tion
Proceedings 1987 IEEE International Confer-ence on. Estimating
uncertain spatial relationshipsin robotics. In Robotics and
Automation. Proceedings.1987 IEEE International Conference on,
1987. 1
R C Smith and P Cheeseman. On the representation andestimation
of spatial uncertainty. The InternationalJournal of Robotics
Research, 5(4):5668, 1986. 1
C Stachniss, D Hahnel, and W Burgard. Explorationwith active
loop-closing for FastSLAM. In IntelligentRobots and Systems, 2004.
(IROS 2004). Proceedings.2004 IEEE/RSJ International Conference on,
pages15051510, 2004. 8
JD Tardos, J Neira, and PM Newman. Robust mappingand
localization in indoor environments using sonardata. . . .
International Journal of . . . , 2002. 7
S Thrun. Simultaneous Localization and Mapping withSparse
Extended Information Filters. The Interna-tional Journal of
Robotics Research, 23(7-8):693716,August 2004. 3.1, 3.2, 4
S Thrun and Y Liu. Multi-robot SLAM with sparseextended
information filers. Robotics Research, pages254266, 2005. 3.2
S Thrun and M Montemerlo. The GraphSLAM Algo-rithm with
Applications toLarge-Scale Mapping of Ur-ban Structures. The
International Journal of RoboticsResearch, 25(5-6):403429, 2006.
5.4, 5.5
S Thrun, W Burgard, and D Fox. A probabilistic ap-proach to
concurrent mapping and localization for mo-bile robots. Autonomous
Robots, 5(3):253271, 1998.1
M R Walter, R.M Eustice, and J.J Leonard. Exactlysparse extended
information filters for feature-basedSLAM. The International
Journal of Robotics Re-search, 26(4):335359, 2007. 3.3
CC Wang, C Thorpe, and S Thrun. Simultaneous lo-calization,
mapping and moving object tracking. . . .Journal of Robotics . . .
, pages 39753980, 2007a. 8
Z Wang, S Huang, and G Dissanayake. D-SLAM: Decou-pled
localization and mapping for autonomous robots.Robotics Research,
pages 203213, 2007b. 3.3
M Yannakakis. Computing the minimum fill-in is NP-complete. SIAM
Journal on Algebraic Discrete Meth-ods, 2(1):7779, 1981. 5.3
15
IntroductionFormulationVariablesMotion model and measurement
modelOnline SLAMFull SLAM
Parametric FiltersExtended Kalman FilterSparse Extended
Information FilterRelated Algorithms
Particle FiltersFastSLAMDP-SLAMRelated Algorithms
Graph/Optimization-based SLAMNonlinear least-squares
formulationSparsity of information matrixDirect solvers and
variable orderingGraph-based SLAMIterative Solvers
SubmappingData Association and Loop ClosingLoop Closing
Conclusions