Hierarchical Optimization on Manifolds for Online 2D and 3D Mapping

Transcription

1 Herarchcal Optmzaton on Manfolds for Onlne 2D and 3D Mappng Gorgo Grsett Raner Kümmerle Cyrll Stachnss Udo Frese Chrstoph Hertzberg Abstract In ths paper, we present a new herarchcal optmzaton soluton to the graph-based smultaneous localzaton and mappng (SLAM) problem. Durng onlne mappng, the approach corrects only the coarse structure of the scene and not the overall map. In ths way, only updates for the parts of the map that need to be consdered for makng data assocatons are carred out. The herarchcal approach provdes accurate non-lnear map estmates whle beng hghly effcent. Our error mnmzaton approach explots the manfold structure of the underlyng space. In ths way, t avods sngulartes n the state space parameterzaton. The overall approach s accurate, effcent, desgned for onlne operaton, overcomes sngulartes, provdes a herarchcal representaton, and outperforms a seres of state-of-the-art methods. I. INTRODUCTION A popular way to address the smultaneous localzaton and mappng (SLAM) problem are graph-based approaches [4, 6, 8, 9, 10, 12, 14, 18, 16]. In ths formulaton, the poses of the robot are modeled by nodes n a graph. Spatal constrants between poses that result from observatons and from odometry are encoded n the edges between the nodes. In graph-based SLAM, two problems need to be addressed: Frst, constrants need to be extracted from sensor data. Ths s referred to as the SLAM front-end. Second, gven the constrants, the most lkely confguraton as well as the pose uncertanty need to be computed. Ths s referred to as the SLAM back-end or as the optmzaton engne. Durng onlne operaton, these two problems are typcally solved n an alternatng way. Addressng the frst problem, requres solvng the data assocaton problem,.e. determnng f the current measurement covers the same part of the envronment as prevous observatons. To avod a comparson to all prevously perceved observatons, effcent SLAM systems bound the search for correspondences. One way to acheve ths s to lmt the search area based on the current uncertanty estmate of the robot. One should note that t s not requred to correct the whole graph for makng data assocatons. It s typcally suffcent to have a corrected estmate of the robot s surroundngs and the area n whch the robot mght be gven ts pose uncertanty estmate. To dentfy ths area, an accurate estmate of the coarse structure of the envronment s suffcent the whole map s not needed. The contrbuton of ths paper s two-fold: Frst, we present a novel and hghly effectve optmzaton approach based Ths work has partly been supported by the DFG under SFB/TR-8 as well as by the European Commsson under FP EUROPA and FP Frst-MM. G. Grsett, C. Stachnss, and R. Kümmerle are wth the Unversty of Freburg. U. Frese and C. Hertzberg are wth the Unversty of Bremen. Fg. 1. Ths fgure shows a 3 level herarchy constructed by our approach whle optmzng a 3D network of 2,000 nodes and 8,647 constrants. Left: the lower level representng the orgnal problem. Mddle: the ntermedate level and rght, the top level. on Gauss-Newton wth sparse Cholesky factorzaton that consders a manfold representaton of the state space. The concept of a manfold allows us to estmate 3D rotatons wth Gauss-Newton wthout runnng nto parameterzaton sngulartes, as they occur for nstance wth Euler angles. Ths leads to a more robust and hghly accurate error mnmzaton approach. Second, we present a novel SLAM back-end that ams at effcently and accurately estmatng the coarse structure of the envronment for onlne mappng. Ths s done by usng a herarchcal approach whch can be seen as lazy n each step because nstead of repeatedly optmzng all nodes of the graph, t computes a soluton to a smplfed problem. Ths smplfed problem, however, contans all relevant nformaton needed by the front-end to operate successfully and t s constructed ncrementally. Our herarchy conssts of multple, sparse pose-graphs representng the envronment, see Fgure 1 for an llustraton. The dfferent levels of the herarchy represent the orgnal problem at dfferent levels of abstracton. The bottom level corresponds to the orgnal nput, whle the hgher levels capture the structural nformaton of the envronment n a compact manner. Every tme a new observaton s obtaned, only the hghest level needs to be optmzed completely. When a hgher level of the herarchy s modfed, only the regons of the map whch are substantally changed are updated n the lower levels. In ths way, we lmt the computatonal requrements whle preservng the global consstency. It s worth mentonng that our sparsfcaton procedure s an accurate non-lnear approxmaton and accordngly, one can compute the covarances of the nodes by consderng the sparse graph only. Ths enables the front-end to operate effcently and to use popular approaches for data assocaton lke the χ 2 test or the ont compatblty test [15]. We valdate our approach on smulated and real world 2D as well as 3D datasets and provde comparsons to state-of-theart approaches ncludng TreeMap [5], TORO [8, 9], and Gauss-Newton wth sparse Cholesky factorzaton.

2 II. RELATED WORK There s a large varety of SLAM approaches avalable n the robotcs communty and we manly focus on graphbased approaches here. Lu and Mlos [14] were the frst to refne a map by globally optmzng the system of equatons to reduce the error ntroduced by constrants. Gutmann and Konolge [10] proposed an effectve way for constructng such a network and for detectng loop closures whle runnng an ncremental estmaton algorthm. Snce then, many approaches for mnmzng the error n the constrant network have been proposed. For example, Howard et al. [12] apply relaxaton to localze the robot and buld a map. Frese et al. [6] propose a varant of Gauss-Sedel relaxaton called mult-level relaxaton (MLR). It apples relaxaton at dfferent resolutons. Olson et al. [18] presented an effcent optmzaton approach whch s based on the stochastc gradent descent and can effcently correct even large posegraphs. Grsett et al. proposed an extenson of Olson s approach that uses a tree parameterzaton of the nodes n 2D and 3D. In ths way, they speed up the convergence [9]. Also the ATLAS framework [1] s related to our approach. It constructs a two-level herarchy and employs a Kalman flter to construct the bottom level. Then, a global optmzaton approach algns the local maps at the second level. Smlar to ATLAS, Estrada et al. proposed Herarchcal SLAM [3] as a technque for usng ndependent local maps. In case of place re-vstng, these maps are oned or augmented. In contrast to ther work, we use a non-lnear sparsfcaton approach whch leads to sparse representatons at dfferent levels of abstracton, all beng consstent. In addton to that, we propose a new optmzaton approach that avods sngulartes n the non-eucldean space of rotatons. Most optmzaton technques focus on computng the best map gven the constrants and are called SLAM backends. In contrast to that, SLAM front-ends seek to nterpret the sensor data to obtan the constrants that are the bass for the optmzaton approaches. Olson [17], for example, presented a front-end wth outler reecton based on spectral clusterng. For makng data assocatons n the SLAM frontends statstcal tests such as the χ 2 test or ont compatblty test [15] are often appled. The work of Nüchter et al. [16] ams at buldng an ntegrated SLAM system for 3D mappng. The man focus les on the SLAM frontend for fndng constrants. For optmzaton, a varant of the approach of Lu and Mlos [14] for 3D settngs s appled. All these front-ends also apply a global optmzaton procedure to compute a consstent map. Combnng them wth the approach presented n ths paper wll make them more effcent. III. MAP LEARNING USING POSE-GRAPHS Throughout ths paper, we consder the SLAM problem n ts graph-based formulaton. The poses of the robot are descrbed by the nodes of a graph and edges between these nodes represent spatal constrants between them. The edges are constructed from observatons or from odometry. A complete graph-based SLAM system has to address dfferent problems, namely, constructng the abstract graph representaton from the raw sensor measurements (frontend) and computng the most lkely confguraton of the poses (back-end), often ncludng uncertanty estmates. To correctly construct the graph, the front-end typcally requres a consstent estmate of the structure of the envronment together wth the expected uncertanty for data assocatons. A. The SLAM Front-end Our front-end s able to construct 2D and 3D maps, from laser data. Every tme the robot travels a mnmum dstance a new node s added to the graph. Edges connectng the current node and the prevous one are added by scanmatchng. To detect loop closures, our approach computes a quck approxmaton of the condtonal covarances of all nodes n the graph, condtoned on the current robot poston. Then, a scan-matchng procedure s appled for every node whose 3σ ellpse ntersects the current pose and a loopclosng edge s added f the matchng succeeds. To reect false closures, we use a spectral-clusterng approach whch determnes the maxmally consstent set of constrants around the current robot locaton. All n all, our approach s an own mplementaton of the front-end descrbed by Olson [17] but extended to 3D. B. The SLAM Back-end The goal of such graph-based mappng algorthms s to fnd the confguraton of the nodes that maxmzes the lkelhood of the observatons. Let x = (x 1,...,x n ) T be a vector of parameters, where x descrbes the pose of node. Let z and Ω be respectvely the mean and the nformaton matrx of an observaton of node seen from node, perturbed by Gaussan nose. Let e(x,x,z ) be a functon that computes a dfference between the expected observaton of the node x seen from the node x and the real observaton z gathered by the robot. For smplcty of notaton, we wll encode the ndces of the measurement n the ndces of the error functon e(x,x,z ) def. = e (x,x ) def. = e (x). (1) Let C be the set of pars of ndces for whch a constrant (observaton) z exsts. The goal of a maxmum lkelhood approach s to fnd the confguraton of the nodes x that mnmzes the negatve log lkelhood F(x) of all the observatons F(x) = e T Ω e (2) }{{}, C F Thus, t seeks to solve Eq. (3). x = argmnf(x). (3) x IV. POSE-GRAPH OPTIMIZATION ON A MANIFOLD Ths secton descrbes the frst contrbuton of ths paper, an accurate and effcent way of optmzng a posegraph, namely solvng Eq. (3) and estmatng the nvolved

3 uncertanty estmates. In bref, our approach apples Gauss- Newton on a manfold usng sparse Cholesky factorzaton. Consderng that the state space s not a Eucldean vector space, a manfold allows us to approprately handle the sngulartes ntroduced by the angular components. The concept of manfolds enables us to fnd a better lnearzaton of the system and thus leads to an effcent and accurate soluton. In the remander of ths secton, we frst descrbe how to compute the soluton to Eq. (3) va teratve lnearzatons. Then, we modfy ths soluton by consderng the concept of manfolds. A. Error Mnmzaton va Iteratve Local Lnearzatons If a good ntal guess x of the robot s poses s known, the numercal soluton of Eq. (3) can be obtaned by usng the popular Gauss-Newton or Levenberg-Marquardt algorthms. The dea s to approxmate the error functon by ts frst order Taylor expanson around the current ntal guess x e ( x + x, x + x ) = e ( x + x) (4) e + J x. (5) Here J s the Jacoban of e (x) computed n x and e def. = e ( x). Substtutng Eq. (5) n the error terms F of Eq. (2), we obtan: F ( x + x) (6) = e ( x + x) T Ω e ( x + x) (7) (e + J x) T Ω (e + J x) (8) = e T Ω e +2e {z } c T Ω J {z } b x + x T J T Ω J x (9) {z } H = c + 2b x + x T H x (10) Wth ths local approxmaton, we can rewrte the functon F(x) gven n Eq. (2) as X F( x + x) = F ( x + x) (11), C X, C c + 2b x + x T H x (12) = c + 2b T x + x T H x. (13) The quadratc form n Eq. (13) s obtaned from Eq. (12) by settng c = c, b = b, and H = H. It can be mnmzed n x by solvng the lnear system H x = b. (14) The matrx H s the nformaton matrx of the system and s sparse by constructon, havng non-zeros between poses connected by a constrant. Its number of non-zero blocks s twce the number of constrans plus the number of nodes. Ths allows to solve Eq. (14) by sparse Cholesky factorzaton. An hghly effcent mplementaton of sparse Cholesky factorzaton can be found n the lbrary CSparse [2]. The lnearzed soluton s then obtaned by addng to the ntal guess the computed ncrements x = x + x. (15) The popular Gauss-Newton algorthm terates the lnearzaton n Eq. (13), the soluton n Eq. (14), and the update step n Eq. (15). In every teraton, the prevous soluton s used as the lnearzaton pont and the ntal guess. The procedure descrbed above s a general approach to multvarate functon mnmzaton, here derved for the specal case of the SLAM problem. The general approach, however, assumes that the space of parameters x s Eucldean, whch s not vald for SLAM. Ths may lead to sub-optmal solutons. B. Lnearzaton on a Manfold To cope wth the fact that n SLAM the state space s not Eucldean, we propose to apply the error mnmzaton on a manfold. A manfold s a mathematcal space that s not necessarly Eucldean on a global scale, but can be seen as Eucldean on a local scale [13]. In the context of our SLAM problem, each parameter x conssts of a translaton vector t and a rotatonal component α. The translaton t clearly forms a Eucldean space. In contrast to that, the rotatonal components α span over the non-eucldean 2D or 3D rotaton group SO(2) or SO(3). To avod sngulartes, these spaces are usually descrbed n an overparameterzed way, e.g., by rotaton matrces or quaternons. Drectly applyng Eq. (15) to these overparameterzed representatons de-normalzes the angles and thus nvaldates the confguraton whch then ntroduces errors n the soluton. To overcome ths problem, one can use a mnmal representaton for the angles (lke Euler angles n 3D). Ths, however, s then subect to sngulartes. An alternatve dea s to consder the underlyng space as a manfold and to defne an operator that maps a local varaton x n the Eucldean space to a varaton on the manfold, x x x. We refer the reader to [11, 1.3] for more mathematcal detals. Wth ths operator, a new error functon can be defned as e x ( x, x ) def. = e ( x x, x x ) (16) = e ( x x) e + J x,(17) where x spans over the orgnal over-parameterzed space. In our approach, we use quaternons. The term x s a small ncrement around the orgnal poston x expressed n a mnmal representaton. Here, we use rotaton axs scaled by the rotaton angle. In more detal, we represent the ncrements x as 6D vectors x T = ( t T α T ), where t denotes the translaton and α T = ( α x α y α z ) T s the axs-angle representaton of the 3D rotaton. Conversely, x T = ( t T q T ) uses a quaternon q to encode the rotatonal part. Thus, the operator can be expressed by frst convertng α to a quaternon q and then applyng the transformaton x T = ( t T q T ) to x. In the equatons descrbng the error mnmzaton, these operatons can ncely be encapsulated by the operator. The Jacoban J can be expressed by J = e ( x x) x. (18) x=0

4 Wth a straghtforward extenson of the notaton, we can nsert Eq. (17) n Eq. (8) and Eq. (11). Ths leads to the followng ncrements: x [k] e [k] x [k] H x = b. (19) Snce the ncrements x are computed n the local Eucldean surroundngs of the ntal guess x, they need to be re-mapped nto the orgnal redundant space by the operator. Accordngly, the update rule of Eq. (15) becomes x = x x. (20) In summary, formalzng the mnmzaton problem on a manfold conssts of frst computng a set of ncrements n a local Eucldean approxmaton around the ntal guess by Eq. (19) and second accumulatng the ncrements n the global non-eucldean space by Eq. (20). V. HIERARCHICAL POSE-GRAPH The second contrbuton of ths paper s a herarchcal pose-graph. It allows us to accurately model the coarse structure of the envronment onlne. Ths nformaton s essental for makng good data assocatons n the SLAM front-end. The key dea of the herarchcal pose-graph s to represent the problem at dfferent levels of abstracton. Each level s a pose-graph and there are connectons modelng correspondences between abstracton levels. The lowest level (k = 0) represents the orgnal nput. Each node at level k > 0 represents a sub-graph at level k 1. An edge between two nodes at level k > 0 models the constrans between the sub-graphs and can be computed analytcally as wll be explaned below. It s obvous that the hgher the level of abstracton, the lower the number of parameters to descrbe the envronment, and thus the lower the qualty of the representaton at that level but the faster the optmzaton. More formally, we represent the problem usng a herarchy of K graphs. Let G [k] be the graph at level k. The graph G [k] conssts of a set of nodes {x [k] } and a set of edges {e [k] }. Each node x [k] at level k s assocated to () a representatve node x [k 1] at level k 1 and () a connected sub-graph G [k 1] at level k 1. An edge e [k] between the nodes x[k] and x [k] at level k > 0 exsts f the two sub-graphs G [k 1] and G [k 1] are connected. Fgure 2 llustrates a smple two layered graph structure. The dea s to construct a hgh level graph by parttonng the lower level n local maps, represented by the sub-graphs }. Each local map s then represented by a node at the hgher level. Edges between nodes at the hgh level encode the relatons between local maps arsng from the connectvty between neghborng local maps. {G [k 1] A. Constructon of the Herarchy To buld the graph G [k] at level k from the graph G [k 1] at level k 1, t s suffcent to defne groups of connected nodes n G [k 1]. In our current mplementaton, we group the nodes based on a straght-forward threshold crteron that G [k 1] x [k 1] G [k 1] x [k 1] Fg. 2. Smple two layered graph structure. Every node x [k] n the hgher level corresponds to a connected sub-graph G [k 1] at lower level and to a node x [k 1] wthn the sub-graph. An edge e [k] exsts f two sub-graphs at low level are connected. consders the dstance on the graph. Note that ths worked well n all our settngs, but may offer room for further mprovements. Let these groups be {G [k 1] }. For each group G [k 1], we choose a representatve node x [k 1] for G [k 1]. Ths representatve becomes the node x [k] at level k. To obtan a consstent herarchy, we have to add an edge e [k] between x[k] and x [k] at level k f the correspondng sub-graphs are connected. Ths edge has to capture the nformaton encoded n all edges of G [k 1] and G [k 1] as well as all edges connectng both. Accordngly, we need to compute a mean z [k] and nformaton matrx Ω [k] for the edge e[k]. The parameters depend only on the confguraton of the sub-graphs G [k 1] and are computed as follows. and G [k 1] Let G [k 1] be the unon of the graphs G [k 1] and G [k 1] and ther nterconnectng edges. We can obtan the relatve poston of x [k 1] z [k] orgn. of e[k] wth respect to x [k 1] by optmzng G[k 1] and thus the mean, whle forcng x [k 1] to the Let H [k 1] be the nformaton matrx of G [k 1] computed accordng to Secton IV durng the optmzaton of ths subgraph. Snce x [k 1] of the node x [k 1] and can be obtaned by extractng the correspondng block Σ [k] les n the orgn, the covarance matrx s equal to the one of the edge e [k] of (H [k 1] ) 1. Snce H [k 1] s sparse by constructon, ths procedure can be carred out effcently. Gven the covarance of the edge, the nformaton matrx s obtaned drectly by Ω [k] = (Σ[k] ) 1. B. Extendng the Herarchcal Pose-Graph As the robot moves through the envronment, nformaton has to be added to the herarchcal pose-graph. Ths s done by addng a node and correspondng edges to the bottom level of the herarchy as done n standard graph-based SLAM. Accordng to a dstance-based threshold crteron, the newly created node s ether added to an exstng group or t becomes the representatve of a new one at level 0. Ths procedure t recursvely executed upwards the herarchy untl no new groups need to be created. Edges are added and ts parameters are updated accordngly.

5 C. Herarchcal Graph Optmzaton After an update of the herarchcal pose-graph, an optmzaton s carred out. The optmzaton always starts at the top level usng the manfold optmzaton approach presented n Secton IV. As a result, all nodes at the hghest level are updated. However, changes are only propagated to lower levels f the optmzaton leads to sgnfcant changes n the node confguratons. These changes are detected by montorng the dfference between each node x [k] and ts representatve x [k 1] at level k 1. Whenever the dstance between x [k] and x [k 1] exceeds a gven threshold (n our current mplementaton: 0.05 m or 2 deg), we propagate the changes downwards. Ths s acheved by applyng a rgd body transformaton to each subgraph G [k 1] so that x [k] = x [k 1]. When requred by the front-end, we generate a locally consstent estmate of a porton of the map by optmzng the correspondng sub-graph at low level. Durng the optmzaton, we mpose the addtonal constrants x [k 1] = x [k]. In ths way, we account for the estmate at the hgher level n the lower level. After the mappng process one may consder to run a last optmzaton at level k = 0 to obtan the best possble map. It should be noted that f one focuses only on offlne mappng wth gven data assocaton and startng from a good ntal guess, the herarchy s not needed snce optmzng the orgnal nput wll provde the desred soluton. VI. EXPERIMENTS The experments are desgned to show 1) that the manfold optmzaton approach s able to fnd a better confguraton of the nodes compared to all other technques evaluated here. 2) that the herarchcal pose-graph s able to effcently compute consstent estmates (mean and covarance) at all levels. 3) that our approach outperforms the other state-of-the-art technques n terms of run-tme and can operate onlne. To support our clams, we compared our approach to Gauss- Newton wth sparse Cholesky factorzaton wthout manfolds (Secton IV-A), TreeMap [5], and TORO wth ts ncremental [8] and batch [9] verson. We performed our test on 2D datasets (Intel research lab dataset and a smulated one) and 3D datasets (Stanford parkng garage and a smulated sphere), see Fgure 3. Both smulated datasets are the ones used n [9]. Durng all experments, we use a three level herarchy (k = 0,1,2). We provde an open-source mplementaton of our approach called HOG-Man wrtten n C++ whch s avalable onlne [7]. A. Manfold Optmzaton In ths frst experment, we evaluated how the optmzaton approach that consders the manfold mproves the performance. Here, the complete set of constrants s provded to the optmzer and we measured the χ 2 error vs. runtme. For ths offlne batch comparson, we used the smulated 3D dataset wth a sgnfcant ntal error and compared Fg. 3. The four datasets used n our expermental evaluaton. Top row: pose-graph and grd map of the Intel research lab, mddle row: pose-graph and 3D map of the Stanford parkng garage, bottom left: smulated 2D dataset (W-10000), bottom rght: smulated 3D dataset (sphere). χ 2 error Gauss-Newton (Euler) TORO Gauss-Newton (Manfold) runtme [s] Fg. 4. Evoluton of the χ 2 error for TORO, Gauss-Newton usng Euler angles, and Gauss-Newton wth manfold on the 3D Sphere dataset. the results of Gauss-Newton wth and wthout the manfold lnearzaton,.e. here by usng Euler angles. We furthermore provde a comparson to TORO [9]. Fgure 4 depcts the results. As can be seen, not consderng the sngulartes approprately can lead to sgnfcant errors. Also TORO quckly converges to a vsually good-lookng soluton but stll leaves space for mprovements. Even after performng a large number of teratons, the remanng χ 2 error of TORO s sgnfcantly larger than the one of our new approach. Ths caused by the fact that TORO s an approxmatve approach that assumes roughly sphercal covarances and optmzes translatons and rotatons separately. B. Consstency of the Herarchcal Approach The second experment s desgned to show that the sparsfed pose-graphs (levels greater than 0) represents a good

6 TABLE I COMPARISON OF THE 3σ COVARIANCE ELLIPSES BETWEEN THE ORIGINAL PROBLEM AND THE LEVELS OF OUR HIERARCHY. Prob. mass not covered Prob. mass outsde Intel 0.10% 10.18% W % 24.05% Stanford 0.01% 7.88% Sphere 2.75% 10.21% TABLE II RUNTIME COMPARISON FOR THE DIFFERENT APPROACHES. avg./std./max.[ms] Our Approach TreeMap [5] TORO [9] Intel 4 / 5 / 31 6 / 5 / 58 3 / 2 / 33 W / 20 / / 1342 / / 97 / 323 Stanford 25 / 26 / 189 2D pose graphs only 35 / 85 / 602 Sphere 89 / 42 / 213 2D pose graphs only 226 / 606 / 4615 approxmaton of the orgnal problem. Ths s especally mportant for the SLAM front-end for effcently makng good data assocatons. Therefore, all experments n the remander of ths secton, are carred out onlne. Ths means that every tme a new node s added to the graph, the optmzaton s carred out (ncrementally). The baselne of the comparson s thus the orgnal problem, fully optmzed wthout the herarchcal approach. To evaluate the qualty of the most sparsfed pose-graph (top level), we compare the Gaussan assocated to each node of these graphs wth the correspondng dstrbuton of the orgnal problem. We compute the probablty mass wthn the 3σ bounds of the orgnal problem that les outsde the same bound of the sparsfed graph and vce versa. Table I llustrates the results based on all data sets. As can be seen, the herarchy approxmates the orgnal problem well. Especally, the probablty mass that s not covered by the sparse pose-graphs (over-confdent estmates) s around or below 0.1% for all real world datasets. In general, the uncertanty ellpses of the sparse graphs are typcally bgger than the ones n the orgnal problem (around 10% for the real world datasets). C. Runtme Comparson In the fnal experment, we analyzed the runtme requred by the dfferent approaches to optmze the pose-graph. The results depcted n Table II show the average, the standard devaton, and the maxmum runtme tme of the optmzaton engne whch was always executed after addng a new node. The tmngs are provded for all datasets analyzed n ths paper. The experment has been executed n a Core2Duo processor wth a 2.4 GHz processor (sngle-thread). As can be seen, our approach clearly outperforms TreeMap. Detaled nvestgaton of TreeMap s tree data structure showed that heavy leaves n the tree,.e. leaves wth many poses, led to the poor performance. Ths s caused by revsted places leadng to a fully connected clque of poses. Even worse, TreeMap combnes several successve poses nto one leaf durng the frst vst and has to add a duplcate pose to everyone of these after each revst. Our method furthermore outperforms TORO. In the comparably densely connected smulated pose-graphs, ths effect was more promnent compared to the real world datasets. VII. CONCLUSION In ths paper, we present a novel SLAM back-end for the graph-based SLAM systems. Its contrbuton s twofold: Frst, an effcent optmzaton approach that takes the sngulartes of the angular components nto account by consderng a manfold when optmzng the pose-graph. Ths leads to a hghly effcent and effectve error mnmzaton approach. Second, a herarchcal pose-graph that s able to model the problem at dfferent levels of abstracton whch can be optmzed fast whle provdng support for makng data assocatons. The overall approach s accurate, effcent, desgned for onlne operaton, overcomes sngulartes, provdes a herarchcal representaton, and outperforms a seres of state-of-the-art methods. REFERENCES [1] M. Bosse, P. M. Newman, J. J. Leonard, and S. Teller. An ATLAS framework for scalable mappng. In Proc. of the IEEE Int. Conf. on Robotcs & Automaton (ICRA), pages , [2] T. A. Davs. Drect Methods for Sparse Lnear Systems. SIAM Seres on the Fundamentals of Algorthms. SIAM, Phladelpha, [3] C. Estrada, J. Nera, and J.D. Tardós. Herachcal SLAM: Realtme accurate mappng of large envronments. IEEE Transactons on Robotcs, 21(4): , [4] J. Folkesson and H. Chrstensen. Graphcal SLAM - a self-correctng map. In Proc. of the IEEE Int. Conf. on Robotcs & Automaton (ICRA), Orlando, FL, USA, [5] U. Frese. Treemap: An O(log n) algorthm for ndoor smultaneous localzaton and mappng. Autonomous Robots, 21(2): , [6] U. Frese, P. Larsson, and T. Duckett. A multlevel relaxaton algorthm for smultaneous localsaton and mappng. IEEE Transactons on Robotcs, 21(2):1 12, [7] G. Grsett, R. Kümmerle, and C. Stachnss. The source code [8] G. Grsett, D. Lod Rzzn, C. Stachnss, E. Olson, and W. Burgard. Onlne constrant network optmzaton for effcent maxmum lkelhood map learnng. In Proc. of the IEEE Int. Conf. on Robotcs & Automaton (ICRA), Pasadena, CA, USA, [9] G. Grsett, C. Stachnss, and W. Burgard. Non-lnear constrant network optmzaton for effcent map learnng. IEEE Transactons on Intellgent Transportaton Systems, In press. [10] J.-S. Gutmann and K. Konolge. Incremental mappng of large cyclc envronments. In Proc. of the IEEE Int. Symposum on Computatonal Intellgence n Robotcs and Automaton (CIRA), [11] C. Hertzberg. A framework for sparse, non-lnear least squares problems on manfolds. Master s thess, Unv. of Bremen, [12] A. Howard, M.J. Matarć, and G. Sukhatme. Relaxaton on a mesh: a formalsm for generalzed localzaton. In Proc. of the IEEE/RSJ Int. Conf. on Intellgent Robots and Systems (IROS), [13] J.M. Lee. Introducton to Smooth Manfolds, volume 218 of Graduate Texts n Mathematcs. Sprnger Verlag, [14] F. Lu and E. Mlos. Globally consstent range scan algnment for envronment mappng. Autonomous Robots, 4: , [15] J. Nera and J.D. Tardós. Data assocaton n stochastc mappng usng the ont compatblty test. IEEE Transactons on Robotcs and Automaton, 17(6): , [16] A. Nüchter, K. Lngemann, J. Hertzberg, and H. Surmann. 6d SLAM wth approxmate data assocaton. In Proc. of the Int. Conference on Advanced Robotcs (ICAR), pages , [17] E. Olson. Robust and Effcent Robotc Mappng. PhD thess, MIT, Cambrdge, MA, USA, June [18] E. Olson, J. Leonard, and S. Teller. Fast teratve optmzaton of pose graphs wth poor ntal estmates. In Proc. of the IEEE Int. Conf. on Robotcs & Automaton (ICRA), pages , 2006.