Auton RobotDOI 10.1007/s10514-014-9386-z

Feature based graph-SLAM in structured environments

P. de la Puente · D. Rodriguez-Losada

Received: 19 November 2012 / Accepted: 24 January 2014© Springer Science+Business Media New York 2014

Abstract Introducing a priori knowledge about the latentstructure of the environment in simultaneous localization andmapping (SLAM), can improve the quality and consistencyresults of its solutions. In this paper we describe and analyzea general framework for the detection, evaluation, incorpo-ration and removal of structure constraints into a feature-based graph formulation of SLAM. We specifically showhow including different kinds and levels of features in a hier-archical manner allows the system to easily discover newstructure and why it makes more sense than other possi-ble representations. The main algorithm in this frameworkfollows an expectation maximization approach to iterativelyinfer the most probable structure and the most probable map.Experimental results show how this approach is suitable forthe integration of a large variety of constraints and how ourmethod can produce nice and consistent maps in regular envi-ronments.

Keywords Mobile robots · Mapping · SLAM ·Structured environments

1 Introduction

Most human designed scenarios present regular geometricalproperties that should be preserved in the maps built and usedby mobile robots aiming at performing safe autonomous orsemi-autonomous navigation. If some information about thelayout characteristics of the operation environment is avail-

P. de la Puente (B) · D. Rodriguez-LosadaETSI Industriales, Universidad Politecnica de Madrid,c/Jose Gutierrez Abascal, 2, 28006 Madrid, Spaine-mail: paloma.delapuente@upm.es

D. Rodriguez-Losadae-mail: diego.rlosada@upm.es

able, its proper representation and application may lead tomuch more meaningful models and significantly improvethe accuracy of the resulting maps. Indeed, when simulta-neous localization and mapping (SLAM) is formulated ina Bayesian framework (Thrun et al. 2005), making use of aprior for the environment based on its structure fits very well,appearing as a need that comes up naturally (de la Puente andCensi 2012). Also, human cognition exploits domain knowl-edge to a large extent, usually employing a priori assumptionsfor the interpretation of situations and environments.

There are several different probabilistic SLAM techniquesto which one can apply this perspective. This article con-cerns the so-called graph-based approach, which has recentlybecome very popular (Olson et al. 2006; Grisetti et al. 2007b;Olson 2008; Grisetti et al. 2010b; Kümmerle et al. 2011;Kaess et al. 2008). Other SLAM solutions can be found inthe literature (Durrant-Whyte and Bailey 2006; Bailey andDurrant-Whyte 2006).

In general, the graph formulation of SLAM seeks to find amaximum probability configuration for a sequence of robotposes and a set of observed features (Grisetti et al. 2010a).Several previous methods (Olson et al. 2006; Grisetti et al.2007b, 2009), apply a marginalization of the features andleave only the trajectory, with robot poses represented asnodes in a graph and constraints between those nodes beingconstituted by rigid body transformations and their associ-ated covariances. Some constraints are usually created fromodometry data, while reobserving features in the environ-ment allows constraints between non successive poses to beadded. The generation of graph constraints is referred to asthe SLAM front-end, while the subsequent minimization ofthe error of those constraints is known as the SLAM back-end.

Recently, it has been noted that the marginalization offeatures may not be the best option for dealing with prior

123

Auton Robot

information (Trevor et al. 2010; Parsley and Julier 2011).Particularly, this kind of approaches are not well suited forhandling or exploiting the possible existence of structure inthe environment. In this paper we study how to manage therepresentation of structure in a hierarchy of nodes and con-straints, with different levels of abstraction, marginalizingout poses instead of features. Only the last robot pose andthe features belong to the graph. Poses are marginalized outat each iteration, with some approximations that work wellin practice.

In the paper, we show some advantages of this new rep-resentation method and present an account of the featuresand constraints we use, with some hints on several problemsthat may be encountered. The nodes in our graph are featurescreated at different levels. Structure constraints are detectedfrom the current state and a graph minimization is performediteratively, following a novel expectation maximization (EM)(Dempster et al. 1977) based approach. These constraints areweighted according to their reliability and constraints con-sidered incorrect can be detected and removed. With thisperspective, the SLAM front and back-end stages can bene-fit from each other’s results. These are the main contributionsof the paper.

The paper is organized as follows. After discussing relatedwork in Sect. 2, the mathematical basis and the derivation ofour EM based method to incorporate structure are explainedin Sect. 3. Then, in Sect. 4, we describe the features andconstraints we use and present our method for hierarchicallyrepresenting the structure of the environment within a fea-ture based graph SLAM approach. In Sect. 5 we describe theincremental graph building process and the structure detec-tion algorithms that we use to create structure features andconstraints, along with the outline of the proposed generalmapping algorithm. Sect. 6 contains our experimental results,comparisons and analysis and, finally, in Sect. 7, we summa-rize our conclusions and future work.

2 Related work

Using prior knowledge about the structure of the environ-ment seems to be promising in robotics. For path planningin unknown but structured environments, Dolgov and Thrun(2008) propose a Markov-random-field model to detect thelocal principal directions of the environment. They extractlines using computer vision morphological operations andthe Hough transform, but their approach is independent ofthe method used to obtain the features. In the SLAM context,depending on the underlying estimation technique, differentapproaches have been followed. Chong and Kleeman (1997)apply collinearity constraints to enhance the state estimationgiven by an Unscented Kalman Filter. Rodríguez-Losada etal. (2006) reduce the linearization errors introduced by the

extended kalman filter (EKF) by the imposition of parallelismand orthogonality constraints between segments within theSPMap (Castellanos et al. 1999) framework. Nguyen et al.(2007) build accurate simplified plane based 3D maps witha fast and lightweight solution based on parallelism and per-pendicularity assumptions. Enforcing a priori known relativeconstraints also leads to consistency and efficiency improve-ments for particle filters, as shown by Beevers and Huang(2008).

The use of prior knowledge in graph-based approacheshas been exploited by Kümmerle et al. (2011) to achieveglobal consistency of outdoor maps by introducing associ-ation constraints for the correspondences detected between3D scans and publicly available aerial images. Also, Karg etal. (2010) propose a solution for SLAM in structured multi-story buildings, adding global alignment constraints betweencorresponding places at different floors.

Parsley and Julier (2010) further study the challenges ofemploying prior information in SLAM. Their work presentssome similarities to ours, as they also consider a hierarchy ofstructure elements, yet theirs is oriented towards part-of rela-tionships and not as much towards pure geometrical relationsbetween features. They propose a probabilistic framework totransfer information between different map representations,for the utilization of generic prior knowledge. Their imple-mentation is EKF based.

Trevor et al. (2010) take an approach closely related toours, too. They propose to incorporate virtual measurementsto create constraints between features in a graph, employ-ing the square root SAM algorithm of Dellaert (2005) andthe M-space feature representation proposed by Folkessonet al. (2007). We deem our framework more flexible, as itintegrates the iterative detection, hierarchical representationand rearrangement of structure. Their tests and conclusionssuggest that several sensors or preprocessing techniques areneeded in order for the alignment of points to be inferred,whereas our system can work with data from a single sensoror from different sensors, with high level features or withdata not preprocessed at all. The hierarchical representa-tion method allows different types of structure elements tobe added or removed at different levels. Furthermore, thismethod allowed us to develop quite a larger variety of con-straints and to conduct more complex experiments, which inturn revealed some possible problems that to our knowledgehave not been tackled before.

A more recent work by Parsley and Julier (2011) alsopresents important similarities to the approach we propose.Their method integrates a prior map into graph SLAM, withdata association between features in each map inferred bymeans of an approximation to EM. This work is anotherone of the very few which consider the need to establishconstraints between features. The main difference of bothapproaches is that we detect the high level features defining

123

Auton Robot

the structure by estimating a hierarchy of structure elements,without needing a prior map. We believe there is value in thiscontribution, as a prior map may not always be available inSLAM and it may not be needed if the environment is indeedstructured. They present one experiment in which planes areobtained from point clouds and constrained to their counter-parts in the prior map. The key novelty of our approach liesin the model selection process over a wide set of priors byusing EM. Actually, the method we propose could be usedfor creating different kinds of prior maps.

The standard graph-based approach originated with thework by Lu and Milios (1997). They presented a brute-forcemethod for globally refining a map by minimizing the errorin a network of pose relations. Thrun and Montemerlo (2006)included landmarks in the graph, together with the robotposes, and proposed applying variable elimination to mar-ginalize out the features. Since then, both the construction(front-end) and the minimization (back-end) of the graphhave been addressed in different ways.

Olson et al. (2006), for instance, developed an efficientstochastic gradient descent based optimization engine thatallowed large pose graphs to be corrected even with poorinitial estimates. Grisetti et al. (2007b) presented an exten-sion of this method, introducing a novel tree parametriza-tion of the nodes. Later, in more recent work, Grisetti et al.(2010b) solved the minimization system with a sparse solverpackage named CSparse (Davis 2006), which is used in thiswork too. Olson (2008) also proposed a front-end based on arobust place recognition algorithm. Kuemmerle et al. (2011)introduced g2o, an open-source C++ general and efficientframework for optimizing graph-based nonlinear error func-tions. Their approach can be easily applied to a large vari-ety of problems, including 2D SLAM with landmarks, andit allows the user to interchange different solvers. Carloneet al. (2011), recently proposed and theoretically analyzeda linear approximation for graph-based SLAM with robotposes.

The full SLAM problem is equivalent to the Structure fromMotion problem studied by the computer vision researchcommunity. In this context, Gallup et al. (2010) use piece-wise planar priors for 3D scene reconstruction in the presenceof non-planar objects. They apply a minimization approachinvolving a data term and a smoothness term, where a classi-fier learned from hand-labeled planar and non-planar imageregions is employed. In general, the concept of priors inSLAM is highly related to the concept of regularizers inoptimization. Newcombe et al. (2011), for instance, pro-pose a regularized cost that promotes smooth reconstructionin dense visual SLAM. In a novel approach to mapping withdense data, the prior is a regularization required to make theproblem well posed (de la Puente and Censi 2012). The workpresented in this paper does not use a prior fully specified inadvance. Yet, once the structure is inferred at each iteration,

its contribution to the error function could be considered sim-ilar to a regularizer.

3 Structure and map inference withexpectation-maximization (EM)

Our graph notation is similar to that used by Olson et al.(2006) and Grisetti et al. (2007b):

– x is the state vector representing the nodes.– A function f (xi , x j ) = fi j (x) represents the constraint

equations between nodes i and j with expected valuesui j and variances Pi j .

– The error value at a given estimate for xi and x j is definedas ei j (x) = fi j (x) − ui j .

The cost, or negative log probability of the node positions,in vectorial form, is given by:

− ln p(x) ∝ eT(x)P−1e(x) (1)

In our case, we propose to distinguish between two dif-ferent types of constraints:

– We denote the error values due to structural constraintsimposed between features i and j by esi j and the corre-sponding covariance by Psi j .

– The rest of the constraints come from the measurements.The corresponding error values will be denoted ezi j , withcovariance Pzi j .

A prior for the structure of the environment could be givenin advance, e.g. the points are aligned and the lines are eitherparallel or orthogonal to each other; or, the points shouldbelong to circles and the circles ought to be concentric; andso on. These priors could be defined in different ways; a goodchoice would be to use a domain-specific language that thesystem could interpret, as proposed by de la Puente and Censi(2012) for a general approach based upon maintaining a com-mon dense representation close to the measurements spacewhile maintaining the expressivity of methods employinggeometric primitives.

With the graph approach and notation described earlier,considering that the involved densities are Gaussian, it seemsintuitive that the cost function to minimize, put in vectorialform as in Eq. 1, would be:

eTz (x)P−1

z ez(x) + eTs (x)P−1

s es(x) (2)

However, the prior for the structure may not always beexplicit and it may rather be inferred from the estimate forthe state x .

In a Bayesian context, we will denote the measurementsz and the structure s. The measurements will be modeled by

123

Auton Robot

the corresponding constraints. The structure will be modeledas a collection of virtual features and constraints (see Sect. 4),with a weight associated to each constraint depending on thecurrent state of the features.

We aim at determining the most probable state x consid-ering both the measurements, z, and the unknown structureof the environment, s. Empirical Bayes techniques (Carlinand Louis 2000) provide a natural framework for performinginference when the prior distribution is not fixed in advance,estimating it from the data. They are based upon an itera-tive procedure which, with our variables, is defined by thefollowing two steps:

p(x | z) =∑

s

p(x | z, s)p(s | z) (3)

p(s | z) =∑

x

p(s | x)p(x | z) (4)

The main issue here is that computing the sums overall possible structures and all possible states is intractable.Therefore, we assume that the true distributions p(s | z) andp(x | z) are sharply peaked and can be replaced by deltafunctions, which results in an approximation to EM:

x∗ = argmaxx

{p(x | z)} ≈ argmaxx

{p(x | z, s∗)p(s∗ | z)}= argmax

x{p(x | z, s∗)} (5)

s∗ = argmaxs

{p(s | z)} ≈ argmaxs

{p(s | x∗)p(x∗ | z)}= argmax

s{p(s | x∗)} (6)

where s∗ and x∗ are point estimates representing the peaksof p(s | z) and p(x | z).

In the first place, optimization must be applied to the graphcontaining constraints derived from the measurements only,so as to obtain an initial estimate for x∗. For solving Eq. 6,different structure detection algorithms can be employed tofind the most probable virtual features and constraints for agiven x∗ (see Sect. 5).

Once s∗ has been inferred, the estimation of the most prob-able state given the measurements and the structure can beupdated by means of Eq. 5. In the literature, it is usuallyaccepted that the landmarks are independent. We argue thatin structured environments this is not completely true, sowe model the relations between them through connectionsdefining the structure.

When the most probable structure given the measurementshas been obtained, z and s∗ are independent given x. It canbe assumed that the prior over the nodes, p(x), is uniform,which implies that:

p(x | z, s∗) ≈ p(x | z)p(x | s∗) (7)

Applying this result, we obtain x∗ by minimizing the errorof the measurements and the structure constraints jointly:

x∗ ≈ argminx

{eTz (x)Pz

−1ez(x) + eTs (x)P ′

s−1es(x)}, (8)

with (P ′s)

−1 formed by the information matrices of the con-straints scaled according to their corresponding weights. Thecovariances of the structure constraints are fixed by the useraccording to the available prior knowledge (e.g. building con-struction tolerances). This implicitly imposes a scale factorbetween different kinds of constraints depending on the rel-ative degree of confidence about the structure of the environ-ment.

Finally, both terms can be grouped into

eTa (x)PA

−1ea(x), (9)

with ea containing the error values from all the constraints(structural and non-structural), and PA

−1 integrated by theproperly scaled information matrices of all the constraints.

This equation is clearly analogous to Eq. 1; likewise theback-end minimization can be carried out in a variety of ways.We follow the approach used by Olson et al. (2006) andGrisetti et al. (2007b) and linearize f (x) = F + JΔx aroundthe last state estimate, with column vector F particularizingf at that estimate and matrix J denoting the Jacobian of theconstraint equations with respect to the state. Defining theresidual r = u − F and substituting e(x) = f (x) − u intothe cost equation 1, it becomes:

− ln p(x) ∝ (JΔx − r)T P−1(JΔx − r) (10)

Differentiating with respect to Δx , minimizing the costcomes to solving the following linear system (Olson et al.2006):

(J T P−1 J )Δx = J T P−1r (11)

The weight or coefficient representing the quality of the fitof the features to a structure constraint will be denoted δ. Wecompute this as the p value from a χ2 test (degrees of freedomequal to the constraint’s dimension), to evaluate the probabil-ity that the residual r comes from a zero-mean normal withthe constraint’s covariance. These weights somewhat com-pensate for the approximation made by considering only themost probable structure, disregarding the rest. If one δ isbelow a given significance level we can reject the hypothesisthat r comes from the assumed distribution; we take this asa good indicator for removing the constraint. Even though ahigher δ value does not imply that the hypothesis is correct,this approach has produced good results thus far. Using arobust kernel or M-estimators (Huber 1981) could be a goodalternative that should be explored.

After getting a solution, those constraints whose δ coef-ficient is small are considered wrong assumptions about thestructure and get broken, so that the following EM step canrelax the topology for a better final solution.

123

Auton Robot

4 Hierarchical representation of structure

Our approach is based on the representation of the underlyingstructure of the environment as weighted structure constraintsbetween features in a graph.

Let us focus on the arrangement of the structural con-straints. We will see how the feature hierarchy, which definesthe structure of the environment, provides an expressive, con-venient and useful means to represent and manage such infor-mation.

Besides adding virtual measurements or relations asTrevor et al. (2010) do, in our method virtual features canbe automatically created to represent common geometricalstructural elements present in most human designed environ-ments, without the need for any extra sensors or prior maps.

4.1 Parametrization of features and formulationof constraints

We have used the following parametrizations for the features.

– Point v = (vx , vy): defined by its cartesian coordinates.– Pose p = (px , py, pθ ): defined by its cartesian coordi-

nates plus the orientation angle in (−π, π ].– Segment s = (sx , sy, sθ ): defined by a pose. A length

parameter ls is used for drawing purposes and data asso-ciation, but it is not included as part of the feature’s state.

– Line l = (d, α): represented by the orthogonal distancefrom the line to the origin and the orientation angle ofthe line. If the pose lp = (lx , ly, lθ ) defines the referenceframe of the line with respect to the origin, these stateparameters can be expressed as:

d = yl cos θl − xl sin θl (12)

α = θl (13)

– Circle c = (cx , cy, R): defined by its center plus thevalue of the radius.

– Orientation o = (oθ ): defined by an angle in (−π, π ].

With those features, some of the most important con-straints we have developed thus far are as follows:

– Point to point distance. This constraint imposes a givendistance between two points on the same plane. Ourimplementation makes it valid for several combinationsof some of the aforementioned features: points, pose loca-tions and circle centers. The function f , considering twopoints v1 = (v1x , v1y ) and v2 = (v2x , v2y ) in general, is:

f (v1, v2) = ||v1−v2||2 =√

(v1x −v2x )2+ (v1y −v2y )

2

(14)

The jacobians with respect to the first and second featuresare, respectively:

δ f

δv1=

⎛

⎝v1x −v2xf (v1,v2)

v1 y−v2 yf (v1,v2)

⎞

⎠T

,δ f

δv2=

⎛

⎝v2x −v1xf (v1,v2)

v2 y−v1 yf (v1,v2)

⎞

⎠T

(15)

The dimensions of the jacobians depend on the featuresemployed; for circles and poses the remaining elementsare completed with zeros in the entries corresponding tonon used parameters (radius or orientation angle, respec-tively).When the expected distance between two points is zero,the jacobians in Eq. 15 happen to present a singularitythat makes the solution oscillate as the points approacheach other. As a nice workaround, we employ a partic-ular constraint for this case and set the expected vectorbetween the two points to be the null vector, with functionf defined as follows:

f (v1, v2) =(

v2x − v1x

v2y − v1y

),

whose jacobians are very easy to obtain.The linearization based method used to solve Eq. 1 cansometimes cause overshooting in the minimization prob-lem, and depending on the formulation of the constraintsthere may be valleys in the linearized weighted error func-tion (Eq. 10). These issues did not really cause any prob-lems in the experiments presented in this paper, so theyare analyzed elsewhere (de la Puente 2012).

– Point to line distance. This constraint allows an orthog-onal distance from one point v = (vx , vy) to a linel = (d, α) to be set. The function f , in this case, is:

f (v, l) = −d − vx sin α + vy cos α (16)

Hence, the jacobians with respect to the point and the lineare, respectively:

δ f

δv=

(− sin α

cos α

)T

δ f

δl=

( −1lx cos α + ly sin α − vx cos α − vy sin α

)T

(17)

– Point to circle distance. This constraint establishes adistance from a point v = (vx , vy) to the circumferenceof a circle c = (cx , cy, R). The function f is definedas:

f (v, c) =√

(vx − cx )2 + (vy − cy)2 − R (18)

123

Auton Robot

so the jacobians with respect to the point is computed asin Eq. 15 and the jacobian with respect to the circle hasan extra component with value equal to −1.

– Pose to pose. This constraint is the one used in standardgraph based SLAM approaches. It forces a given relativetransformation between two poses p1 = (p1x , p1y , p1θ )

and p2 = (p2x , p2y , p2θ ). The function f is now three-dimensional (three dof); it is given by:

f ( p1, p2) = �( p1) ⊕ p2 =⎛

⎝xrel

yrel

θrel

⎞

⎠ (19)

where ⊕ is the standard operator for the compositionof transformations and � is the inverse transformationoperator. The corresponding jacobians can be found inthe literature (Smith and Cheeseman 1986). As the para-metrization is the same, this constraint is also used todefine relative transformations from poses to segmentsand from segments to other segments.

– Pose to segment collinearity. This constraint allows usto correct a pose p = (px , py, pθ ) based on its relativeposition with respect to a segment s = (sx , sy, sθ ), with-out changing the segment, just for pure localization. Ingeneral, as the segment may not be detected entirely dueto limitations in the sensors’ range and occlusions, therobot pose is only corrected in terms of orientation andperpendicular distance to the segment. The function f ,therefore, has dimension two and is defined as:

f ( p, s)

=(

sx sin sθ − sy cos sθ − px sin sθ + py cos sθ

pθ − sθ

)

(20)

while the jacobians are implemented as:

δ f

δ p=

(− sin sθ cos sθ 00 0 1

),δ f

δs= 02x3 (21)

– Line to orientation. This constraint establishes a givenangle from a line l = (d, α) to an orientation featureo = (oθ ). In this case, the function f is one-dimensionaland linear:

f (l, o) = oθ − α (22)

The jacobians with respect to the line and the orientationfeature are trivial to compute, so we do not include themhere. We also implemented an equivalent constraint forsegments but it is very similar to this last constraint, sowe do not think it is necessary to describe it here.

Fig. 1 Hierarchical representation of structure. In this example, pointsextracted from sensor data are included as level 0 features of the graph.Structure detection discovers the alignment of points, which can bemodeled by virtual lines that are incorporated as level 1 features. Sub-sequent structure detection produces virtual orientations added as level2 features

– Orientation to orientation. This constraint sets a certainangle between two orientations o1 = o1θ and o2 = o2θ .The function f is again one-dimensional and linear:

f (o1, o2) = o2θ − o1θ (23)

The jacobians are also very simple and hence omittedhere.

4.2 Hierarchy of features

In general, the initial graph is composed of features (e.g.points or segments) and distance relations obtained directlyfrom local measurements. As we will see in Sect. 5, detec-tion algorithms for frequently found structural elements areembedded into the framework itself and allow higher virtuallevels in the graph to be created. For instance, if we havean environment with points (they could be trees in a garden)and we know that some of them may be aligned, we candefine the structure as a virtual line (two degrees of freedom,dof) and a set of one dof distance constraints between eachof the aligned points and the line. As the virtual line wouldhave been extracted from the points, it would be added tothe graph as a one level higher feature. Carrying on with thismotivating example, if several virtual lines were detected andadded to the graph it would be possible that these lines orien-tations were somehow related. Instead of imposing orienta-tion constraints between pairs of lines (applying the equalitytransitive relation), we also consider referring them to vir-tual orientation features one level above in the graph. Thisway, a better common model can be used for all the lineswith related properties and more relations can be found forthe new virtual features defined. Fig. 1 shows a hierarchicalfeature-based graph corresponding to one example of thiskind.

Features in a given level allow the next one to be cre-ated, so that new structure features and constraints can bediscovered from previously found structure. This provides

123

Auton Robot

Table 1 Some examples of possible configurations for the features

Example # Level 0 Level 1 Level 2 Level 3 Level 4

1 Points (laser measurements) Lines Orientations – –

2 Points (laser measurements) Circles Circle centers Lines Orientations

3 Segments (laser measurements) Orientations – – –

4 Segments (laser measurements) Rectangles Orientations – –

5 Circles Circle centers Lines Orientations –

a flexible framework that allows to replace and interchangedifferent structure detectors at a given level, or to skip levelsif more advanced detectors are created. For instance, if wewere to obtain circles directly from sensor data, they couldbe considered level 0 features and we could define a virtualpoint at level 1 to be a common center for the circles and thenimpose point to point distance constraints with the centers.This idea is general and could be applied equivalently just bytaking those features one level up if the circles were extractedfrom points. In practice this is very simple and just comesto changing a level identifier when adding the features to thegraph.

Table 1 shows different possibilities for the arrangementof the features. Depending on the characteristics of the envi-ronment and on the available input, the most suitable config-uration can be chosen. Also, several of these options may becombined. Note that the parametrization of rectangles andthe implementation of the rectangle detection algorithm forexample # 4 remain beyond the scope of this paper. Example# 5 aims at further showing the versatility of the system, butit has not been used here.

Our proposal about keeping a hierarchy of features canbe used to represent any structure, with or without having afixed prior. This representation method allows decisions tobe taken at higher levels of abstraction and later propagate toother levels below, being especially useful for dealing withincorrect constraints (see Sect. 6 for an example).

In addition to all this, this hierarchical model for the repre-sentation of the environment could be useful in other roboticdomains such as planning or human–robot interaction, withdifferent robotic capabilities gathering information from dif-ferent levels of abstraction in the graph. The presented frame-work could evolve following some ideas borrowed from otherhierarchical spatial models such as the spatial semantic hier-archy (SSH) by Kuipers (2000), the Route Graph by Krieg-Brückner et al. (2004) or the spatial–conceptual hierarchyproposed by Galindo (2005).

5 Implementation

This section provides a more detailed description of our sys-tem. First of all, the robot pose is added as the first node of

the graph. Our current implementation utilizes 2D laser datato obtain the features, but other sensors could be used as well.

5.1 Incremental graph building: robot localization,observation constraints and odometry updates

As a laser scan is acquired, we compute the point coordinatesand apply a segmentation algorithm to extract segments. Thesegments belonging to the first scan are clamped to the ori-gin. In subsequent cycles, data association with close enoughfeatures in the graph is applied in order to create an auxiliarsubgraph with the robot pose and the recognized features.This refers to distance in the graph, not metric distance (onlya small part of the graph is considered, typically up to threelevels away from the robot).

The jacobians of the constraints between the robot and theobserved segments have their components with respect to thesegments set to zero, so that the minimization of the subgraphcorrects only the robot pose (pose to segment collinearityconstraint). The observations are used as expected values andthe minimization of the subgraph is carried out as describedby Olson et al. (2006), applying the corresponding lineariza-tion to the constraints.

Newly observed features are then added to the graph,with constraints from the already corrected robot pose andfrom preexisting features previously connected to the robot(properly computing the propagation of covariances). Thisincludes constraints between pairs of features belonging tothe same scan and hence observed from the same robot pose(see Fig. 2). Only features within a given metric distanceremain linked to the robot at each iteration, so not all the fea-tures in the graph are densely connected. Ignoring the directcorrelation between features that are far away is a reasonableassumption (Liu and Thrun 2002). Even though it may resultin a suboptimal estimation, relative map consistency can bemaintained (Walter et al. 2007). In our case, the introduc-tion of the structure constraints provides geometrical consis-tency and can facilitate data association. Since the correla-tion strength is related to the distance between landmarks, weemploy a distance threshold instead of disregarding correla-tions corresponding to a high degree of uncertainty. Empiri-cally, we observed that these approximations had a negligible

123

Auton Robot

Fig. 2 Proposed approach for the marginalization of robot poses. Threeconsecutive situations. Constraints from the robot to far away featuresare removed at each iteration (out of range links)

effect, but it would be interesting to study their implicationsmore deeply.

When odometry data are received, the correspondingGaussian relative transformation is applied to the robot pose.The constraints from it are then accordingly updated, both interms of expected value and covariance. Constraints to fea-tures that come to be further than a given distance thresholdfrom the robot are deleted at this point. A satisfactory valuefor this distance threshold was found empirically (10 m forthe experiments presented here).

When the zero level features are points, a similar algorithmis employed. In this case, point to point distance constraintsare used. It is important to keep the number of distance con-straints for each point limited. Otherwise, the graph may betoo rigid and the structure constraints may then have littleinfluence. We keep constraints from one point to its five clos-est neighbours in an Euclidean sense.

More details about how the graph is built in an incrementalmanner, including the pseudocode, are given in (de la Puente2012). Some more examples and experiments are also pro-vided. It should be noted that the proposed method could alsobe tested with other graph building SLAM algorithms.

5.2 Structure detection

As mentioned earlier, we establish different levels of abstrac-tion for the features. The robot pose and the features extractedfrom sensor data (points and segments in our case) are addedas explained in the previous subsection. They constitute thezero level, with no structure constraints whatsoever. At thislevel we have point to point distance constraints, segment tosegment relative pose constraints and pose to segment con-straints (with or without clamping the pose).

To estimate the structure given a certain state of the graph,we firstly look for a set of possible geometrical entities iden-tifiable from the lowest level. If we have points at the low-est level, we seek lines and circles. In our implementationwe use Ransac (Fischler and Bolles 1981) based algorithmsto detect the lines and the circles. Nodes are expanded sothat the models are extracted out from interconnected nodes.Other detection algorithms, employing the Hough transform(Duda and Hart 1972), for instance, could also be used. Least-

squares fitting is applied to refine the parametrization of thelines and circles and to compute the residuals. Lines and cir-cles detected reliably enough are added to the graph as levelone features and the corresponding point to line or point tocircle constraints are created for the features connected tothem from the lower level.

If there are any circles, their centers are divided into clus-ters. The center of gravity of each cluster is established to bethe common center for the circles in that cluster. It is addedas a higher level point feature in the graph, with zero point topoint distance constraints with the corresponding centers thatare close enough. On this level, alignment or other relationsfor the centers could be detected and incorporated.

As for the segments and lines, we look for common ori-entations to be added at a higher level. To do so, we removeoutliers and then we obtain the main Gaussian components ofthe orientations distribution by fitting the data to a GaussianMixture Model with an a priori unknown number of compo-nents. Our approach is based on the one presented in (Ponsaand Roca 2002). Initially, one single component is consid-ered and if the data do not fit it well, then it is divided and themaximum likelihood estimation of the parameters of the newcomponents is recomputed. The normality test we employ isD’Agostino’s K 2 test (D’Agostino et al. 1990), which ana-lyzes skewness and kurtosis. We also check if a component’sstandard deviation is small enough, for we do not want verydifferent orientations to be grouped together. The main ori-entations and the corresponding segment to orientation andline to orientation relative angle constraints are added to thegraph. On this level, it is possible to detect if the relativeangles between orientations are similar to those in a prede-fined set of probable values given by the user (e.g. 0◦, 45◦,90◦, …). Alternatively, this set of values may be provided bythe user in the form of a resolution value so that an orien-tation relation may be given by any of its multiples. This isuseful for introducing orientation to orientation constraintsfor orthogonality, for instance, but also allows for more arbi-trary relations to be established. Anyway, if the method findsthese constraints to be unlikely they are removed as describedearlier.

5.3 General algorithm outline

After updating the graph when receiving sensor data (Algo-rithm 1, line 4), optimization is applied and a solution isfound. Then, unlikely constraints with high strain values andvirtual features no longer supported are removed from thegraph. From this solution, structure detectors extract virtualfeatures and constraints in a hierarchical fashion (line 11).Once structure has been added, optimization is carried outagain to find a more probable arrangement for the features.This schema is repeated iteratively until convergence. Fig. 3shows the flowchart of the whole process.

123

Auton Robot

Algorithm 1 Graph based SLAM with structure

12while (!convergence):3% add new observations or apply odometry4graph = update(graph, odometry, observations)5% CSparse optimization (Eq. 13)6graph = optimization(graph)7% remove unlikely constraints (too strained) and virtual features8graph = remove_unlikely_structure(graph, δ_threshold = 0.3)9% CSparse optimization (Eq. 13) after readjusting the structure10graph = optimization(graph)11% detect structure12for all level ε [0,num_levels−1]13%add virtual features and constraints14[f,c] = find_structure(graph,level)15graph = add_structure(f,c,graph)16% CSparse optimization (Eq. 13) after adding structure17graph = optimization(graph)

The general algorithm we propose (Algorithm 1) does notdepend on particular features, constraints, local localizationmethods or structure detectors. Our current implementationfor those components has been presented already, but it couldbe very easily reconfigured.

6 Experiments

We conducted experiments with both synthetic and real data.We present results on some complex data sets of each type.

6.1 Synthetic data experiments

We created several synthetic test cases to separately applyour approach to different geometries, using the features,

constraints and algorithms described in Sect. 5. With thegroundtruth available, it was possible to objectively evalu-ate the results in terms of accuracy for different levels ofnoise.

Introducing structure (virtual features and constraints)may significantly reduce the error in the position of featuresat the lowest level. Figure 4a shows an example of an initialconfiguration for a set of points uniformly distributed alonglines and circles but corrupted with a considerable amount ofnoise. Minimizing the graph containing only distance con-straints between connected features leads to a solution thatdoes not conform to the actual geometry very well (Fig. 4b).On the other hand, applying our EM based approach one canobtain a visually satisfactory solution, as depicted in Figs. 4cand 5, that further diminishes the error, as shown in Fig. 6for the average of 20 such experiments.

Denoting the real global position of a feature i with xri andits final estimated position with x pi , we compute its globalposition error as

e = √||xri − x pi ||2 (24)

Other metrics may be employed (Kümmerle et al. 2009).Since the proposed method does not obtain a full trajectoryestimate and the ground truth data association of the syntheticdata is very easy to obtain, a comparison based on the land-mark locations is informative for these experiments. Also,this application focuses on improving the global accuracyof the geometry of a building, so we want to penalize all thedeviations in the location of the features. Evaluating the errorof the graph constraints could be another option, yet in some

Fig. 3 Structure and mapinference with EM

123

Auton Robot

Fig. 4 Graph created with 572 point features and 1,193 point to pointdistance constraints between nearby points. The groundtruth is formedby lines and circles. a Initial configuration. Noise = 0.25 m for thepoints’ x and y coordinates, noise = 0.05 m for the expected distancevalues of the constraints (uniform distributions). b After minimizationwithout introducing structure (just considering point to point distance

constraints) the points are much better distributed, but there is stillnoticeable error. c The final solution after imposing structure constraintsis very close to the real geometry. a Graph configuration before mini-mization. b Graph configuration after minimization without introducingstructure. c Graph configuration after minimization with our approach(see also Fig. 5)

Fig. 5 Another view of our solution to the problem presented in Fig. 4.The structure was correctly detected and incorporated into the graph, asrepresented with different height levels. In the end, 28 virtual features(14 lines, 2 orientations, 8 circles and 4 common centers) were addedto the graph

cases a low relative error does not indicate convergence toa correct global solution (de la Puente 2012). A good alter-native could be to incorporate the ground truth knowledgeabout the structure of the environment into the evaluationmethod itself (Kümmerle et al. 2009). This is left for futurework.

We should remark that our current solution is in generalnot deterministic, as the structure detection algorithms for thelines and circles are based on the initial selection of randompoints. However, even if the lines and circles are not alwaysdetected in the same order or with the same points, the finalsolutions reached are usually quite similar.

Varying the noise levels had different effects. Changingthe noise in the coordinates of the initial points from 0.05mto 0.3m in 0.05m intervals scarcely affected the final error

nor the number of iterations to detect the structure. Increasingthe noise level above that value, nevertheless, often made itdifficult to find the correct structure, but this could be solvedwith some parameter tuning for the geometry detectors.

The noise level of the expected distances has more influ-ence on the results. Figure 7 compares the minimization ofthe initial graph containing only point to point distance con-straints with two different values of noise for the expected or“measured” distances. As this noise level increases, the initialminimization converges to a worse solution, from which it isharder to detect structure elements such as lines and circles.However, even if the structure cannot always be completelydetected (there might be a circle missing, for instance), theremay be important improvements with respect to that ini-tial solution due to a partial detection of correct structure(Fig. 8).

We also conducted some tests introducing a limited num-ber of random outliers. Our structure detection algorithmsare quite robust and generally produce a good output in thepresence of outliers. When not so, the δ coefficients helpedremove incorrect constraints and virtual features. Figure 9shows the error evolution average of 20 experiments with 20outliers.

In all the previous experiments the measurementes aloneprovide quite a good starting solution, with the correct topol-ogy. Applying our method in more complicated situations isexpected to have more impressive results, for example allow-ing loop closures to be correctly found because the introduc-tion of structure makes the data association process easier.

A simple test case illustrates why introducing virtual fea-tures at different levels allows us to better represent the struc-ture and hence obtain more accurate solutions, emphasizingthis advantage of the proposed hierarchical method. Besides

123

Auton Robot

Fig. 6 Evolution of the globalposition error per point featurefrom a graph like the one shownin Fig. 4a to a solution like theone shown in Fig. 4c, averagevalues for 20 experiments. Thedashed line indicates the errorvalue after minimizing the graphwith only point to point distanceconstraints (Fig. 4b). Othersignificant drops correspond tothe detection of structure

Fig. 7 Minimization of an initial graph containing only point to pointdistance constraints with different levels of noise in the measured dis-tances. Left noise = 0.01 m. Right noise = 0.1 m

providing a lot of flexibility, it is useful for the integrationand correct management of pairwise constraints. The con-clusions drawn here are extensive to other types of featuresand structure relations.

The concentricity of a group of circles could be appliedeither with constraints for the centers in pairs (Method 1),with constraints linking each of them to one of them (Method2), with all possible constraints among them (Method 3), orwith a common virtual center defined for all of them (Method4). Let us assume that we have a set of circles and one ofthem is not concentric with the rest but is mistakenly con-sidered so (see an example in Fig. 10). What we do is deletethose constraints whose δ coefficient is below a threshold,as exemplified in Fig. 11. It can be observed that the fourthapproach is the only one providing a correct solution. Withthis approach, the circle that is not really concentric with theothers can be easily detected and once its concentricity con-straint is removed the representation of the structure remainsadequate (Fig. 11d).

Figure 12 shows the evolution of the average global posi-tion error per circle for this test case, for the four approachespresented. As expected, besides allowing us to correctly rep-resent the existing structure, the establishment of a commonvirtual center outperforms the other methods regarding theaccuracy of the solution. For more details about this, see anexpanded discussion (de la Puente 2012).

In this experiment, there was enough evidence to detectthat one circle should not be concentric with the others evenif it seemed approximately so. Other cases, however, may bemore ambiguous. Let us consider a semistructured outdoorsenvironment where there are several walls that can be con-sidered parallel to each other and some rows of trees whichare not perfectly aligned. Each row of trees could have beenplanted so that the trees are alternatively placed at one side oranother of a fictitious line. If the distance from the trees to thelines is substantially larger than the error in the measurementsand this is known in advance, the distance threshold of thelines detection algorithm could be adjusted so that the rowsof trees are not considered aligned in the first place. Keepinga higher distance threshold would result in some incorrectstructure being detected, but using the δ coefficients can helpidentify when this happens.

Figure 13 shows the differences in the output when remov-ing (using the δ coefficients) or not removing incorrect con-straints. If incorrect assumptions are made and not recon-sidered, the global position error of the point features mayincrease, as shown in Fig. 14. On the other hand, using the δ

coefficients for removing incorrect constraints can result inan error reduction.

By contrast, if the data from the real lines were very noisyand the error in the not aligned points had a smaller magni-tude, telling both cases apart could be really difficult. Para-meter tuning efforts vary for intermediate situations.

123

Auton Robot

Fig. 8 Error evolution fordifferent levels of noise in theexpected distances Top noise =0.01 m. Bottom noise = 0.1 m

Fig. 9 Evolution of the globalposition error per pointfeature(settings as in Fig. 4 plus20 outliers), average values for20 experiments

123

Auton Robot

Fig. 10 Some concentriccircles (with 0.1 m of noise forthe x and y coordinates of thecenters) and one outlier (withrandom x and y coordinates inthe interval [0.2,0.5] m)depicted in black

Figure 15 shows an example of the negative consecuencesof adding incorrect structure constraints and the solutionobtained for the same data set when removing incorrect con-straints by using the δ coefficients. Applying this versionof our method produces a better solution than that obtainedwhen structure is not considered. For environments similarto this one, it would also be possible to increase the number

Fig. 11 Hierarchical representation of structure. Graph example forsix circles that should be concentric and one that should not (drawn inblack). All the circles are clamped to the origin with initial estimates

for their poses and radii. Constraints with a δ value lower than 0.3 aredepicted in black. a Method 1. b Method 2. c Method 3. d Method 4

Fig. 12 Comparison of theevolution of the global positionerror per circle for the differentstructure representation methodsanalyzed, corresponding to thetest case described in Fig. 11.We ran 20 experiments witheach method, with six circles inrandom positions around theorigin and one outlier, as shownwith an example in Fig. 10.These results correspond to themean values for each method.Method 4 outperformed theothers in all the experiments

Fig. 13 Minimization of agraph built in a semistructuredenvironment with some alignedpoints and some points that arenot fully aligned (deviation ofthe points = 0.1 m, noise in theexpected distances = 0.01 m).Left not removing incorrectconstraints. Right using the δ

coefficients for removingincorrect constraints

123

Auton Robot

Fig. 14 Evolution of the globalposition error per point featurefor the testcase shown inFig. 13, average values for 20experiments

Fig. 15 Left Wrong structuredetection, when the δ

coefficients are not used forremoving incorrect constraints(noise in the expected distances= 0.01 m). Final global positionerror per point feature = 0.10 m.Right Removing incorrectstructure constraints, using the δ

coefficients. Final globalposition error per point feature =0.035 m

Fig. 16 Killian Courtenvironment from the outside,map retrieved from GoogleMaps website http://maps.google.com/ (rotated). Left mapview. Right satellite view

of aligned points required to detect a line, in order to improvethe estimation of structure in the first place.

6.2 Real data experiments

We also tested our approach with real data from different sce-narios. The Killian Court (Fig. 16) data set is very interestingfor our experiments because it comes from quite a structuredenvironment and is a very large data set (robot path length of1450 m).

Our results were visually better than those achieved bythe standard graph based approach (Olson et al. 2006) andthose produced with our implementation of feature basedgraph SLAM without structure constraints (Fig. 17). Otherapproaches (Grisetti et al. 2007a; Olson 2009; Kuemmerleet al. 2011) obtained very good results for this data set, butwe believe our feature based framework presents impor-tant advantages for further environment interpretation. In(Grisetti et al. 2007a), a grid mapping approach is employed,and in (Olson 2009), a sophisticated place recognition strat-

123

Auton Robot

Fig. 17 Killian Court map built without including structure con-straints. The result is not consistent and the orthogonal nature of theenvironment is not well preserved

egy is adopted. The results presented in (Kuemmerle et al.2011) are excellent in a topological sense, but they show thatsome parallelism and orthogonality properties of the envi-ronment may be slightly not well respected. We propose anexplicit mechanism for enforcing geometric relations whendesired.

We applied our method as described in the previous sec-tions and the automatic detection of orientations and orienta-tion relations allowed us to properly recover the orthogonalityof the environment, as shown in Fig. 18. No loop closureswere introduced manually.

The way the map is incrementally built is a conserva-tive approximation. Even though our data association forthe robot pose correction is based on an extremely simpleand naive nearest neighbour algorithm, good results are stillachieved thanks to the incorporation of structure. Only localdata associations were allowed. If a global data associationalgorithm were employed, better results could be achieved.We think this approach could facilitate loop closure withmethods based on the current pose estimate.

Regarding execution time, our solution without introduc-ing structure constraints (Fig. 17) took 38s on a Pentium D(3.2 GHz) low end computer. Our solution with structureconstraints (Fig. 18) was obtained in 72s in total, includingall the processing time of both the front and back-ends.

Another experiment was conducted with the first part ofthe data set from the Intel Reasearch Lab in Seattle. Figure 19shows the result of applying feature based graph SLAM with-out structure detection. The map can be considered correct ata local level, but there are important deviations with respectto the groundtruth and state of the art methods. This resultcan be improved with global data association, which couldbecome easier (excluding appearance based techniques) byapplying our method with structure detection, as shown in

Fig. 18 Killian Court map built with our method. The subjective qual-ity of the map is much better due to the introduction and managementof structure constraints. Please note that global data association was notemployed

Fig. 19 Intel Research Lab map built with feature based graph SLAMwithout including structure constraints

Fig. 20. Once again, this result was obtained without employ-ing any data association algorithm at a global level. Still theresult is quite good even in the presence of slightly curvedwalls.

7 Conclusions and future work

We have presented a new approach to graph-based mapping,detecting and introducing structure constraints between fea-

123

Auton Robot

Fig. 20 Intel Research Lab map built with our method including struc-ture constraints. Even though some structure was not properly detectedthe result is still better than that obtained without considering any struc-ture (Fig. 19). Please note that this result was obtained without usingany global data association algorithm

tures in different levels in a flexible manner. Our experimen-tal results show that adding this kind of information to thegraphs can lead to more consistent and closer to the truestate maps when the data are sufficiently good and no wrongassumptions are made.

In partially structured environments, using the δ coeffi-cients for removing incorrect constraints helps avoid makingwrong assumptions. In general, our recommendation wouldbe to use as conservative as possible values for the parametersof the structure detection algorithms employed. This shouldbe studied in more detail so that more formal advice can begiven.

The presented method can provide important advantagesfor building maps which should reflect the real geometryof the environment, and not just present local topologicalconsistency. Our system can work well even in the absence ofloop closures, which often help reduce the uncertainty in themap and provide important corrections. The integration of aglobal data association algorithm should further improve theresults. It could be interesting for future research to comparethe results presented in this paper with those obtained byintegrating the proposed method with other popular SLAMsolutions for building the graph.

Furthermore, our hierarchical representation of structureallows other features and constraints to be introduced veryeasily. We are currently working on the addition of higherlevel features such as rooms and corridors. Furthermore,we believe that studying the incorporation of inequality

constraints to deal with visibility issues could be interesting,too. Also, this approach could be extended to 3D features and6D poses as well, which may originate new development andimplementation challenges and provide more benefits.

Acknowledgments The authors would like to thank Mike Bosse forcollecting and sharing the Killian Court data set. Thanks go to DieterFox for providing the Intel Research Lab data. This data set was obtainedfrom the Robotics Data Set Repository (Radish) (Howard and Roy2003). This work was supported in part by Comunidad de Madrid andEuropean Social Fund (ESF) programs and also by the Spanish Min-istry of Science and Technology, DPI2010-21247-C02-01—ARABOT(Autonomous Rational Robots).

References

Bailey, T., & Durrant-Whyte, H. (2006). Simultaneous localization andmapping (SLAM): Part II. IEEE Robotics & Automation Magazine,13, 108–117.

Beevers, K., & Huang, W. (2008). Algorithmic foundations of robot-ics VII. Chap inferring and enforcing relative constraints in SLAM.Berlin, Heidelberg:Springer.

Carlin, B., & Louis, T. (2000). Bayes and empirical Bayes methodsfor data analysis texts. Statistical science. Boca Raton: Chapman &Hall/CRC.

Carlone, L., Aragues, R., Castellanos, J., & Bona, B. (2011). A linearapproximation for graph-based simultaneous localization and map-ping. In Proceedings of Robotics: Science and Systems, Los Angeles,CA, USA.

Castellanos, J. A., Montiel, J. M. M., Neira, J., & Tardos, J. D. (1999).The SPmap: a probabilistic framework for simultaneous localizationand map building. IEEE Transactions on Robotics and Automation,15, 948–953.

Chong, K., & Kleeman, L. (1997). Sonar based map building for amobile robot. In Proceedings of the IEEE International Conferenceon Robotics and Automation (ICRA).

D’Agostino, R. B., Belanger, A., & D’Agostino, J. (1990). A suggestionfor using powerful and informative tests of normality. The AmericanStatistician, 44(4), 316–321.

Davis, T. A. (2006). Direct methods for sparse linear systems (funda-mentals of algorithms 2). Philadelphia: Society for Industrial andApplied Mathematics.

Dellaert, F. (2005). Square root SAM: Simultaneous localization andmapping via square root information smoothing. In Proceedings ofthe Robotics: Science and Systems I (RSS).

Dempster, A., Laird, N., & Rubin, D. (1977). Maximum likelihoodfrom incomplete data via the EM algorithm. Journal of the RoyalStatistical Society Series B, 39(1), 1–38.

Dolgov, D., & Thrun, S. (2008). Detection of principal directions inunknown environments for autonomous navigation. In Proceedingsof the Robotics: Science and Systems IV (RSS).

Duda, R., & Hart, P. (1972). Use of the Hough transformation to detectlines and curves in pictures. Communications of the ACM, 15, 11–15.

Durrant-Whyte, H., & Bailey, T. (2006). Simultaneous localization andmapping (SLAM): Part I. In IEEE Robotics & Automation Magazine(pp. 99–108).

Fischler, M., & Bolles, R. (1981). Random sample consensus: A para-digm for model fitting with applications to image analysis and auto-mated cartography. Communications of the ACM, 24(6), 381–395.

Folkesson, J., Jensfelt, P., & Christensen, H. I. (2007). The M-spacefeature representation for SLAM. IEEE Transactions on Robotics,23(5), 106–115.

Galindo, C., Saffiotti, A., Coradeschi, S., Buschka, P., Fernandez-Madrigal, J., & Gonzalez, J. (2005). Multi-hierarchical semantic

123

Auton Robot

maps for mobile robotics. In Proceedings of the IEEE InternationalConference on Intelligent Robots and Systems (IROS).

Gallup, D., Frahm, J. M., & Pollefeys, M. (2010). Piecewise planar andnon-planar stereo for urban scene reconstruction. In CVPR, IEEE(pp. 1418–1425).

Grisetti, G., Stachniss, C., & Burgard, W. (2007). Improved tech-niques for grid mapping with Rao-Blackwellized particle filters.IEEE Transactions on Robotics, 23(1), 34–46.

Grisetti, G., Stachniss, C., Grzonka, S., & Burgard, W. (2007). Atree parameterization for efficiently computing maximum likelihoodmaps using gradient descent. In Proceedings of Robotics, Scienceand Systems III (RSS).

Grisetti, G., Stachniss, C., & Burgard, W. (2009). Non-linear constraintnetwork optimization for efficient map learning. In IEEE Transac-tions on Intelligent Transportation Systems (p. 2009).

Grisetti, G., Kümmerle, R., Stachniss, C., & Burgard, W. (2010). A tuto-rial on graph-based SLAM. IEEE Intelligent Transportation SystemsMagazine, 2(4), 31–43.

Grisetti, G., Kümmerle, R., Stachniss, C., Frese, U., & Hertzberg, C.(2010). Hierarchical optimization on manifolds for online 2d and 3dmapping. In Proceedings of the IEEE International Conference onRobotics and Automation (ICRA).

Howard, A., & Roy, N. (2003). The robotics data set repository (radish).Retrieved February 8, 2014 from http://radish.sourceforge.net/.

Huber, P. J. (1981). Robust statistics. New York: Wiley.Kaess, M., Ranganathan, A., & Dellaert, F. (2008). iSAM: Incremen-

tal smoothing and mapping. IEEE Transactions on Robotics, 24(6),1365–1378.

Karg, M., Wurm, K., Stachniss, C., Dietmayer, K., & Burgard, W.(2010). Consistent mapping of multistory buildings by introducingglobal constraints to graph-based SLAM. In Proceedings of the IEEEInternaional Conference on Robotics and Automation (ICRA).

Krieg-Brückner, B., Frese, U., Lüttich, K., Mandel, C., Mossakowski,T., & Ross, R. J. (2004). Specification of an ontology for routegraphs. In C. Freksa, M. Knauff, B. Krieg-Brückner, B. Nebel, &T. Barkowsky (Eds.), Spatial cognition. Lecture Notes in ComputerScience (Vol. 3343, pp. 390–412). Berlin: Springer.

Kuemmerle, R., Grisetti, G., Strasdat, H., Konolige, K., & Burgard,W. (2011). g2o: A general framework for graph optimization. InProceedings of the IEEE International Conference on Robotics andAutomation (ICRA).

Kuipers, B. (2000). The spatial semantic hierarchy. Artificial Intelli-gence, 19, 191–233.

Kümmerle, R., Steder, B., Dornhege, C., Ruhnke, M., Grisetti, G.,Stachniss, C., et al. (2009). On measuring the accuracy of SLAMalgorithms. Journal of Autonomous Robots, 27, 387.

Kümmerle, R., Steder, B., Dornhege, C., Kleiner, A., Grisetti, G., &Burgard, W. (2011). Large scale graph-based SLAM using aerialimages as prior information. Journal of Autonomous Robots, 30(1),25–39. doi:10.1007/s10514-010-9204-1.

Liu, Y., & Thrun, S. (2002). Results for outdoor-SLAM using sparseextended information filters. In Proceedings of the IEEE Interna-tional Conference on Robotics and Automation (ICRA) (pp. 1227–1233).

Lu, F., & Milios, E. (1997). Globally consistent range scan alignmentfor environment mapping. Autonomous Robots, 4, 333–349.

Newcombe, R., Lovegrove, S., & Davison, A. (2011). DTAM: Densetracking and mapping in real-time. In ICCV (pp. 2320–2327).

Nguyen, V., & Harati, A. (2007). A lightweight SLAM algorithm usingorthogonal planes for indoor mobile robotics. In Proceedings of theIEEE International Conference on Intelligent Robots and Systems(IROS).

Olson, E. (2008). Robust and efficient robotic mapping. Ph. D Thesis,Massachusetts Institute of Technology, Cambridge, MA, USA.

Olson, E. (2009). Recognizing places using spectrally clustered localmatches. Robotics and Autonomous Systems, 57(12), 1157–1172.

Olson, E., Leonard, J., & Teller, S. (2006). Fast iterative optimization ofpose graphs with poor initial estimates. In Proceedings of the IEEEInternational Conference on Robotics and Automation (ICRA).

Parsley, M., & Julier, S. (2010). Towards the exploitation of prior infor-mation in SLAM. In Proceedings of the IEEE International Confer-ence on Intelligent Robots and Systems (IROS).

Parsley, M., & Julier, S. (2011). Exploiting prior information in Graph-SLAM. In Proceedings of the IEEE International Conference onRobotics and Automation (ICRA).

Ponsa, D., & Roca, X. (2002). Unsupervised parameterisation ofGaussian mixture models. In M. Escrig, F. Toledo, & E. Golobardes(Eds.), Topics in artificial intelligence. Lecture Notes in ComputerScience (Vol. 2504, pp. 388–398). Berlin, Heidelberg: Springer.

de la Puente, P. (2012). Probabilistic mapping with mobile robots instructured environments. Ph. D Thesis, Universidad Politécnica deMadrid.

de la Puente, P., Censi, A. (2012). Dense map inference with user-defined priors: from priorlets to scan eigenvariations. In C. Stachniss,K. Schill, & D. Uttal (Eds.), Proceedings of Spatial Cognition VIII.Lecture Notes in Computer Science (Vol. 7463, pp. 94–113). Berlin:Springer.

Rodríguez-Losada, D., Matía, F., Jiménez, A., & Galán, R. (2006). Con-sistency improvement for SLAM-EKF for indoor environments. InProceedings of the IEEE International Conference on Robotics andAutomation (ICRA).

Smith, R., & Cheeseman, P. (1986). On the representation and estima-tion of spatial uncertainly. International Journal Robotics Research,5(4), 56–68.

Thrun, S., & Montemerlo, M. (2006). The GraphSLAM algorithm withapplications to large-scale mapping of urban structures. InternationalJournal Robotics Research, 25, 403–429.

Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic robotics. Cam-bridge: MIT.

Trevor, A., Rogers, J., Nieto, C., & Christensen, H. (2010). Apply-ing domain knowledge to SLAM using virtual measurements. InProceedings of the IEEE International Conference on Robotics andAutomation (ICRA).

Walter, M. R., Eustice, R. M., & Leonard, J. J. (2007). Exactly sparseextended information filters for feature-based SLAM. InternationalJournal of Robotics Research, 26(4), 335–359.

123

Auton Robot

P. de la Puente obtainedher engineering degree in Auto-matic Control and Electronics inNovember 2007 and her Ph.D.in Robotics and Automation inDecember 2012, both from Uni-versidad Politecnica de Madrid(UPM). She was a postdocresearcher at DISAM-UPM andcurrently is a postdoc researcherat ACIN Institute of Automa-tion and Control-Vienna Univer-sity of Technology. Her mainresearch interests are mobilerobots navigation, robotic map-

ping and sensor data processing.

D. Rodriguez-Losada receivedthe M. Sc. degree in Mechan-ics (Industrial Engineering) andthe Ph. D. in Robotics from Uni-versidad Politecnica de Madrid,Spain in 1999 and 2004, respec-tively. He is currently a TenureProfessor at the same universityand CTO at Biicode. He has beeninvolved in many national andEuropean R&D projects in robot-ics, as well as in many indus-trial projects, as a software engi-neer and developer. His mainresearch interests are Simultane-ous Localization and Mappingand Service Robots.

123