1153

Multimedia Contents 46. Simultaneous Localization Simultaneoand Mapping u atE|46 | E Part

Cyrill Stachniss, John J. Leonard, Sebastian Thrun

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

This chapter provides a comprehensive introduction tured, and of limited size. Robustly mapping unstruc- into one of the key enabling technologies of mobile tured, dynamic, and large-scale environments in an on- robot navigation: simultaneous localization and map- line fashion remains largely an open research problem. ping, or in short SLAM. SLAM addresses the problem The historical roots of methods that can be applied of acquiring a spatial map of an environment while si- to address the SLAM problem can be traced back to multaneously localizing the robot relative to this model. Gauss [46.1], who is largely credited for inventing the The SLAM problem is generally regarded as one of the least squares method. In the Twentieth Century, a num- most important problems in the pursuit of building truly ber of fields outside robotics have studied the making autonomous mobile robots. It is of great practical im- of environment models from a moving sensor platform, portance; if a robust, general-purpose solution to SLAM most notably in photogrammetry [46.2–4]andcom- can be found, then many new applications of mobile puter vision [46.5]. Strongly related problems in these robotics will become possible. fields are bundle adjustment and structure from mo- While the problem is deceptively easy to state, it tion. SLAM builds on this work, often extending the presents many challenges, despite significant progress basic paradigms into more scalable algorithms. Mod- made in this area. At present, we have robust methods ern SLAM systems often view the estimation problem for mapping environments that are mainly static, struc- as solving a sparse graph of constraints and applying (46.3) ], who graph- +1 7 .Ifwe, +1 t t t , z x x 6 , we refer ], and Thrun 8 , Grisetti et al. +1 t problem. Arcs in- SLAM u SLAM , the robot seeks to re- t t SLAM z x ssume that the robot takes : SLAM g T map of the environment. The z t u and the robot location describes their locations. The en- true m m ;:::; is often assumed to be time-invariant, m 3 z illustrates the variables involved in the –1 –1 m ; ]. t t 2 9 z x z ; 1 46.1 z Graphical model of the –1 problem. It shows the sequence of locations and denote the t Df u T m Figure Z Finally, the robot senses objects in the environment. The robot measurements establish information be- For more a detailed treatment of Fig. 46.1 without loss of generality,exactly a one measurement atquence each of point measurements in is given time, as the se- surements are noisy,inevitably and diverge from path the truth. integration techniques Let dicate causal relationships, andobservable shaded to nodes the are robot.cover directly In the unobservable variables SLAM sensor measurements, and thetween causal these relationships variables. be- This diagram represents a environment may be comprised ofsurfaces, landmarks, etc., objects, and vironment map i. e., static. tween features in the reader to Durrant-Whyte andprovide Bailey an [46. in-depth tutorial for chosen by the practitionerfactors, will such depend as on the a desiredtime, number map and of resolution, the the nature update on. of Nevertheless, the the features in threechapter the cover methods map, the discussed major and paradigms in so in this this field. et al., which dedicates a numberof of SLAM chapters [46. to the topic for a tutorial on graph-based SLAM [46. is t x (46.2) (46.1) 1and ). The SLAM  problem 1 t SLAM , and the robot t might be denote the odometry method. The method T t u ining the robot’s position : . However, odometry mea- g g 0 SLAM T T x u x :::; ;:::; 3 often serves as a point of reference 2 is best described in probabilistic ter- . Its motion is uncertain, making it u x 0 0 ; would be sufficient to recover the poses ; x x 2 : Problem Definition 1 T u x . For mobile robots on a flat ground, 2-D) coordinate in the plane plus a single ; t problem is defined as follows: A mobile ; U 1 x 0 ,path is then given as u x SLAM is some terminal time ( SLAM Df Df ; such data might be obtained from the robot’s T T t T SLAM U X Odometry provides relative information between This chapter begins with a definition of the minology. Let uslocation by denote time by time wheel encoders or from the controlstors. given Then to the those sequence mo- that characterized the motion between time usually a three-dimensional vector, comprising itsdimensional two- ( rotational value for its orientation.cations, The or sequence of lo- for the estimation algorithm; othersensed. positions cannot be two consecutive locations. Let gradually more difficult to determineglobal its coordinates. current As pose it in roams,environment the with robot a can noisy sense sensor.is its The the problem ofwhile building simultaneously a determ maprelative to of this the map environment given noisy data. initial location characterizes the relative motion of the robot.free For noise- motion, Here 46.1.1 Mathematical Basis Formally, The robot roams aninitial unknown environment, location starting at an 46.1 nonlinear optimization to compute the mapjectory and of the the tra- robot.autonomous As robots, we an emerging strive challenge to ismassive enable to sensor handle long-lived data streams. from the initial location problem, which shall includeferent a versions brief of taxonomy the of problem.chapter dif- The is centerpiece of a this laymanparadigms in introduction this into field, the andexist. three the As various major the extensions reader that willsingle quickly best recognize, solution to there the is no Moving in the Environment

Part E

Part E | 46.1 1154 Simultaneous Localization and Mapping 46.1 SLAM: Problem Definition 1155 ical model for SLAM. It is useful in understanding the When building 2-D maps, point-landmarks may cor- dependencies in the problem at hand. respond to door posts and corners of rooms, which, The SLAM problem is now the problem of recov- when projected into a 2-D map are characterized by ering a model of the world m and the sequence of a point coordinate. In a 2-D world, each point-landmark robot locations XT from the odometry and measurement is characterized by two coordinate values. Hence the data. The literature distinguishes two main forms of the world is a vector of size 2N,whereN is the number SLAM problem, which are both of equal practical im- of point-landmarks in the world. In a commonly stud- atE|46.1 | E Part portance. One is known as the full SLAM problem:it ied setting, the robot can sense three things: the relative involves estimating the posterior over the entire robot range to nearby landmarks, their relative bearing, and path together with the map the identity of these landmarks. The range and bearing may be noisy, but in the most simple case the identity . ; ; /: p XT m j ZT UT (46.4) of the sensed landmarks is known perfectly. Determin- ing the identity of the sensed landmarks is also known Written in this way, the full SLAM problem is the as the data association problem. In practice, it is one of problem of calculating the joint posterior probability the most difficult problems in SLAM. over X and m from the available data. Notice that the T To model the above described setup, one begins variables right of the conditioning bar are all directly with defining the exact, noise-free measurement func- observable to the robot, whereas those on the left are tion. The measurement function h describes the work- the ones that we want. As we shall see, algorithms for ings of the sensors: it accepts as input a description of the full SLAM problem are often batch, that is, they the environment m and a robot location x ,anditcom- process all data at the same time. t putes the measurement The second, equally important SLAM problem is the online SLAM problem. This problem is defined via h.xt; m/: (46.6)

p.xt; m j Zt; Ut/: (46.5) Computing h is straightforward in our simplified land- Online SLAM seeks to recover the present robot loca- mark setting; it is a simple exercise in trigonometry. The tion, instead of the entire path. Algorithms that address probabilistic measurement model can be derived from the online problem are usually incremental and can pro- this measurement function by adding a noise term. It cess one data item at a time. In the literature, such is a probability distribution that peaks at the noise-free algorithms are typically called filters. value h.xt; m/ but allows for measurement noise, for ex- To solve the SLAM problem, the robot needs to ample, be endowed with two more models: a mathematical model that relates odometry measurements ut to robot p.zt j xt; m/ D N .h.xt; m/; Qt/: (46.7) locations xt1 and xt; and a model that relates measure- ments zt to the environment m and the robot location xt. Here N denotes the 2-D normal distribution, which is These models correspond to the arcs in Fig. 46.1. centered at h.xt; m/. The 2-by-2 matrix Qt is the noise In SLAM, it is common to think of those mathemat- covariance, indexed by time. ical models as probability distributions: p.xt j xt1; ut/ The motion model is derived from a kinematic characterizes the probability distribution of the location model of robot motion. Given the location vector xt1 xt assuming that a robot started at a known location and the motion ut, textbook kinematics tells us how to xt1 and measured the odometry data ut. And likewise, calculate xt. Let this function be denoted by g p.zt j xt; m/ is the probability for measuring zt if this measurement is taken at a known location xt in a known g.xt1; ut/: (46.8) environment m. Of course, in the SLAM problem we do not know the robot location, and neither do we know the The motion model may then be defined by a normal dis- environment. As we shall see, Bayes rule takes care of tribution centered at g.xt1; ut/ but subject to Gaussian this, by transforming these mathematical relationships noise into a form where we can recover probability distribu- tions over those latent variables from the measured data. p.xt j xt1; ut/ D N .g.xt1; ut/; Rt/: (46.9)

46.1.2 Example: SLAM in Landmark Worlds Here Rt is a covariance. It is of size 3-by-3, since the location is a three-dimensional 3-D vector. One common setting of SLAM involves an assumption With these definitions, we have all we need to that the environment is populated by point-landmarks. develop a SLAM algorithm. While in the literature, data problems algorithm algorithms. SLAM SLAM SLAM ) receiver. . When closing a loop, the , the robot actively explores GPS algorithm is purely observing. SLAM . It is one of the most difficult prob- SLAM algorithms, some other entity controls . algorithms assume that the environment problems are defined for a single robot methods tend to yield more accurate maps algorithms allow only for small errors in SLAM problems are distinguished by the degree of SLAM assumes static environments. Dynamic effects problems come in many flavors. In some, robots loop closing problem SLAM SLAM SLAM SLAM Active Versus Passive Static Versus Dynamic Small Versus Large Uncertainty Single-Robot Versus Multi-Robot the location estimate. Theywhich are a robot good goes for downitself, situations a and path in that then does returnsenvironments not it intersect along is the possible samefrom to path. multiple reach In directions. the many a same Here large location the amount of robotas uncertainty. may This the problem accrue isuncertainty known may be large.a The key ability characteristic to of closeThe modern-day loops uncertainty is can beinformation reduced about if its the robot positiondinate can in frame, sense e.g., some through absoluteglobal the coor- positioning system use ( of a satellite-based In passive the robot, and the previously observed landmarks inlem the of map. estimating The the correspondence prob- association is problem known as lems in Static does not changefor over changes time. in Dynamic theSLAM methods environment. The allow vastare literature on often treatedods just that as reason measurementmore about outliers. involved, motion but Meth- they in tendapplications. the to be environment more are robust in most SLAM location uncertainty that they can handle.ple The most sim- controls only the pointing directionsors, of but not the the robot’s motion sen- direction. Most platform, althoughrobot recently exploration has the gained inSLAM popularity. problem Multi-robot ofget to multi- observe each other, inrelative others, robots initial are locations. told their Multirobot The vast majority ofgive algorithms the are robot of designerbitrary this the motion type; freedom controllers, they and toobjectives. pursue implement In arbitrary ar- active motion its environment in thetive pursuit of an accuratein map. less Ac- time, but theyexist constrain the hybrid robot techniques motion. in There which the algo- SLAM SLAM ,andthe / m ; t SLAM is adjacent to x j A t z Problem algorithms are not . p in volumetric techniques tend to be m SLAM . Note that none of those SLAM / t u ; methods provide metric infor- 1 SLAM  t x , the map is sampled at a resolution j t x SLAM . p due to the fact that the extraction of fea- SLAM , the measurement model m problems are distinguished along a number of extracts sparse features from the sensor stream. ). Metric SLAM . Feature-based B Volumetric Versus Feature-Based Known Versus Unknown Correspondence Topological Versus Metric motion model distributions has to bedone restricted in the to example Gaussian above. noise as confined to landmark worlds.map But representation and no the sensor matter modality,algorithm what any needs the a similarlytures crisp in definition of the fea- 46.1.3 Taxonomy of the SLAM different dimensions. Most importantidentify research the type papers of problemsunderlying addressed assumptions by making explicit. the Wetered already one encoun- suchcommon distinction: distinctions are full as follows: versus online. Other In volumetric mation between the relationyears, topological of methods such have places.despite fallen ample In out evidence that of recent humans fashion, ical often information use for topolog- navigation. The correspondence problem ising the the identity problem of of sensedIn things relat- the to other landmark sensed exampleidentity things. above, of we assumed landmarksrithms that is the make known. such anones Some assumption, that others do dotimating not not. the provide The correspondence special of mechanisms for measured es- features to point-landmark problems withare range-bearing by sensing far the most studied, more efficient, but their results maymetric be inferior to volu- tures discards information in the sensor measurements. Some mapping techniques recover only ascription qualitative de- of therelation environment, of which basic locations. characterizes Suchtopological. methods the are A known topological as mapa might be set defined of over tions distinct between places these places andplace (e.g., a place set of qualitative rela- The map ispoint-landmark example is then an instance of onlySLAM feature-based comprised of features. Our high enough to allow forof photo-realistic reconstruction the environment. Theis map usually quite high-dimensional,the with computation the can result beSLAM that quite involved. Feature-based Moving in the Environment

Part E

Part E | 46.1 1156 Simultaneous Localization and Mapping 46.2 The Three Main SLAM Paradigms 1157 are also distinguished by the type of communication al- to traditional methods. They enable the robot to com- lowed between the different robots. In some, the robots pute a solution given the resource constraints of the can communicate with no latency and infinite band- system. The more resources available, the better the width. More realistic are setups in which only nearby solution. robots can communicate, and the communication is As this taxonomy suggests, there exists a flurry of subject to latency and bandwidth limitations. SLAM algorithms. Most modern-day conferences ded- icate multiple sessions to SLAM. This chapter focuses atE|46.2 | E Part Any-Time and Any-Space on the very basic SLAM setup. In particular it assumes Robots that do all computations onboard have limited a static environment with a single robot. Extensions are resources in memory and computation power. Any- discussed towards the end of this chapter, in which the time and any-space SLAM systems are an alternative relevant literature is discussed.

46.2 The Three Main SLAM Paradigms

This section reviews three basic SLAM paradigms, sentations is a critical issue in SLAM, and intimately from which most others are derived. The first, known as related to the topics of sensing and world modeling dis- EKF SLAM, is in robotics historically the earliest but cussedinChap.36 andinPartC. has become less popular due to its limiting computa- The EKF algorithm represents the robot estimate by tional properties and issues resulting from performing a multivariate Gaussian single linearizations only. The second approach uses nonparametric statistical filtering techniques known as p.xt; m j Zt; Ut/ D N .t; †t/: (46.10) particle filters. It is a popular method for online SLAM and provides a perspective on addressing the data asso- The high-dimensional vector t contains the robot’s ciation problem in SLAM. The third paradigm is based best estimate of its own current location xt and the on graphical representations and successfully applies location of the features in the environment. In our sparse nonlinear optimization methods to the SLAM point-landmark example, the dimension of t would be problem. It is the main paradigm for solving the full 3 C 2N, since we need three variables to represent the SLAM problem and recently also incremental tech- robot location and 2N variables for the N landmarks in niques are available. the map. The matrix †t is the covariance of the robot’s as- 46.2.1 Extended Kalman Filters sessment of its expected error in the guess t.The matrix †t is of size .3 C 2N/  .3 C 2N/ and it is pos- Historically, the EKF formulation of SLAM is the earli- itive semi-definite. In SLAM,thismatrixisusually est, and perhaps the most influential, SLAM algorithm. dense. The off-diagonal elements capture the correla- EKF SLAM was introduced in [46.10, 11] and [46.12, tions in the estimates of different variables. Nonzero 13], which were the first papers to propose the use of correlations come along because the robot’s location is a single state vector to estimate the locations of the uncertain, and as a result the locations of the landmarks robot and a set of features in the environment, with an in the maps are uncertain. associated error covariance matrix representing the un- The EKF SLAM algorithm is easily derived for our certainty in these estimates, including the correlations point-landmark example. Suppose, for a moment, the between the vehicle and feature state estimates. As the motion function g and the measurement function h were robot moves through its environment taking measure- linear in their arguments. Then, the vanilla Kalman fil- ments, the system state vector and covariance matrix ter, as described in any textbook on Kalman filtering, are updated using the extended Kalman filter [46.14, would be applicable. EKF SLAM linearizes the func- 15]. As new features are observed, new states are added tions g and h using Taylor series expansion. In its most to the system state vector; the size of the system covari- basic form and in the absence of any data association ance matrix grows quadratically. problems, EKF SLAM is basically the application of This approach assumes a metrical, feature-based the EKF to the online SLAM problem. environmental representation, in which objects can be Figure 46.2 illustrates the EKF SLAM algorithm for effectively represented as points in an appropriate pa- an artificial example. The robot navigates from a start rameter space. The position of the robot and the loca- pose that serves as the origin of its coordinate system. tions of features form a network of uncertain spatial As it moves, its own pose uncertainty increases, as in- relationships. The development of appropriate repre- dicated by uncertainty ellipses of growing diameter. It the ) ], al- ) d small ( 18 , has been a–c ( 17 ]. With an landmarks assumes that 21 and only add , all shaded. ellipses 16 , and its estimates applied to the EKF SLAM problem. The robot’s EKF robot. The method has EKF SLAM white.In ellipses dotted line SLAM , and their location estimates are 20]. Typical implementations also the robot’s positional uncertaintyincreasing, is as is its uncertaintythe landmarks about it encounters. In Fig.46.2a–d online path is a of its own position are Eight distinguishable landmarks of unknown location are shown as dots shown as robot senses the first landmarkand again, the uncertainty of decreases, as does theof uncertainty its current poseof (image Michael courtesy Montemerlo, Stanford University) , 19 which of the landmarks in the map cor- provisional landmark list idea becomes inapplicable. The solution here is The basic formulation of EKF to reason about thea most landmark likely data is associationon observed. when proximity: This isresponds most usually likely doneThe to based proximity the calculation landmarknoise considers just and the observed? the measurement actualand uncertainty in the the metric postera estimate, used Mahalanobis distance, in which isdistance. this a To weighted calculation quadratic minimize is theciations, chances known many of implementations as use falsedistinguish visible data individual features asso- landmarks to andof associate groups landmarks observed simultaneouslythough [46. distinct featureslaser can data also [46. bemaintain computed a from landmarks to theobserved internal sufficiently map frequently when [46. they have been appropriate landmark definition and carefultation of implemen- the data association step, the location of featuresfrom in a the single mapbeen position is extended fully of to observable the situations with partial observability, applied successfully in ausing wide airborne, range of underwater, indoor, environments, platforms. and various other .If 46.2d: can also be ap- 46.2d – notice b) d) ]. Information that helps 16 EKF SLAM uncertain data association posterior [46. SLAM With a few adaptations, c) a) also senses nearby landmarksuncertainty that and combines maps the fixed them measurementtainty uncer- with with an the increasing posethe uncertainty. uncertainty As a in result, time. the The landmark interesting locations transitionHere grows happens the robot over in observes the Fig. landmarkbeginning it of saw mapping, in and the whose very well location known. is Through relatively this observation, theerror robot’s pose is reduced,the as very indicated small inThis error observation Fig. ellipse also reduces forlandmarks the the uncertainty in for the final other map.a robot correlation This pose. that phenomenon is arisesof expressed the from in Gaussian the posterior. covariance Since most matrix in of earlier the uncertainty landmarkpose, estimates and is since this caused verythe uncertainty by location estimates persists of the over those landmarks time, are robot correlated. When gaining information on theformation robot’s pose, spreads this to in- This previously effect is observed probably the landmarks. most importantof characteristic the localize the robot is propagated through thea map, result and improves as the localizationthe of map. other landmarks in plied in the presencethe of identity of observed features is unknown, the basic Moving in the Environment

Part E

Part E | 46.2 1158 Simultaneous Localization and Mapping 46.2 The Three Main SLAM Paradigms 1159 with range-only [46.22] or angle-only [46.23, 24] mea- who coined the name fastSLAM (fast simultaneous lo- surements. The technique has also been utilized using calization and mapping). Let us first explain the basic a feature-less representation, in which the state consists FastSLAM algorithm on the simplified point-landmark of current and past robot poses, and measurements take example, and then discuss the justification for this ap- the form of constraints between the poses (derived for proach. example from laser scan matching or from camera mea- At any point in time, FastSLAM maintains K parti- surements) [46.25, 26]. cles of the type atE|46.2 | E Part A key concern of the EKF approach to SLAM Œk; Œk;:::;Œk ; †Œk;:::;†Œk : lies in the quadratic nature of the covariance matrix. Xt t;1 t;N t;1 t;N (46.11) A number of researchers have proposed extensions to Here Œk is the index of the sample. This expression the EKF SLAM algorithms that achieve scalability, for states that a particle contains: example through submap decomposition [46.27–30]. Œk A related family of approaches [46.31–34] employs the  A sample path Xt ,and Œk Extended Information Filter, which operates on the in-  AsetofN 2-D Gaussians with means t;n and verse of the covariance matrix. A key insight is that †Œk variances t;n), one for each landmark in the en- whereas the EKF covariance is densely populated, the vironment. information matrix is sparse when the full robot tra- jectory is maintained, leading to the development of Here n is the index of the landmark (with 1 Ä n Ä efficient algorithms and providing a conceptual link N). From that it follows that K particles possess K path to the pose graph optimization methods described in samples. It also possesses KN Gaussians, each of which Sect. 46.2.3. models exactly one landmark for one of the particles. The issues of consistency and convergence in Initializing FastSLAM is simple: just set each parti- EKF SLAM have been investigated in [46.35, 36]. cle’s robot location to the starting coordinates, typically . ; ; /T Observability-based rules for designing consistent EKF 0 0 0 , and zero the map. The particle update then SLAM estimators are presented in [46.37]. proceeds as follows:  When an odometry reading is received, new loca- 46.2.2 Particle Methods tion variables are generated stochastically, one for each of the particles. The distribution for generat- The second principal SLAM paradigm is based on par- ing those location particles is based on the motion ticle filters. Particle filters can be traced back to [46.38], model but they have become popular only in the last two Œk . Œk ; /: decades. Particle filters represent a posterior through xt p xt j xt1 ut (46.12) asetofparticles. For the novice in SLAM, each par- Œ  Here x k is the previous location, which is part ticle is best thought as a concrete guess as to what t1 of the particle. This probabilistic sampling step is the true value of the state may be. By collecting many easily implemented for any robot whose kinematics such guesses into a set of guesses, or set of particles, can be computed. the particle filter approximates the posterior distribu- When a measurement z is received, two things hap- tion. Under mild conditions, the particle filter has been  t pen: first, FastSLAM computes for each particle the shown to approach the true posterior as the particle set probability of the new measurement z .Letthein- size goes to infinity. It is also a nonparametric repre- t dex of the sensed landmark be n. Then the desired sentation that represents multimodal distributions with probability is defined as follows ease. The key problem with the particle filter in the con- w Œk N . Œk; Œk; †Œk/: t D zt j xt t;n t;n (46.13) text of SLAM is that the space of maps and robot paths Œk is huge. Suppose we have a map with 100 features. How The factor wt is called the importance weight, many particles would it take to populate that space? since it measures how important the particle is in In fact, particle filters scale exponentially with the di- the light of the new sensor measurement. As before, mension of the underlying state space. Three or four N denotes the normal distribution, but this time it dimensions are thus acceptable, but 100 dimensions are is calculated for a specific value, zt. The importance generally not. weights of all particles are then normalized so that The trick to make particle filters amenable to the they sum to 1. SLAM problem goes back to [46.39, 40] and is known Next, FastSLAM draws with replacement from the as Rao–Blackwellization. It has been introduced into set of existing particles a set of new particles. The the SLAM literature in [46.41], followed by [46.42], probability of drawing a particle is its normalized . , / ]. t Z 44 ; t U SLAM SLAM SLAM j  k Œ t in Gaussian X . / t p Z This subtle but ; t ]. Thus, in U 43 ;  ltiple data association k Œ t X 46.3, then the landmark j m . p 46.3: we find that if we remove small Gaussians, one for each land- , and linear in the number of parti- N N shows results for a point-feature prob- 46.4 illustrates the concept graphically. In problems. Each particle has a sample of an en- . position. 46.4a shows the path of the vehicle obtained by 46.3 M FastSLAM also breaks down the posterior over Figure The FastSLAM algorithm has a number of inter- Blackwellization, and it yieldspling better from results the than joint. sam- FastSLAMin applies that this it technique, samples from the path posterior form. maps (conditioned ondimensional paths) Gaussians. into The sequencescomposition justification of is for low- subtle. this Ittional independence de- arises assumption that from is native to aFig. specific condi- knowledge of the robot pathmates renders independent. all This is landmark esti- easilyical shown for network the in graph- the Fig. path variables from Fig. and represents the map variables are all disconnected [46. important observation impliesa that large, monolithic evenper Gaussian if particle, for we of thetween course), entire used different the map landmarks off-diagonal (one would elementIt simply be- is remain therefore zero. efficiently, legitimate using to implement the map more any dependence between multipleis landmark mediated estimates through the robot path. lem; here thetrunks point as features observedused by are here an is the known outdoor as centersFig. the robot. Victoria of Park The dataset tree dataset [46. cles mark. This explains theFastSLAM. efficient map representation in integrating the vehicle controls, without perception.can As be seen, controls are athis poor predictor vehicle; of location after for 30 minsition of of driving, the the vehicleGPS estimated is po- well over 100 m away fromesting its properties. First,SLAM it solves bothtire full path and but online themost actual recent update pose. equation Thisond, only makes FastSLAM uses FastSLAM can maintain a the mu filter.hypotheses. Sec- It is straightforward totion make decisions on data a associa- per-particleto basis, adopt instead the of same having hypothesiswe for the will entire filter. While notnote give that the any resultingdeal mathematical FastSLAM with justification, algorithm unknown data can we the association even extended – Kalman somethingFastSLAM filter that can cannot be claim. implementedadvanced tree And very methods efficiently third, to using representthe the update map can estimates, be performedsize in of time logarithmic the in map the . . b b ). and 3 +2 +2 t t 2-D m z x a ,drivenby 2 resampling C +2 and one for t t , based on the x u a  n k ; ,where t Œ / b can be described in † it observes a nearby ; / 2 a t +1 +1 a t . t x m z x j p the individual features in b . to location p 1 . This graphical network illus- +1 t g  u t 3 x . This trick is known as Rao– m separate / ; 2 a . This update follows the standard j and covariance m t t t ; z z x b  n 1 k . ; t Œ , all low-dimensional (typically p m  , and attach to each particle a closed-form problem depicted as Bayes network graph. / a Df 1 t . u m p m update rules – note that the extended Kalman posterior. The derivation of FastSLAM exploits SLAM This all may sound complex, but FastSLAM is FastSLAM has been shown to approximate the full EKF filters maintained in FastSLAMEKF SLAM are, in contrast to The intuition behindfor which resampling the is measurementa is that more higher plausible particles chance have cess. of surviving the resamplingFinally, FastSLAM pro- updates for thethe new particle mean set importance weight. This step is called measurement –1 –1 t t z x The three techniques: Rao–Blackwellization, conditional in- dependence, and resampling. Rao–Blackwellizationthe is following concept. Suppose wepute would like a to probability com- distribution quite easy tomodel implement. usually Sampling involves simple fromComputing kinematic the the calculations. importance motion ofstraightforward a too, measurement especially isment often for noise. Gaussian And measure- updatingfilter is a also low-dimensional not particle complicated. SLAM are arbitrary random variables. The vanilla particlewould filter draw particles fromis, each the particle joint would distributions, have a that value for However, if the conditional closed form, it iscles equally from legitimate to just draw parti- description of Moving in the Environment –1 t u

Part E the map from each other.no If other the locations path are involvingtween known, any variables there two remains whose features value inposterior the is of map. any not This two known, features lack(given in the be- of locations) the a map path conditionally renders independent the a sequence offeature controls. in At the each map location trates that the location variables Fig. 46.3 The robot moves from location

Part E | 46.2 1160 Simultaneous Localization and Mapping 46.2 The Three Main SLAM Paradigms 1161

a) Raw vehicle path b) FastSLAM (solid), GPS path (dashed) c) Path and map with aerial image atE|46.2 | E Part

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

a) b)

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

FastSLAM has been extended in several ways. One whose maps are most consistent with the measure- set of variants are grid-based versions of FastSLAM, ment. A resulting large-scale map is shown in Fig. 46.5. in which the Gaussians used to model point landmarks Further extensions can be found in [46.48, 49], whose are replaced by an occupancy grid map [46.45–47]. The methods are called DP-SLAM and operate on ances- variant of [46.46] is illustrated in Fig. 46.5. try trees to provide efficient tree update methods for Figure 46.6 illustrates a simplified situation with grid-based maps. Related to that, approximations to three particles just before closing a large loop. The three FastSLAM in which particles share their maps have different particles each stand for different paths, and been proposed [46.50]. they also posses their own local maps. When the loop The works in [46.45, 47, 51] provide ways to in- is closed importance resampling selects those particles corporate new observations into the location sampling t – 55 .Fur- t SLAM u ], or fil- is a fol- problem 50 SLAM SLAM problem. In more , assuming that at time i ] improve the situation but SLAM Map particle of 1 i. Edges in this graph are m 53 ]. 67 – ]. The graph-based representation 65 54 ], particles sharing maps [46. 45 and landmarks t 3 particles and their trajectories x Optimization Techniques is tied together by an edge that represents the t x ; The basic intuition of graph-based 1  t parameter. Second, nested loops combinedsive with exten- re-visits ofparticle previously depletion, which mapped in turn areas may preventfrom the can system estimating lead a consistent to map.strategies Adaptive [46. resampling the robot sensed landmark ther edges exist betweenlocations the nodes that correspond to lows. Landmarks and robot locations canas be nodes in thought a of graph. Everyx consecutive pair of locations 46.2.3 Graph-Based A third family of algorithmsthrough solves nonlinear the sparse optimization. They drawintuition their from a graphicalproblem representation and of the the first workingproposed solution in in robotics [46. was used here is closely related64]. to a We series of note papersoffline that [46. and most address of therecent the full years, earlier new techniques incrementalre-use versions are the that previously computed effectively solution haveposed been pro- such as [46. information conveyed by the odometry reading ter backup approaches [46. cannot eliminate this problem in general. . Map particle of 1 (46.14) SLAM problem. ; / educated guess SLAM 46.12). This ex- t ], there are sev- u ; 47 1 /  t k  t Œ u x ; dditionalputational com 1 j  t k  t Œ x . x ; p 1 /  t k  t Œ x ; m 1 j  k  t t Œ z . m ]. This leads to an improved sampling p j t 52 z . p Application of the grid-based variant of the FastSLAM algorithm. Each particle carries its own map and the Map particle of 1

 k t Œ The so-far developed particle filters-based Finally, there are approaches that aim to overcome x demands. the assumption thatcharacteristics. the As observations show showneral Gaussian situations in in which [46. thealso model multimodal. is A nonGaussian sumparticle and of bases, Gaussians model however, can onin a be the per- efficiently particle considered practice filter without and introducing it a eliminates this problem in systems suffer from two problems. First,samples the that number of are requiredis to often compute consistent set maps The manually larger by the making uncertaintyresent an that during mapping, the the filter more needs critical to becomes rep- this tension makes FastSLAM and especiallyvariants robust its tools grid-based for addressing the which incorporates the odometryat and the the same observation tribution time. leads Using toThis more an in accurately improved turn sampled proposal leadsa to locations. dis- smaller more number accurate of mapsusing particles compared and the to requires sampling approaches process given in ( process for landmarks andwork grid in maps, [46. basedprocess on prior Fig. 46.6 importance weights of themap particles are computed based on the likelihood of the measurements given the particle’s own Moving in the Environment

Part E

Part E | 46.2 1162 Simultaneous Localization and Mapping 46.2 The Three Main SLAM Paradigms 1163 soft constraints. Relaxing these constraints yields the If we think of the graph as a spring-mass mod- robot’s best estimate for the map and the full path. el [46.60], computing the SLAM solution is equivalent The construction of the graph is illustrated in to computing the state of minimal energy of this model. Fig. 46.7. Suppose at time t D 1, the robot senses To see, we note that the graph corresponds to the log- landmark m1. This adds an arc in the (yet highly in- posterior of the full SLAM problem (46.4) complete) graph between x1 and m1. When caching . ; ; /: the edges in a matrix format (which happens to cor- log p XT m j ZT UT (46.15) atE|46.2 | E Part respond to a quadratic equation defining the resulting constraints), a value is added to the elements be- Without derivation, we state that this logarithm is of the form tween x1 and m1, as shown on the right hand side of Fig. 46.7a. log p.XT ; m j ZT ; UT / Now suppose the robot moves. The odometry read- X . ; / ing u2 leads to an arc between nodes x1 and x2,as D const C log p xt j xt1 ut shown in Fig. 46.7b. Consecutive application of these Xt two basic steps leads to an graph of increasing size, C log p.zt j xt; m/; (46.16) as illustrated in Fig. 46.7c. Nevertheless this graph is t sparse, in that each node is only connected to a small number of other nodes (assuming a sensor with limited assuming independence between the individual obser- sensing range). The number of constraints in the graph vations and odometry readings. Each constraint of the . ; / is (at worst) linear in the time elapsed and in the num- form log p xt j xt1 ut is the result of exactly one robot ber of nodes in the graph. motion event, and it corresponds to an edge in the graph. Likewise, each constraint of the form log p.zt j xt; m/ is the result of one sensor measurement, to which we can a) also find a corresponding edge in the graph. The SLAM x1 m1

x1 problem is then simply to find the mode of this equa- m1 tion, i. e., ;  . ; ; /: XT m D argmax log p XT m j ZT UT (46.17) m1 XT ;m

x1 Without derivation, we note that under the Gaussian noise assumptions, which was made in the point- b) landmark example, this expression resolves to the fol-

x1 x2 m1 lowing quadratic form

x1 m1 x 2 log p.X ; m j Z ; U / D const XT T T Œ . ; /T 1 Œ . ; / C xt  g xt1 ut Rt xt  g xt1 ut m1 „ ƒ‚ … t odometry reading x1 x2 X Œz h.x ; m/T Q1 Œz h.x ; m/ : C „t  t ƒ‚t t  t … t c) feature observation

x1 x2 x3 x4 m1 m2 m3 m4 m2 (46.18) x1

m1 m4 x2 m3 This quadratic form yields a sparse system of equations x3

x4 and a number of efficient optimization techniques can

m1 be applied. Common choices include direct methods m2 such as sparse Cholesky and QR decomposition, or iter- x1 x2 x3 x4 m3 ative ones such as gradient descent, conjugate gradient, m 4 and others. Most SLAM implementations rely on iter- atively linearizing the functions g and h, in which case Fig.46.7a–c Illustration of the graph construction. The (a) the objective in (46.18) becomes quadratic in all of its diagram shows the graph, the (b) the constraints in matrix variables. form. (a) Observation ls landmark m1. (b) Robot motion Extensions to support an effective correction of from x1 to x2. (c) Several steps later large-scale graphs are hierarchical methods. One of ]in EKF 95 SLAM ]). Most 79, , 92 ], proposing 66 78 ]forthe ] combined with 96 46.16). As a result 97 is the covariance matrix, is their ability to constantly re- ] of stochastic gradient descent methods have the advantage that ]. ] have been proposed that estimate 94 83 ]. , 46.18) depends among other things EKF 91 EKF SLAM , 81 93 82, 8 – , SLAM 8 79 ]. There has also been an extension that can ] and pose-graphs [46. , 77 89 75 , for example pseudo Huber and several alterna- , exploiting the sparsity of the graph. The key Graphical We note that the graph-based paradigm is very a max-mixture representation for maintaining efficiency of the log likelihood optimizingof in ( that, thepact multimodal on extension the has runtimemost and optimizers. only Also can little robust easily costSLAM im- function be are incorporated used for in tives [46. they scale to much higher-dimensionalSLAM maps than limiting factor in which takes space (andsize update of the time) map. quadraticmethods. No in The such update the constraint time exists of inthe the graphical graph amount is of constant, memory and mild required assumptions). is A further linear advantage (under ofmethods some graph-based over the mizer [46. deal with multimodal constraints [46. linearize the error functionresults. which Performing often the leads optimization tohowever. Technically, can finding better be the optimal expensive, datation associa- is suspected to be anpractice NP-hard the problem, number although of in plausible assignmentssmall. is The usually continuoushood optimization function of in the ( on log the likeli- number andinitialization size can of have a loops strong ina impact the good on the initial map. result guess Also and canstantially the [46. simplify the optimization sub- closely linked to information theory, in that thestraints soft con- constitute the informationworld the (in robot an has information-theoretic on sense the [46. problem, variable ordering impactsthe the performance optimization. of Otherstures use [46. hierarchical data struc- methods in the fieldthe are entire robot offline path. and Ifmization they the robot may optimize path become for is cumbersome. long,years, the Over opti- the however, last incrementalhave five been optimization proposed techniques thatbut aim not at necessarily providing perfectat a model sufficient every of point thedecisions in environment based time. on This thedetermine current allows exploration model, goals. a In for this robot example,tal context, to incremen- to variants make [46. techniques [46. the smoothing and mapping frameworkat can be each execute stepperformance of by the variable mappingearization. ordering process As and and also selective evaluated achieve relin- in the [46. which part of thenew graph sensor data. requires Incremental methods re-optimization [46. given i ] ). ], m 4 74 (46.19) SLAM 46.18 ] propose an ], which con- ], which recur- 74 , 25 69 ] and in the opti- to be large). Since ] and Kinect cam- 73 ] formulates [46. 76 70 € 75 assigning identical locations /: i not m  j m . ]. It builds a multilevel pose-graph and from the graph and attach all adjacent € j ] is a technique for using independent 65 ] of the form m T 68 / , or we can add a soft correspondence con- i ] have been proposed. i 72 m m 71 is 2-by-2 diagonal matrix whose coefficients de- in the map corresponded to one and the same  j j € m m . When it comes to computing highly accurate envi- The graphical paradigm can be extended to han- Data association errors typically have a strong im- edges to straint [46. Here termine the penalty for and to two landmarks (hence we want employs an incremental, lazy optimizationallows scheme for that optimizing large graphs andcan at be the same executed time atternative hierarchical each approach step is [46. duringsively partitions mapping. the An graph al- using into the multiple-level nested submaps dissection algorithm. ronment reconstructions, approaches thatoptimize do the not poses only vidual of measurement the ofbetter a robot results. dense In but the sensor also spiritapproaches of often each for bundle provide adjustment laser indi- [46. scanners [46. local maps, whicha are place. merged A insented fully in case [46. hierarchical of approach re-visiting has been pre- the first is thestructs ATLAS a framework two-level [46. hierarchy combiningthat a operates Kalman in filter thetion lower level at and a the globalSLAM higher optimiza- [46. level. Similar to that, Hierarchical physical landmark inther remove the world. Then, we can ei- eras [46. dle the data associationadditional problems knowledge on as data we association can into integrate ( graphical methods are usually usedproblem, for the the optimization full cansearch for be the interleaved optimal with data association. the pact in the resulting map estimate.of Even wrong a small data number associationssistent is map likely estimates. to Recently, result novelbeen approaches in proposed have that incon- are robust under afalse certain associations. number For of example, [46. iterative procedure that allows for disablingan constraints, action that is associated withof a this cost. method A introduced generalization in [46. Suppose some oracle informed us that landmarks as a robusttational cost requirements. function Sucha approaches also significant can reducing number deal of the falsevide with associations compu- high-quality and still maps. pro- closure Consistency hypotheses checks can for beas loop found well, in both in other the approaches front-end [46. Moving in the Environment

Part E

Part E | 46.2 1164 Simultaneous Localization and Mapping 46.2 The Three Main SLAM Paradigms 1165

Table 46.1 Recent open-source graph-based SLAM implementations Name Comment Dynamic covariance Optimization with a robust cost function for dealing with outliers scaling (DCS) [46.75] Integrated into g2o g2o [46.80] Flexible and easily extendable optimization framework for SLAM Comes with different optimization approaches and error functions Supports external plugins atE|46.2 | E Part GTSAM2.1 [46.79] Flexible optimization framework for SLAM and SFMstructure from motion Implements direct and iterative optimization techniques Implements smoothing and mapping (SAM), iSAM, and iSAM2 Implements bundle adjustment for Visual SLAM and SFM HOG-Man [46.65] Incremental optimization approach via hierarchical pose graphs and lazy optimization Requires pose-graphs with full rank constraints iSAM2 [46.66] General incremental nonlinear optimization with variable elimination Variable re-ordering to retain sparsity On-demand re-linearization of selected variables KinFu (KinectFusion Open source reimplementation of KinectFusion [46.84] within the point cloud library (PCL) reimplemented) Dense and highly accurate reconstruction using a Kinect camera Currently limited to medium sized rooms MaxMixture [46.78] Optimization for multimodal constraints and outliers Robust to outliers Plugin for g2o Parallel tracking and System for tracking a hand-held monocular camera and observed features mapping (PTAM) [46.85] Operates on comparably small workspaces RGBD-SLAM [46.86] Kinect-frontend for HOG-Man and g2o Fairly standard combination of SURF matching and RANSAC ScaViSLAM [46.87] SLAM system for stereo and Kinect-style cameras Combines local bundle adjustment with sparse global optimization for on-the-fly processing SLAM6-D [46.88] SLAM system that operates on point clouds from 3-D laser data Applies iterative closest point algorithm (ICP) and global relaxation Sparse surface adjustment Optimizes robot poses and proximity sensor data jointly (SSA) [46.70, 71] Provides smooth surface estimates Assumes a range sensor (e.g., laser scanner, Kinect, or similar) TreeMap [46.89] Incremental optimization approach Update in O .log N/ time Provides only a mean estimate TORO [46.90] Optimization approach that extends stochastic gradient descent (SGD) [46.91] Robust under bad initial guesses Recovers quickly from large errors but slow convergence at minimum Assumes that constraints have roughly spherical covariance matrices Provides only a mean estimate Vertigo [46.74] Switchable constraints for robust optimization Plugin for g2o a lazy optimization for on-the-fly mapping [46.65]. As on motivations that are not dissimilar to the graphical an alternative to global methods, relative optimization approach [46.27, 28, 101]. approaches [46.98] aim at computing locally consistent Recently, researchers addressed the problem of geometric maps but only topological maps on the global long-term operation and frequent revisits of already scale. Hybrid approaches [46.87] seek to combine the mapped terrain. To avoid densely connected pose- best of both worlds. graphs that lead to slow convergence behavior, the robot There also exists a number of cross-overs that ma- can switch between SLAM and localization, can merge nipulate the graph online so as to factor out past nodes to avoid a growth of the graph [46.90, 102], or robot location variables. The resulting algorithms are can discard nodes or edges [46.32, 103–105]. filters [46.25, 33, 99, 100],andtheytendtobeinti- Graphical and optimization-based SLAM algorithm mately related to information filter methods. Many of are still subject of intense research and the paradigm the original attempts to decompose EKF SLAM repre- scales to maps large numbers of nodes [46.25, 55, 57, sentations into smaller submaps to scale up are based 59, 63–65, 89, 90, 106, 107]. Arguably, the graph-based ]. 5 , sys- 4 prob- SLAM SLAM has become SLAM ], but did not approach. Sub- seeks to achieve 116 – SLAM EKF ]. This system could 113 SLAM 122 as it allows for compu- , brought up several graph- 23 ] that tracked distinctive vi- er corresponds to a motion or 121 SLAM systems, initially using a real-time was primarily concerned with off- is closely related to the structure from ) problem in computer vision [46. SFM SLAM SFM SLAM Graph-based methods address the full Davison was an early pioneer in developing com- become independent, anda hence are result, decorrelated. FastSLAMa As can sampled robot representGaussians for pose, the its landmarks. and posterior The particle manyoffers representation by advantages local, for independent tationally efficient updates and forassociations. On sampling the over negative side, data thesary number particles of can neces- grow very large,seeking especially to for map robots multiple nested loops. early approaches were hamperedcient computational by resources the to handle lackdata massive of video streams. suffi- Earlyon approaches extended were Kalman typically filters [46. based lem, hence are inThey the standard draw formulation not theircan online. intuition be modeled form bywhere the a each constraint sparse fact eith graph that a of measurement soft event. constraints, Dueefficient to optimization the methods availability for of sparse nonlineartimization highly op- problems, graph-based the method ofRecent choice developments for have based building methods large-scale for maps. incrementalbe map executed building at that every timeassociation can step search during can navigation.mathematical Data be framework incorporated and different intoare approaches the even that robust under basic wrong data associationsable are today. avail- Historically, sequent work developed the firsttem real-time that operated withas the a only single data freely source [46. moving camera compute the full covariancecamera for trajectory, the resulting featureVisual in poses and a lossmotion of ( consistency. line batch processing, whereas a solution for online operation, suitableinteraction for of closed-loop a robotcomparison or to laser user scanners, with cameras provide itsdrant a environment. of fire In hy- information, makingimpossible online processing until nearly recent increasesbecome available. in computation have plete visual active stereo head [46. sual features with a full covariance build sparse, room-size maps offrame-rate indoor in scenes real-time, at a 30 notable Hz historical achievement , 86 maps ], but or can SLAM SLAM 112 , SLAM . By sampling . As discussed, using data from black box is anticipated to SLAM components, such community started EKF 3-D models of the DOF SLAM ical interaction with the SLAM SLAM SLAM ] are flexible and powerful are based on building local with monocular, stereo, om- 80 , ], entail handheld camera motions, 46.1). Especially the optimization ]. Visual 79 , 85 66 110 ] or RGB-D (Kinect) sensors [46. SLAM – the challenge of building maps and applications of interest, such as aug- comes with a computational hurdle that 108 EKF SLAM SLAM SLAM ]. Visual sensors offer a wealth of information that Attempting Particle filter methods sidestep some of the is- Visual navigation and mapping was a key early goal cameras [46. 109 enables the constructionworld. They of also rich enableclosing difficult to be issues addressed such ininformation novel as ways loop- using [46. appearance be a critical arearobotics, for as future we research seekare in to capably perception develop of intelligent low-cost for world. phys systems that A popular and important topicVisual in recent years hastracking been the robot pose in full 6- 46.3 Visual and RGB-D submaps; however, in manyrithms resemble ways the graph-based the approach. resulting algo- sues arising from thein natural the inter-feature correlations map – which hindered the to release flexible optimization frameworksimplementations and under openport source further licenses developments tocomparisons, and (Table sup- to allow for efficient 46.2.4 Relation of Paradigms The three paradigms justjority discussed cover of the vast work ma- EKF in SLAM theposes field serious of scaling limitationsmay lead and to inconsistent the maps. The linearization tensions most promising of ex- from robot poses, the individual landmarks in the map paradigm has generated someever the built. largest Furthermore, the nidirectional, or RGB-Ddifficulty cameras of raises many the of level-of- the frameworks [46. state of the art tools forsystems. developing They graph-based can be either used as a as data associationscribed and above. computational A efficiency,visual key de- challenge ismented reality robustness. [46. Many which present greaterin difficulties comparison for to state the motion estimation,a of flat a floor. wheeled robot across in the mobile robotics community [46.111 be easily extended though plugins. Moving in the Environment

Part E

Part E | 46.3 1166 Simultaneous Localization and Mapping 46.3 Visual and RGB-D SLAM 1167

a) b) –200 –140

–120 –150 –100

–100 –80 46.3 | E Part

x (m) –60

–50 –40

–20 0 0

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

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

a) b)

Fig. 46.9 (a) 3-D Model and (b) close-up view of a corridor environment in the Paul G. Allen building at University of Washington built from Kinect data (after [46.109]; image courtesy of Peter Henry, University of Washington) in visual SLAM research. A difficulty encountered with A milestone in creating robust visual SLAM sys- initial monocular SLAM [46.23] was coping with the tems was the introduction of keyframes in parallel initialization of points that were far away from the cam- tracking and mapping (PTAM) [46.85], which sep- era, due to nonGaussian distributions of such feature lo- arated the tasks of keyframe mapping and localiza- cations resulting from poor depth information. This lim- tion into parallel threads, improving robustness and itation was overcome in [46.24], introducing an inverse performance for online processing. Keyframes are depth parameterization for monocular SLAM,akey now a mainstream concept for complexity reduc- development for enabling a unified treatment of initial- tion in visual SLAM systems. Related approaches ization and tracking of visual features in real-time. using keyframes include [46.87, 123–126]. The work as ) sys- b ( shows 46.9 SLAM ]. Figure image depicts a larger ) 86 c ( ] and [46. 109 A local scene as a normal map and ) a Results obtained with KinectFusion (af- ( ]). 134 Other researchers aim at exploiting the surface a) b) c) scene (image courtesyCollege of London) Richard Newcombe, Imperial ter [46. Fig.46.10a–c a Phong-shaded rendering. The tems include [46. examples of the output of these systems. properties of scanned environments to correct for sen- provements in mapping and navigationdoor systems environments. State-of-the-art for RGB-D in- , ] , sys- 129 118 tech- ]tolo- SLAM 131 SLAM SLAM ] and [46. is that camera ] to compute a full 128 ] aim at large-scale 98 SLAM ]. The idea of employ- 137 46.8). ] that is optimized for , 46.8). Finally, view-based 138 130 – 136 , 3-D mapping and localization ]. Here, [46. 136 , 126 ] was one of the first to employ 127 126 110 , systems developed in the early 2000s. ], which combines bag-of-words loop ]. Other notable recent examples in- 119 133 , 135 . ] analyzes the tradeoffs between filtering and 98 SLAM systems [46. 108 laser 46.2.3 ] has demonstrated appearance-only place recogni- As pointed out by Davison and other researchers, Visual information offers a tremendous source of in- The techniques described above have formed the Several compelling in [46. keyframe-based bundle adjustment in visual and concluded that keyframe bundle adjustment outper- forms filtering, as it provides theof most computing accuracy time. per unit an appealing aspectmeasurements of can provide visual odometryindeed information, visual and odometry is aSLAM key component of modern efficient operation on smallis available unmanned today. aerial vehicles formation for loop closing, not present2-D in the canonical provide an extensive tutorialodometry, of including techniques feature for detection,ing, feature visual outlier match- rejection,trajectory optimization. and Finally, a constraint publiclysual available estimation, vi- odometry and library [46. The work in [46. techniques for visual object recognition [46. cation recognition. More recently132 FAB-MAP [46. tion at large scale,of mapping 1000 trajectories km. withwith Combining a a length a probabilisticinference bag-of-features place model approach leads andagainst to Chow–Liu perceptual place tree aliasingtationally recognition while efficient. that Other remainingincludes is work [46. compu- on robust place recognition basis for a numbertems of developed notable large-scale insue recent of years. thea A IEEE good snapshot 2008 Transactions of special onniques recent state-of-the-art is- Robotics [46. provides clude [46. closing with tests ofconditional geometrical random consistency fields (Fig. based on ing relative bundle adjustment [46. maximum likelihood solutioneven for in loop an closures,resentation by that online employing does a fashion, not manifoldconstraints attempt rep- to results enforce Euclidean inhigh frame maps rate (see that also Fig. can be computed at mapping systems [46. and/or life-long visualgraph optimization mapping techniques described basedtion above on in Sec- the pose have been created in recentsensors. years The with combination RGB-D of direct (Kinect) range measurements with dense visual imagery can enable dramatic im- Moving in the Environment

Part E

Part E | 46.3 1168 Simultaneous Localization and Mapping 46.4 Conclusion and Future Challenges 1169

Fig. 46.11 Spatially ex- tended KinectFusion output produced in real-time with Kintinuous (after [46.139]) atE|46.4 | E Part

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

46.4 Conclusion and Future Challenges

This chapter has provided an introduction into SLAM, The following references provide an in-depth tuto- which is defined as the problem faced by a mobile rial on SLAM and much greater depth of coverage on platform roaming an unknown environment, and seek- the details of popular SLAM algorithms. Furthermore, ing to localize itself while concurrently building a map several implementations of popular SLAM systems, in- of the environment. The chapter discussed three main cluding most of the approaches listed in Table 46.1, paradigms in SLAM, which are based on the ex- can be found in online resources such as http://www. tended Kalman filter, particle filters, and graph-based openslam.org or in the references [46.6, 9, 21, 62]. sparse optimization techniques, and then described re- The considerable progress in SLAM in the past cent progress in Visual/Kinect SLAM. decade is beyond doubt. The core state estimation at per- ], more re- 105 , 102 robustly for days, weeks, SLAM poses difficult challenges to most !1 t navigation and mapping – the capability for An ultimate goal is to realize the challenge of iew-chapter/46/videodetails/454 search is needed formistakes techniques and that enable canthe robots recover environment and from to enabling a dealexistence. long-term autonomous with changes in current algorithms; innavigation fact, algorithms most robot arepassage mapping doomed of and time, torecent as fail encouraging errors with solutions inevitably [46. accrue. the Despite sistent a robot toor perform months atsion, a in complex timethe and with limit dynamic as minimal environments. Taking human supervi- lisation and mapping at the level of objects http://handbookofrobotics.org/view-chapter/46/videodetails/446 http://handbookofrobotics.org/view-chapter/46/videodetails/447 http://handbookofrobotics.org/view-chapter/46/videodetails/449 http://handbookofrobotics.org/view-chapter/46/videodetails/450 http://handbookofrobotics.org/view-chapter/46/videodetails/451 http://handbookofrobotics.org/view-chapter/46/videodetails/452 http://handbookofrobotics.org/view-chapter/46/videodetails/453 http://handbookofrobotics.org/v http://handbookofrobotics.org/view-chapter/46/videodetails/455 http://handbookofrobotics.org/view-chapter/46/videodetails/442 http://handbookofrobotics.org/view-chapter/46/videodetails/443 http://handbookofrobotics.org/view-chapter/46/videodetails/444 http://handbookofrobotics.org/view-chapter/46/videodetails/445 http://handbookofrobotics.org/view-chapter/46/videodetails/439 http://handbookofrobotics.org/view-chapter/46/videodetails/440 http://handbookofrobotics.org/view-chapter/46/videodetails/441 is now quite well understood, Extended Kalman filter SLAM available from Pose graph compression for laser-basedavailable SLAM from DTAM: Dense tracking and mappingavailable in from real-time MonoSLAM: Real-time single camera SLAM available from SLAM++: Simultaneous loca available from Graph-based SLAM using TORO available from Sparse pose adjustment available from Pose graph compression for laser-basedavailable SLAM from Pose graph compression for laser-basedavailable SLAM from Graph-based SLAM available from Graph-based SLAM available from Graph-based SLAM available from Graph-based SLAM available from Deformation-based loop closure for Denseavailable RGB-D from SLAM Large-scale SLAM using the Atlasavailable framework from Graph-based SLAM available from SLAM hardware development. VIDEO 455 VIDEO 451 VIDEO 452 VIDEO 453 VIDEO 454 VIDEO 446 VIDEO 447 VIDEO 449 VIDEO 450 VIDEO 442 VIDEO 443 VIDEO 444 VIDEO 445 VIDEO 439 VIDEO 440 VIDEO 441 GPU Video-References the heart of and a numberbeen developed, of including impressivesource several software implementations widely implementations used have cial and open projects. some None-the-less, a commer- number ofchallenges open remain research formapping the in general complex problem andextended of periods dynamic of robotic environments time, over includingtending, robots and sharing, ex- revising previously builtfailure models, efficient recovery, zero useron intervention, resource-constrained systems. and Another operation excitingfor area the future is thevisual further mapping development of systems fully exploiting dense in the latest advances Moving in the Environment

Part E

Part E | 46 1170 Simultaneous Localization and Mapping References 1171

References

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

46.2 D.C. Brown: The bundle adjustment – Progress (ISER) (2010) 46 | E Part and prospects, Int. Arch. Photogramm. 21(3), 3:3– 46.20 G.D. Tipaldi, L. Spinello, W. Burgard: Geometrical 3:35 (1976) flirt phrases for large scale place recognition in2D 46.3 G. Konecny: Geoinformation: Remote Sensing, range data, Proc. IEEE Int. Conf. Robotics Autom. Photogrammetry and Geographical Information (ICRA) (2013) Systems (Taylor Francis, London 2002) 46.21 T. Bailey: Mobile Robot Localisation and Mapping 46.4 B.Triggs,P.McLauchlan,R.Hartley,A.Fitzgibbon: in Extensive Outdoor Environments, Ph.D. Thesis Bundle adjustment – A modern synthesis, Lect. (Univ. of Sydney, Sydney 2002) Notes Comput. Sci. 62, 298–372 (2000) 46.22 J.J. Leonard, R.R. Rikoski, P.M. Newman, M. Bosse: 46.5 R. Hartley, A. Zisserman: Multiple View Geometry Mapping partially observable features from mul- in Computer Vision (Cambridge Univ. Press, Cam- tiple uncertain vantage points, Int. J. Robotics bridge 2003) Res. 21(10), 943–975 (2002) 46.6 T. Bailey, H.F. Durrant-Whyte: Simultaneous lo- 46.23 A.J. Davison: Real-time simultaneous localisation calisation and mapping (SLAM): Part II, Robotics and mapping with a single camera, Proc. IEEE 9th Autom. Mag. 13(3), 108–117 (2006) Int. Conf. Comput. Vis. (2003) pp. 1403–1410 46.7 H.F. Durrant-Whyte, T. Bailey: Simultaneous lo- 46.24 J.M.M. Montiel, J. Civera, A.J. Davison: Unified in- calisation and mapping (SLAM): Part I, Robotics verse depth parametrization for monocular SLAM, Autom. Mag. 13(2), 99–110 (2006) Robotics Sci. Syst., Vol. 1 (2006) 46.8 G. Grisetti, C. Stachniss, W. Burgard: Nonlinear 46.25 M. Bosse, P.M. Newman, J. Leonard, S. Teller: constraint network optimization for efficient map Simultaneous localization and map building in learning, IEEE Trans. Intell. Transp. Syst. 10(3), large-scale cyclic environments using the Atlas 428–439 (2009) Framework, Int. J. Robotics Res. 23(12), 1113–1139 46.9 S.Thrun,W.Burgard,D.Fox:Probabilistic (2004) Robotics (MIT Press, Cambridge, 2005) 46.26 J. Nieto, T. Bailey, E. Nebot: Scan-SLAM: Combin- 46.10 R. Smith, M. Self, P. Cheeseman: A stochastic ing EKF-SLAM and scan correlation, Proc. IEEE Int. map for uncertain spatial relationships, Proc. Int. Conf. Robotics Autom. (ICRA) (2005) Symp. Robotics Res. (ISRR) (MIT Press, Cambridge 46.27 J. Guivant, E. Nebot: Optimization of the si- 1988) pp. 467–474 multaneous localization and map building algo- 46.11 R. Smith, M. Self, P. Cheeseman: Estimating un- rithm for real time implementation, IEEE Trans. certain spatial relationships in robotics. In: Au- Robotics. Autom. 17(3), 242–257 (2001) tonomous Robot Vehicles, ed. by I.J. Cox, G.T. Wil- 46.28 J.J. Leonard, H. Feder: A computationally efficient fong (Springer Verlag, Berlin, Heidelberg 1990) method for large-scale concurrent mapping and pp. 167–193 localization, Proc. 9th Int. Symp. Robotics Res. 46.12 P. Moutarlier, R. Chatila: Stochastic multisensory (ISRR), ed. by J. Hollerbach, D. Koditschek (1999) data fusion for mobile robot location and envi- pp. 169–176 ronment modeling, 5th Int. Symp. Robotics Res. 46.29 J.D. Tardós, J. Neira, P.M. Newman, J.J. Leonard: (ISRR) (1989) pp. 207–216 Robust mapping and localization in indoor envi- 46.13 P. Moutarlier, R. Chatila: An experimental sys- ronments using sonar data, Int. J. Robotics Res. tem for incremental environment modeling by 21(4), 311–330 (2002) an autonomous mobile robot, 1st Int. Sym. Exp. 46.30 S.B. Williams, G. Dissanayake, H.F. Durrant- Robotics (ISER) (1990) Whyte: Towards multi-vehicle simultaneous lo- 46.14 R. Kalman: A new approach to linear filtering and calisation and mapping, Proc. IEEE Int. Conf. prediction problems, J. Fluids 82, 35–45 (1960) Robotics Autom. (ICRA) (2002) pp. 2743–2748 46.15 A.M. Jazwinsky: Stochastic Processes and Filtering 46.31 R.M. Eustice, H. Singh, J.J. Leonard: Exactly sparse Theory (Academic, New York 1970) delayed-state filters for view-based SLAM, IEEE 46.16 M.G. Dissanayake, P.M. Newman, S. Clark, Trans. Robotics 22(6), 1100–1114 (2006) H.F. Durrant-Whyte, M. Csorba: A solution to 46.32 V. Ila, J.M. Porta, J. Andrade-Cetto: Information- the simultaneous localization and map building based compact pose SLAM, IEEE Trans. Robotics (SLAM) Problem, IEEE Trans. Robotics Autom. 17(3), 26(1), 78–93 (2010) 229–241 (2001) 46.33 S. Thrun, D. Koller, Z. Ghahramani, H.F. Durrant- 46.17 J. Neira, J. Tardos, J. Castellanos: Linear time Whyte, A.Y. Ng: Simultaneous mapping and lo- vehicle relocation in SLAM, Proc. IEEE Int. Conf. calization with sparse extended information fil- Robotics Autom. (ICRA) (2003) pp. 427–433 ters, Proc.5th Int. Workshop Algorithmic Found. 46.18 J. Neira, J.D. Tardos: Data association in stochas- Robotics, ed. by J.-D. Boissonnat, J. Burdick, tic mapping using the joint compatibility test, K. Goldberg, S. Hutchinson (2002) ental mapping, Auton. ,217–236(2012) 31 , 31–43 (2010) 2 (6), 1365–1378 (2008) , 141–150 (2005) (3), 287–300 (2002) 21 24 , 333–349 (1997) 4 12 (1), 30–38 (2007) omput. Intell. Robotics Autom. (CIRA) (2000) pp. 318–325 A Tutorial on Graph-based SLAM, IEEE Trans.Transp. Intell. Syst. Mag. C. Hertzberg: Hierarchical optimization onfolds mani- for online 2D and 3D mapping,Conf. Proc. Robotics IEEE Autom. Int. (ICRA) (2010) J.J. Leonard,smoothing F. and mapping Dellaert:Int. J. using Robotics iSAM2: Res. the Bayes Incremental tree, Natl. Conf. Artif.2004) pp. Intell. 457–463 (MIT Press, Cambridge mapping of urban structures, SpringerRobotics Tract. Adv. cremental Smoothing andRobotics Mapping, IEEE Trans. wellized particle filters, J.55 RoboticsAuton. Syst. FastSLAM 2.0: An improved particle filteringrithm algo- for simultaneous localization andthat mapping provably converges, Int. Jt. Conf.(IICAI) Artif. Intell. (Morgan Kaufmann,pp. 1151–1156 San Francisco 2003) The unscented particle filter,Proc. Adv. Neural form. In- Process. Syst. Conf. (2000) pp. 584–590 particle diversity incle a filter Rao-Blackwellized forProc. parti- IEEE SLAM Int. Conf. after Roboticspp. Autom. actively (ICRA) 655–660 (2005) closing loops, alignment for environm Robots A self-correctingRobotics Autom. map, (ICRA) (2004) pp. Proc. 383–390 IEEEand mapping Int. – A discussion, Proc. Conf. IJCAIReason. Workshop Uncertain. Robotics (2001) pp. 17–26 of dead-reckoning errors inIEEE/RSJ map Int. Conf. building, Intell. Robots Proc. Syst.pp. (IROS) 905–911 (1998) large cyclic environments, Proc.C IEEE Int. Symp. ed. by S. Thrun,(MIT G. Sukhatme, Press, S. Cambridge Schaal, 2005) O. Brock ally consistent maps by relaxation, Proc.Conf. Robotics IEEE Autom. Int. (ICRA) (2000) pp. 3841–3846 line learning of globally consistentRobots maps, Auton. 46.62 G. Grisetti, R. Kümmerle, C. Stachniss, W. Burgard: 46.65 G. Grisetti, R. Kümmerle, C. Stachniss, U. Frese, 46.66 M. Kaess, H. Johannsson, R. Roberts, V. Ila, 46.63 K. Konolige: Large-scale map-making, Proc. AAAI 46.64 M. Montemerlo, S. Thrun: Large-scale robotic 3-D 46.67 M. Kaess, A. Ranganathan, F. Dellaert: iSAM: In- 46.51 D. Roller, M. Montemerlo, S. Thrun, B. Wegbreit: 46.52 R. van der Merwe, N. de Freitas, A. Doucet, E.46.53 Wan: C. Stachniss, G. Grisetti, W. Burgard: Recovering 46.54 F. Lu, E. Milios: Globally consistent range scan 46.58 J. Folkesson, H.I. Christensen:46.59 Graphical SLAM: U. Frese, G. Hirzinger: Simultaneous localization 46.60 M. Golfarelli, D. Maio, S. Rizzi: Elastic correction 46.61 J. Gutmann, K. Konolige: Incremental mapping of 46.55 F. Dellaert: Square root SAM,46.56 Robotics Sci. Syst., T. Duckett, S. Marsland, J. Shapiro: Learning glob- 46.57 T. Duckett, S. Marsland, J. Shapiro: Fast, on- , , 18 23 ,ed. ,502– (Morgan 29 (4), 335–359 26 (5), 1036–1049 (2007) 23 Rao–Blackwellized parti- . Baiker: Autonomous nav- 44(247), 335–341 (1949) (3), 81–91 (1945) 37 Probabilistic Reasoning in Intelligent sparse extended informationbased filters SLAM, for Int. feature- J.(2007) Robotics Res. Consistency ofIEEE/RSJ the Int. Conf. Intell. EKF-SLAM Robots Syst.pp. (IROS) algorithm, 3562–3568 (2006) Proc. sistency analysis for extended Kalman filter based SLAM, IEEE Trans. Robotics servability-based Rules forEKF Designing SLAM Consistent Estimators, Int. J. Robotics Res. 528 (2010) J. Am. Stat. Assoc. ased sequential estimation, Ann. Math. Stat. 105–110 (1947) estimation of statistical parameters, Bull. Calcutta Math. Soc. 34–46 (2007) efficient FastSLAM algorithmgeneratingmaps for cyclic of large-scale environments from rawrange laser measurements, Proc. IEEE/RSJ Int.tell. Conf. In- Robots Syst. (IROS) (2003) Evaluation of gaussian proposalmapping distributions for with rao-blackwellized particleProc. filters, IEEE/RSJ Int. Conf. Intell.(2007) Robots Syst. (IROS) neous localization and mappingtermined without landmarks, prede- Proc. 16th Int.Intell. Jt. Conf. (IJCAI) Artif. (2003) pp. 1135–1142 Int. Conf. Roboticspp. Autom. 1314–1320 (ICRA), Vol. 2 (2004) D. Nardi: Fast and accurate SLAM with Rao-Black- cle filtering forSequential dynamic MonteBayesian Carlo networks. Methods In: in Practice by A. Docout, N.Berlin de 2001) Freitas, pp. N. 499–516 Gordon (Springer, FastSLAM: A factored solutionous localization to and mapping the problem, Proc. simultane- AAAI Natl. Conf. Artif. Intell. (2002) Systems: Networks of Plausible Inference Kaufmann, San Mateo 1988) igation and map buildingsors using laser in range sen- outdoor17(10), 565–583 applications, (2000) J. Robotics Syst. techniques for gridwellized particle mapping filters, with IEEE Trans. Rao-Black- Robotics 46.34 M.R. Walter, R.M. Eustice, J.J. Leonard: Exactly 46.35 T. Bailey, J. Nieto, J. Guivant, M. Stevens, E. Nebot: 46.36 S. Huang, G. Dissanayake: Convergence and con- 46.37 G.P. Huang, A.I. Mourikis, S.I. Roumeliotis: Ob- 46.38 N. Metropolis,46.39 S. Ulam: The Monte Carlo method, D. Blackwell: Conditional expectation and unbi- 46.40 C.R. Rao: Information and accuracy obtainable in 46.46 D. Hähnel, D. Fox, W. Burgard, S. Thrun: A highly 46.47 C. Stachniss, G. Grisetti, W. Burgard, N.46.48 Roy: A. Eliazar, R. Parr: DP-SLAM: Fast, robust simulta- 46.49 A. Eliazar, R.46.50 Parr: DP-SLAM G. 2.0, Grisetti, G.D. Proc. Tipaldi, C. Stachniss, IEEE W. Burgard, 46.41 K. Murphy, S. Russel: 46.42 M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit: 46.43 J. Pearl: 46.44 J. Guivant, E. Nebot, S 46.45 G. Grisetti, C. Stachniss, W. Burgard: Improved Moving in the Environment Part E

1172 Part E | 46 Simultaneous Localization and Mapping References 1173

46.68 C. Estrada, J. Neira, J.D. Tardós: Hierarchical 46.85 G. Klein, D. Murray: Parallel tracking and map- SLAM: Real-time accurate mapping of large en- ping for small AR workspaces, IEEE ACM Int. Symp. vironments, IEEE Trans. Robotics 21(4), 588–596 Mixed Augment. Real. (ISMAR) (2007) pp. 225– (2005) 234 46.69 K. Ni, F. Dellaert: Multi-level submap based SLAM 46.86 F. Endres, J. Hess, N. Engelhard, J. Sturm, D. Cre- using nested dissection, Proc. IEEE/RSJ Int. Conf. mers, W. Burgard: An evaluation of the RGB-D Intell. Robots Syst. (IROS) (2010) SLAM system, Proc. IEEE Int. Conf. Robotics Autom. 46.70 M. Ruhnke, R. Kümmerle, G. Grisetti, W. Burgard: (ICRA) (2012) atE|46 | E Part Highly accurate maximum likelihood laser map- 46.87 H.Strasdat,A.J.Davison,J.M.M.Montiel,K.Kono- ping by jointly optimizing laser points and robot lige: Double window optimisation for constant poses, Proc. IEEE Int. Conf. Robotics Autom. (ICRA) time visual SLAM, Int. Conf. Computer Vis. (ICCV) (2011) (2011) 46.71 M. Ruhnke, R. Kümmerle, G. Grisetti, W. Burgard: 46.88 A. Nüchter: 3D robot mapping, Springer Tract. Adv. Highly accurate 3D surface models by sparse sur- Robotics 52 (2009) face adjustment, Proc. IEEE Int. Conf. Robotics 46.89 U. Frese: Treemap: An O.log n/ algorithm for in- Autom. (ICRA) (2012) door simultaneous localization and mapping, 46.72 Y. Liu, S. Thrun: Results for outdoor-SLAM using Auton. Robots 21(2), 103–122 (2006) sparse extended information filters, Proc. IEEE Int. 46.90 G. Grisetti, C. Stachniss, S. Grzonka, W. Burgard: Conf. Robotics Autom. (ICRA) (2003) A tree parameterization for efficiently computing 46.73 N. Sünderhauf, P. Protzel: BRIEF-Gist – Closing the maximum likelihood maps using gradient de- loop by simple means, Proc. IEEE/RSJ Int. Conf. In- scent, Robotics Sci. Syst. (2007) tell. Robots Syst. (IROS) (2011) pp. 1234–1241 46.91 E. Olson, J.J. Leonard, S. Teller: Fast iterative 46.74 N. Sünderhauf, P. Protzel: Switchable constraints alignment of pose graphs with poor initial esti- for robust pose graph SLAM, Proc. IEEE/RSJ Int. mates, Proc. IEEE Int. Conf. Robotics Autom. (ICRA) Conf. Intell. Robots Syst. (IROS) (2012) (2006) pp. 2262–2269 46.75 P. Agarwal, G.D. Tipaldi, L. Spinello, C. Stach- 46.92 T.M. Cover, J.A. Thomas: Elements of Information niss, W. Burgard: Robust map optimization using Theory (Wiley, New York 1991) dynamic covariance scaling, Proc. IEEE Int. Conf. 46.93 E. Olson, J.J. Leonard, S. Teller: Spatially-adap- Robotics Autom. (ICRA) (2013) tive learning rates for online incremental SLAM, 46.76 E. Olson: Recognizing places using spectrally clus- Robotics Sci. Syst. (2007) tered local matches, J. Robotics Auton. Syst. 46.94 G. Grisetti, D. Lordi Rizzini, C. Stachniss, E. Olson, 57(12), 1157–1172 (2009) W. Burgard: Online constraint network optimiza- 46.77 Y. Latif, C. Cadena Lerma, J. Neira: Robust loop tion for efficient maximum likelihood map learn- closing over time, Robotics Sci. Syst. (2012) ing, Proc. IEEE Int. Conf. Robotics Autom. (ICRA) 46.78 E. Olson, P. Agarwal: Inference on networks of (2008) mixtures for robust robot mapping, Robotics Sci. 46.95 M. Kaess, A. Ranganathan, F. Dellaert: Fast incre- Syst. (2012) mental square root information smoothing, Int. 46.79 F. Dellaert: Factor graphs and GTSAM: A hands-on Jt. Conf. Artif. Intell. (ISCAI) (2007) introduction, Tech. Rep. GT-RIM-CP & R-2012-002 46.96 P. Agarwal, E. Olson: Evaluating variable reorder- (Georgia Tech, Atlanta 2012) ing strategies for SLAM, Proc. IEEE/RSJ Int. Conf. 46.80 R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, Intel Robots Syst. (IROS) (2012) W. Burgard: G²o: A general framework for graph 46.97 K. Ni, D. Steedly, F. Dellaert: Tectonic SAM: exact, optimization, Proc. IEEE Int. Conf. Robotics Autom. out-of-core, submap-based SLAM, Proc. IEEE Int. (ICRA) (2011) Conf. Robotics Autom. (ICRA) (2007) pp. 1678–1685 46.81 D.M. Rosen, M. Kaess, J.J. Leonard: An incre- 46.98 G. Sibley, C. Mei, I. Reid, P. Newman: Adaptive mental trust-region method for robust online relative bundle adjustment, Robotics Sci. Syst. sparse least-squares estimation, Proc. IEEE Int. (2009) Conf. Robotics Autom. (ICRA) (2012) pp. 1262– 46.99 P.M. Newman, J.J. Leonard, R. Rikoski: Towards 1269 constant-time SLAM on an autonomous under- 46.82 L. Carlone, R. Aragues, J. Castellanos, B. Bona: water vehicle using synthetic aperture sonar, 11th A linear approximation for graph-based simul- Int. Symp. Robotics Res. (2003) taneous localization and mapping, Robotics Sci. 46.100 M.A. Paskin: Thin junction tree filters for simul- Syst. (2011) taneous localization and mapping, Int. Jt. Conf. 46.83 G. Grisetti, R. Kümmerle, K. Ni: Robust optimiza- Artif. Intell. (IJCAI) (Morgan Kaufmann, New York tion of factor graphs by using condensed mea- 2003) pp. 1157–1164 surements, Proc. IEEE/RSJ Int. Conf. Intell. Robots 46.101 S.B. Williams: Efficient Solutions to Autonomous Syst. (IROS) (2012) Mapping and Navigation Problems, Ph.D. Thesis 46.84 S. Izadi, R.A. Newcombe, D. Kim, O. Hilliges, (ACFR Univ. Sydney, Sydney 2001) D. Molyneaux, S. Hodges, P. Kohli, J. Shotton, 46.102 H. Johannsson, M. Kaess, M.F. Fallon, J.J. Leonard: A.J. Davison, A. Fitzgibbon: Kinectfusion: Real- Temporally scalable visual SLAM using a reduced time dynamic 3D surface reconstruction and in- pose graph, RSS Workshop Long-term Oper. Au- teraction, ACM SIGGRAPH Talks (2011) p. 23 ton. Robotic Syst. Chang. Environ. (2012) (2), 78– 19 (8), 958– 29 (6), 1052–1067 29 reduction, Proc. (4), 80–92 (2011) 18 (5), 1066–1077 (2008) 24 (6), 854–867 (2011) 115 3–20 (2006) (5), 929–931 (2008) (1), Part I: The firstRobotics 30 Autom. Mag. years fundamentals, and IEEE Part II: Matching, robustness,applications, IEEE Robotics optimization, Autom. Mag. and 90 (2012) D. Maturana, D. Fox, N.mapping Roy: Visual for odometry autonomous and flightcamera, Proc. using Int. Symp. Robotics Res.RGB-D an (ISRR) (2011) trieval approach to object matching inConf. videos, Computer Int. Vis. (ICCV) (2003) p. 1470 pearance basedProc. navigation IEEE Int. and Conf. Roboticspp. loop Autom. 2042–2048 (ICRA) (2007) closing, J. Neira: Robustcameras, Proc. place IEEE/RSJ recognitionSyst. Int. (IROS) with Conf. (2010) stereo Intell. Robots O. Hilliges, J. Shotton,D. D. Kim, Molyneaux, S. A.dense Hodges, Fitzgibbon: surface Kinectfusion: mappingInt. Real-time and Sym. tracking, Mixedpp. IEEE/ACM 127–136 Augment. Real. (ISMAR) (2011) special issue on visual24 SLAM, IEEE Trans. Robotics maps, Proc. IEEE/RSJ Int. Conf.(IROS) Intell. (2009) Robots pp. Syst. 1156–1163 lich, M. Calonder, V. Lepetit, P. Fua: View- dle adjustment, Int. J.980 Robotics (2010) Res. using active vision, Eur. Conf.(1998) Comput. pp. Vis. 809–825 (ECCV) MonoSLAM: Real-time singleTrans., camera Pattern Anal. Mach. SLAM, Intell. IEEE (2007) mented reality using cameraping tracking in and multiple regions, map- Comput. Vis.derst. Image Un- recovery for real time monocularVis. SLAM, Conf. Br. Mach. (2008) graph SLAMIEEE/RSJ with Int. Conf. Intell. complexity Robotspp. Syst. (IROS) 3017–3024 (2010) dle adjustment to real-time visualTrans. mapping, Robotics IEEE for ground vehicle applications, J.23 Field Robotics 46.128 D. Scaramuzza, F. Fraundorfer: Visual odometry. 46.129 F. Fraundorfer, D. Scaramuzza: Visual odometry. 46.130 A.S. Huang, A. Bachrach, P. Henry, M. Krainin, 46.131 J. Sivic, A. Zisserman: Video Google:46.132 A text re- M. Cummins, P.M. Newman: Probabilistic ap- 46.133 C. Cadena, D. Gálvez, F. Ramos,46.134 J.D. Tardós, R.A. Newcombe, A.J. Davison, S. Izadi, P. Kohli, 46.135 J. Neira, A.J. Davison, J.J. Leonard: Guest editorial 46.136 K. Konolige, J. Bowman: Towards lifelong visual 46.137 K. Konolige, J. Bowman, J.D. Chen, P. Mihe- 46.121 A. Davison, D. Murray: Mobile robot localisation 46.122 A.J. Davison, I. Reid, N.46.123 Molton, O. R.O. Castle, G. Klein, Stasse: D.W. Murray: Wide-area aug- 46.124 E. Eade, T. Drummond: Unified loop closing46.125 and E. Eade, P. Fong, M.E.46.126 Munich: K. Monocular Konolige, M. Agrawal: FrameSLAM: From bun- 46.127 D. Nister, O. Naroditsky, J. Bergen: Visual odometry 7(6), (3), 239–248 (6), 792–803 3 5 (12), 1198–1210 57 (11), 1219–1230 (2012) (5), 595–599 (2009) 31 (10), 1144–1158 (2012) 28 61 (9), 1100–1123 (2010) (5), 647–663 (2012) 31 30 , 11–12 (2009) 28 45–65 (1988) sion androbots, navigation IEEE Trans. Robotics in Autom. buildings for mobile a vocabulary tree, Proc.Vis. IEEE Pattern Int. Recognit. Conf. (ICCVPR) Comput. (2006) pp. 2161– making, Proc. Int. Symp. Robotics Res.Press, (ISRR) Cambridge (MIT 1984) pp. 287–293 in the Real WorldThesis by a (Stanford Seeing Univ., Robot Stanford Rover, 1980) Ph.D. fusing noisy visual maps, Int. J. Robotics Res. time monocular SLAM: WhyConf. filter?, Robotics Proc. Autom. IEEE (ICRA) Int. (2010) RGB-D mapping: Using depth3D cameras for dense modelingJ. Robotics Res. of indoor environments, Int. (1989) navigation, IEEE J. Robotics Autom. (1987) forward stereo processing,pp. Alvey 97–102 Vis. Conf. (1989) P. Newman: The new college vision andset, laser Int. data J. Robotics Res. loop, Proc. IEEE/RSJ Int.(IROS) Conf. (2006) Intell. Robots Syst. J.J. Leonard:visual Real-time SLAMJ. 6-DOF Robotics Auton. over Syst. multi-session large scale environments, based node marginalizationcation and for edge pose-graph SLAM, sparsifi- Robotics Proc. Autom. IEEE (ICRA) Int. (2013) Conf. a square root informationciation, matrix J. for Robotics data Auton. asso- (2009) Syst. retic compression of pose graphsSLAM, for Int. laser-based J. Robotics Res. SLAM atJ. Robotics large Res. scale withmins, FAB-MAP A. 2.0, Harrison,D. Int. C. Schroter, Mei, L.I. I. Reid: Murphy, Posner, Navigating, R. W.urban spaces recognising Shade, with Churchill, vision and and laser, D. Int,Res. J. describing Robotics Cole, outdoor navigation using adaptive relative bun- 2168 46.114 D. Kriegman, E. Triendl, T. Binford: Stereo vi- 46.110 D. Nister, H. Stewenius: Scalable recognition with 46.111 R.A. Brooks: Aspects of mobile robot visual map 46.112 H. Moravec: Obstacle Avoidance and Navigation 46.113 N. Ayache, O. Faugeras: Building, registrating, and 46.108 H. Strasdat, J.M.M. Montiel, A.J. Davison: Real- 46.109 P. Henry, M. Krainin, E. Herbst, X. Ren, D. Fox: 46.115 L. Matthies, S. Shafer: Error modeling46.116 in stereo S. Pollard, J. Porrill, J. Mayhew: Predictive feed- 46.117 M. Smith, I. Baldwin, W. Churchill, R. Paul, 46.106 U. Frese, L. Schröder: Closing a million-landmarks 46.107 J. McDonald, M. Kaess, C. Cadena, J. Neira, 46.103 N. Carlevaris-Bianco, R.M. Eustice: Generic factor- 46.104 M. Kaess, F. Dellaert: Covariance recovery from 46.105 H. Kretzschmar, C. Stachniss: Information-theo- 46.118 M. Cummins, P.M. Newman: Appearance-only 46.119 P.M. Newman, G. Sibley, M. Smith, M. Cum- 46.120 G. Sibley, C. Mei, I. Reid, P. Newman: Vast-scale Moving in the Environment Part E

1174 Part E | 46 Simultaneous Localization and Mapping References 1175

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