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 one’s location based on rela-tive measurements of the map) and mapping (i.e. givenone’s 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 robot’s 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 robot’s 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(st−1, ut) +N (0,Λu)

describes the robot’s current pose st given its previouspose st−1 and the driving signal (or odometry) ut−1.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 therobot’s 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|st−1, ut) = N(f(st−1, 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|st−1, ut)p(st−1,m|z1:t−1,u1:t−1)dst−1.

(3)

This update of the posterior can be understood as:first the integral is a marginalization over the previouspose st−1, 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τ |sτ−1, uτ )p(zτ |sτ ,m)

=

t∏τ=1

p(sτ |sτ−1, uτ )

Nτ∏i=1

p(zτ,i|sτ , lcτ,i)

∝ exp

−

t∑τ=1

(‖f(sτ−1, uτ )− sτ‖2Λu

+

Nτ∑i=1

‖h(sτ , lcτ,i)− zτ,i‖2Λz)

Here ‖x − y‖2Λ , (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(sτ−1, uτ )−sτ‖2Λu+

Nτ∑i=1

‖h(sτ , lcτ,i)−zτ,i‖2Λz

)(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(µt−1, ut)

Σt = AtΣt−1ATt +GΛuG

T

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 estimateµt and Σt with new measurement. This requirescomputing the Kalman Gain Kt.

Kt = ΣtCTt (CtΣtC

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

−1t−1(I +At)

T + SΛuST]−1

bt = (bt−1H−1t−1 + f(µt−1, 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 + CtΛ−1z CTt

bt = bt + (zt − zt + CTt µt)TΛ−1

z CTt

where 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:t−1)

= p(s1:t|z1:t,u1:t−1)p(m|s1:t, z1:t)

This factorization allows us to first estimate only thetrajectory p(s1:t|z1:t,u1:t−1), 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 is

obtained from the previous generation x(i)t−1 that

follows the proposal distribution π:

x(i)1:t ∼ π(·|z1:t, u1:t−1)

2. Importance weighting An importance weight w(i)t

is assigned to each particle:

w(i)t =

p(x(i)1:t|z1:t, u1:t−1)

π(x(i)1:t|z1:t, u1:t−1)

In most SLAM algorithms, the proposal π is re-stricted to have the recursive form:

π(x1:t|z1:t, u1:t−1) = π(xt|x1:t−1, z1:t, u1:t−1)

· π(x1:t−1|z1:t−1, u1:t−2)

so that the importance weights can be computedrecursively,

w(i)t ∝

p(zt|m(i)t−1, x

(i)t )p(x

(i)t |x

(i)t−1, ut−1)

π(xt|x(i)1:t−1, z1:t, u1:t−1)

· w(i)t−1

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:t−1)

= p(s1:t|z1:t,u1:t−1)K∏k=1

p(µk,Σk|s1:t, z1:t,u1:t−1)

6

The algorithm uses a particle filter to estimatethe trajectory p(s1:t|z1:t,u1:t−1), 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:t−1). 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,

π = p(x1:t|z1:t−1, u1:t)

Particles x(i)1:t−1 are updated purely based on the mo-

tion model p(xt|xt−1, ut−1) 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) is

a 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 particle’s current estimate for a particular grid(when computing weight for this particle), one can searchthis grid’s observation log for entries made by this par-ticle’s 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 to“bundle 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)‖2Ωij 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)‖2Ωij

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,0‖2Ωij

=∑(i,j)

‖Ω12ijJij∆ + Ω

12ijeij,0‖

22

=∑(i,j)

‖Aij∆ + qij‖2

= ‖A∆ + q‖2

where Aij , Ω12ijJij , qij , Ω

12ijeij,0. A ∈ Rm×n 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 ∈ Rn×n 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 ∈ Rn×n is the square root information matrixas above, and Q ∈ Rm×m 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 Hpl

Hlp Hll

] [∆∗p

∆∗l

]=

[bpbl

]where ∆∗

p is the pose variables and ∆∗l is the landmark

variables.An equivalent reduced system on just ∆∗

p can beformed by taking the Schur complement of H ,

(Hpp −HplH−1ll Hlp)∆

∗p = bp −HplH

−1ll bl

Note that computing H−1ll is easy since Hll is block

diagonal (no constraints between landmarks). After ∆∗p

is obtained, we solve

Hll∆∗l = bl +Hlp∆

∗p

9

for the landmark estimates. Again the structure of Hll

simplifies 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 theχ2 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):1113–1139, 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. ICRA’03. IEEE International Conference on, pages 1899–1906, 2003. 6

M Cummins and P Newman. Probabilistic AppearanceBased Navigation and Loop Closing. In Robotics andAutomation, 2007 IEEE International Conference on,pages 2042–2048, 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 176–183, 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 1–10, 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 1314–1320, 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):588–596, 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):381–395, 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):196–207, 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):34–46, 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):428–439, 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 318–325, 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 206–211, 2003. 4.1

D Hahnel, S Thrun, B Wegbreit, and W Burgard. Towardslazy data association in SLAM. Robotics Research,pages 421–431, 2005. 7

Shoudong Huang and Gamini Dissanayake. Convergenceand Consistency Analysis for Extended Kalman FilterBased SLAM. Robotics, IEEE Transactions on, 23(5):1036–1049, 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 4238–4243, 2001. 3.1

M Kaess, A Ranganathan, and F Dellaert. iSAM: In-cremental Smoothing and Mapping. Robotics, IEEETransactions on, 24(6):1365–1378, 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 457–463,2004. 5.5, 6

K Konolige and M Agrawal. FrameSLAM: From bun-dle adjustment to real-time visual mapping. IEEETransactions on Robotics, 24(5):1066–1077, 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 22–29, 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 1442–1447, 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, pages3975–3980, 2010. 8

F Lu and E Milios. Globally consistent range scan align-ment for environment mapping. Autonomous Robots,4(4):333–349, 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 534–539, 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:1151–1156, 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:1015–1021, 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):890–897, 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 635–642, 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 1180–1187,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 2558–2565, 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 1678–1685, 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 2262–2269,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):1107–1120, 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 661–666, 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):56–68, 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, pages1505–1510, 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):693–716,August 2004. 3.1, 3.2, 4

S Thrun and Y Liu. Multi-robot SLAM with sparseextended information filers. Robotics Research, pages254–266, 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):403–429, 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):253–271, 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):335–359, 2007. 3.3

CC Wang, C Thorpe, and S Thrun. Simultaneous lo-calization, mapping and moving object tracking. . . .Journal of Robotics . . . , pages 3975–3980, 2007a. 8

Z Wang, S Huang, and G Dissanayake. D-SLAM: Decou-pled localization and mapping for autonomous robots.Robotics Research, pages 203–213, 2007b. 3.3

M Yannakakis. Computing the minimum fill-in is NP-complete. SIAM Journal on Algebraic Discrete Meth-ods, 2(1):77–79, 1981. 5.3

15