Robotics and Autonomous Systems. Large scale multiple robot visual mapping with heterogeneous landmarks in semi-structured terrain

Transcription

1 Robotcs and Autonomous Systems 59 () Contents lsts avalable at ScenceDrect Robotcs and Autonomous Systems journal homepage: Large scale multple robot vsual mappng wth heterogeneous landmarks n sem-structured terran Teresa A. Vdal-Calleja a,, Cyrlle Berger b,c, Joan Solà b,c, Smon Lacrox b,c a Australan Centre for Feld Robotcs, Unversty of Sydney, NSW 6, Australa b CNRS; LAAS; 7 avenue du colonel Roche, F-377 Toulouse, France c Unversté de Toulouse; UPS, INSA, INP, ISAE; LAAS; F-377 Toulouse, France artcle nfo abstract Artcle hstory: Receved 7 August Receved n revsed form May Accepted 4 May Avalable onlne June Keywords: Mult-robots cooperaton Vsual SLAM Ths paper addresses the cooperatve localzaton and vsual mappng problem wth multple heterogeneous robots. The approach s desgned to deal wth the challengng large sem-structured outdoors envronments n whch aeral/ground ensembles are to evolve. We propose the use of heterogeneous vsual landmarks, ponts and lne segments, to acheve effectve cooperaton n such envronments. A large-scale SLAM algorthm s generalzed to handle multple robots, n whch a global graph mantans the relatve relatonshps between a seres of local sub-maps bult by the dfferent robots. The key ssue when dealng wth multple robots s to fnd the lnk between them, and to ntegrate these relatons to mantan the overall geometrc consstency; the events that ntroduce these lnks on the global graph are descrbed n detal. Monocular cameras are consdered as the prmary extereoceptve sensor. In order to acheve the undelayed ntalzaton requred by the bearng-only observatons, the well-known nverse-depth parametrzaton s adopted to estmate 3D ponts. Smlarly, to estmate 3D lne segments, we present a novel parametrzaton based on anchored Plücker coordnates, to whch extensble endponts are added. Extensve smulatons show the proposed developments, and the overall approach s llustrated usng real-data taken wth a helcopter and a ground rover. Elsever B.V. All rghts reserved.. Introducton There are several cooperaton schemes n whch the complementarty of aeral and ground robots can be exploted to enhance the effcency of autonomous robotcs operatons. For nstance, aeral robots can provde the ground robots wth nformaton related to the envronment, e.g. traversablty maps or landmarks, they can localze the ground robots by percevng them, or provde them wth communcaton lnks wth a remote staton. Aeral and ground robots can also cooperate n a more symmetrc way to acheve a task, such as exploraton, survellance or target detecton and trackng. All these tasks can beneft from the enhanced observaton capabltes brought by both knds of robots. In ths context, the ablty to buld and share envronment models among the robots s an essental prerequste to the development of cooperaton schemes. Be t for exploraton, survellance or nterventon mssons, envronment models are ndeed necessary to plan and coordnate paths, but also to determne the utlty of vantage ponts, to assess whether robots wll be able to communcate or not, and to localze the robots n a common frame. In partcular, 3D nformaton on the Correspondng author. Tel.: ; fax: E-mal addresses: t.vdal@acfr.usyd.edu.au (T.A. Vdal-Calleja), cberger@laas.fr (C. Berger), jsola@laas.fr (J. Solà), Smon.Lacrox@laas.fr (S. Lacrox). envronment s requred. Not only do the robots evolve n the three dmensons, but the determnaton of vantage ponts calls for vsblty computatons n the 3D space. Also, vson s consdered the prmary sensor to buld envronment representatons. Besdes the fact that mages carry a lot of nformaton on the envronment, vson s passve, t has the man advantage of percevng features that are arbtrarly far away, and t can be embedded onboard any knd of drone, even the smallest ones. Smlarly to the sngle robot case, one can not take for granted the fact that the robots are at all tmes perfectly localzed. Centmeter accuracy RTK-GPS s often dsturbed by sgnal outages, and no onboard sensors can ensure a robust long term localzaton. Nevertheless, the maps must be spatally consstent; ther spatal uncertantes must be compatble wth the actual envronment s spatal characterstcs, and the process of mappng must tolerate the fact that the robots postons are not always perfectly known. Ths naturally leads to explotng a SLAM approach, n whch uncertantes on the envronment features and on the robots poses are explctly managed over tme. Mappng n a mult-robot system brngs forth the followng addtonal ssues, that have to be tackled n our aeral/ground context: Some applcatons contexts even preclude ts usage /$ see front matter Elsever B.V. All rghts reserved. do:.6/j.robot..5.8

2 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () Fg.. Three aeral mages of a vllage acqured at about 4 m alttude (top) and three mages of the same area acqured by a ground rover. The red angular sector n the aeral mages approxmately represents the poston and feld of vew of the ground rover camera. How could these data be regstered and fused n a spatally consstent data structure? (For nterpretaton of the references to colour n ths fgure legend, the reader s referred to the web verson of ths artcle.) The mappng algorthms must be dstrbuted among the robots. They can not rely on a complete and permanent communcaton nfrastructure between the robots, as ths would mpose strong requrements on communcaton coverage and bandwdth, whch can hardly be satsfed n most feld applcatons. The structure of the maps bult by the varous robots must allow to match, regster or fuse them, so that they can be shared among the robots. For these purposes, the nformaton on the envronment they exhbt must be common to the varous robots. In our context the 3D geometry of the envronment s the only ntrnsc envronment characterstc on whch one can rely to tackle data assocaton, as t can cope wth the fact that the sensors or vewponts can be very dfferent among the dfferent robots (see example data n Fg. ). Approach. To tackle these two ssues, we am at defnng a 3D multple robot mappng framework usng vson, that s able to handle any localzaton means, ncludng matchng perceved data wth pre-exstng 3D maps. The produced envronment representaton must also allow the ncorporaton of range data such as those provded by stereo vson or Ldars, these sensors beng wdely used on ground robots. For these purposes, we propose to use 3D ponts and 3D lne segments to represent the envronment from sequences of aeral and ground mages. These representatons should gather most of the geometrc structure n the envronment, and s the bass to regster and fuse the data acqured from dfferent vantage ponts, dfferent sensors, and pror 3D nformaton on the envronment. In the drecton towards the achevement of such a mappng framework, the contrbuton of ths paper s twofold: We propose a mappng approach n whch robots buld seres of local maps usng a classcal EKF-based SLAM paradgm. The overall spatal consstency of the maps among the robots s ensured by an optmzaton process, that takes nto account varous nter-robot and absolute localzaton estmates. Ths approach takes after the work on herarchcal SLAM proposed by Estrada et al. n [], whch reles on a herarchcal representaton of the map; the global level s an adjacency graph, where nodes are local maps (or sub-maps ), and edges are relatve locatons between local maps. In a mult-robot context, the sub-maps are not necessarly bult n a sequental manner, and varous events can trgger loop closures and later map mergng, namely rendezvous between robots, feature correspondences ( matchng ) and absolute localzatons, provded ether by GPS fxes or by matches wth an a pror exstng map []. These events produce loop closures; the global graph exhbts a cycle, and thus defne constrants that allow the system to refne the estmates of the sub-maps orgns. We analyze the mpact of these events on the overall map structure and propose a way to handle them n a dstrbuted context. We explot 3D ponts and 3D lne segments detected n the mages n order to create representaton of the envronment that s nvarant to the vantage ponts. Ths envronment representaton s bult usng bearng-only nformaton. Bearng-only nformaton calls for a dedcated mplementaton of the EKF-based SLAM approach, whch reles on the undelayed ntalzaton. We use the well-known nverse-depth parametrzaton (IDP) for 3D ponts and we propose a new parametrzaton based on anchored Plücker 3D lnes. Plücker coordnates are well known n computer vson for ther nce projectve propertes, smlar to what happens wth homogeneous ponts. To mprove ther behavor wthn EKF-SLAM, we add an anchor to the parametrzaton as t s done n IDP. Outlne. Secton deals wth the overall localzaton and mappng approach to deal wth multple robots. It presents the prncple of herarchcal SLAM by means of a probablstc graph model, and ts extenson n the mult-robot case, analyzng n partcular the varous loop closures that can occur between the robots. It also dscusses how the mappng process could be dstrbuted among the robots, and smulaton results are depcted. Secton 3 s devoted to the lne segments detected n monocular magery. It ntroduces the way such segments can be used as landmarks n a monocular EKF SLAM approach; the anchored Plücker lne parametrzaton s used, the ntalzaton and update processes are depcted, the map-matchng wth lnes s presented, and smulaton results are analyzed. Fnally, Secton 4 presents results obtaned wth data gathered by a helcopter and a ground rover n a vllage-lke envronment, and a dscusson concludes the paper.

3 656 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () Managng multple maps among multple robots.. Related work The mult-robot SLAM problem has recently ganed a lot of attenton. Several approaches have been presented to solve ths problem, based on extended Kalman flters [3 5], nformaton flters [6,7], maxmum lkelhood estmators [8,9] and partcle flters [,]. Early work on cooperatve localzaton and mappng extended the well-studed sngle robot SLAM problem to multple robots. Ths extenson s straghtforward when the robots start from a known locaton and the nformaton s centralzed. In [8] a maxmum lkelhood algorthm s appled to fnd the maps that are maxmally consstent wth sensor data and odometry. They assume the robots start from a known locaton and a central mapper mproves the map by teratvely combnng data from the robots. Ref. [] also assumes known locaton to fuse maps from dfferent robots combnng a maxmum lkelhood wth an a-posteror pose estmator. The robots poses are propagated usng a partcle flter representaton of ther belef functons, whle the map s compled n a dstrbuted manner among robots usng laser range data. In [3], the authors do not address mplementaton ssues but the theoretcally attanable estmaton accuracy, by provdng bounds for the steady state covarance of the poston estmates. Further developments have been acheved for the case when the robots do not have pror knowledge about ther relatve locatons. In [6] a sparse extended nformaton flter s presented wth no knowledge of ntal locatons. The algnment of local maps nto a sngle global map s acheved by a tree-based algorthm to search for smlar-lookng local landmark confguratons, pared wth a hll clmbng algorthm that maxmzes the overall lkelhood by searchng n the space of correspondences. Ref. [] deals wth completely unknown startng locatons by usng an adapted partcle flter n combnaton wth a predctve model of ndoor envronments n order to sequentally determne whether and how the partal maps of two robots overlap. The authors n [4] proposed an algorthm to algn maps bult by dfferent vehcles that do not necessarly overlap. They used the nformaton provded by the robot-to-robot measurements (rendezvous) to obtan the transformaton between ther coordnate frames. They used an Extended Kalman Flter (EKF) to estmate the robots and landmarks postons. For maps that do overlap, [5] presents an ncremental mnmum cycle bass algorthm that combned wth herarchcal SLAM [] manages the global level after a map mergng s performed. The work of [3] uses a maxmum lkelhood approach for mergng maps wth ntal unknown correspondences wth manfold operatons and scan matchng. Ths work subdvdes the mult-robot problem nto three subproblems; ncremental localzaton and mappng, loop closure and sland mergng. Ths latter work s related to our work n the sense that we also consder ths subdvson, although our approach reles on events such as rendezvous, landmark correspondences or GPS fxes to frst trgger a loop closure n global level and at the end f requred do the map mergng. The theoretcal soluton for the centralzed mult-robot SLAM algorthm s acheved by appendng robots together wth landmarks to the same state-space, usng the same flter. The practcal soluton requres a central staton where all the nformaton s concentrated [8,]. Some efforts have been made n terms of decentralzaton, where robots broadcast the gathered nformaton and, ndependently, each robot computes ts own pose and full map. A decentralzed SLAM approach was proposed by [7] usng channel flters. In order to reduce the communcaton bandwdth the authors proposed the use of the covarance ntersecton algorthm. An nterestng dstrbuted SLAM approach s presented n [4], where communcaton s lmted to an upper trapezodal matrx whch condenses the entre measurement hstory on the ndvdual robots. More feasble algorthms can acheve dstrbuted mappng usng cooperatve localzaton [5,6]. Dstrbuted mappng s accomplshed by the fact that multple robots buld ther own maps. Our soluton s related wth cooperatve localzaton n the sense that robots only communcate some past robot poses (orgns of local maps) to each other. Range and bearng observaton characterzed most of the approaches n the lterature for mult-robot SLAM [8,,,7]. The work n [8] s one of the very few works on mult-robot SLAM usng vson. The mult-robot terran mappng algorthm uses localzaton nformaton to combne vson-based range estmates wth an elevaton profle across the terran. Also, n [] the proposed approach consders the use of a stereo vson system, where agan range and bearng observatons are acqured. It reles on a fully centralzed Rao Blackwellzed partcle flter, and only presents smulaton results... Herarchcal SLAM Dfferent scalable SLAM approaches n whch a sngle vehcle bulds multple local maps have been proposed, manly to reduce computatonal complexty and to delay lnearzaton errors untl the map mergng [9,,,,]. When the maps are merged nto a sngle one, on the bass of ether common landmarks between local maps, or smply the sequental constrant, a fully correlated map of the envronment s obtaned. Successful fast mplementatons explotng the topology of the representaton to systematcally jon and fuse local maps have been proposed for the sngle vehcle case, such as trees [3], or bnary trees []. We formulate the multple robot localzaton and mappng problem usng sub-maps, n a smlar manner to herarchcal SLAM n [] or hybrd metrc-topologcal SLAM n [], where there are two levels; the local level (sub-maps), and global level (adjacency graph). We make use of the probablstc graphcal models representaton to descrbe our approach. The global level represents the relatonshps s j between local maps and j. The local level contans the sub-maps, composed of the set of landmarks m and the current robot pose x k (at nstant k). At a certan pont a new local map s generated wth the robot pose actng as the new local reference frame (lrf ). Thus the robot pose x truly represents the relaton between the prevous map and the new one, and one can set s = + x. Other non-correlatve relatons s j may be establshed between maps as we wll see. Based on smple frame compostons, nformaton n the world reference frame (wrf ) s also avalable for the orgns S of each map, and for the map tself (X, M) f t s requred. The Bayesan network n Fg. shows the representaton of ths herarchcal/hybrd SLAM. Local level. The local level contans the feature-based locally referred stochastc maps, bult wth the standard EKF-SLAM. The -th local map s defned by x = x m, () m where x s the current pose of the robot, and m = [l...l m ] s the set of m mapped landmarks, both wth respect to the -th lrf. EKF-SLAM keeps a Gaussan estmate x m N {ˆx, m P m } of ths map, namely ˆx = ˆx m, ˆm P = Px x P x m m. () (P x m ) P m m The maps are bult sequentally as mentoned above. Once a threshold s passed, ether n number of landmarks or n robot s

4 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () Fg.. Graphcal model as a smplfed Bayesan network that represents the multple local sub-maps. Sub-maps consttute the local level (x, m ) and the global level on top lnks the local sub-maps (S ). In ths representaton there s no nformaton shared between local sub-maps. The map buldng here s sequental, correspondng to one robot explorng and not closng loops, havng always s = + x. Loop closures wll establsh non-sequental lnks s j that are not present n ths fgure (see Fgs. 5 7 for examples). uncertanty, a new map s created. No nformaton s shared between these sub-maps, thus the new map starts n a lrf wth the robot s pose and error covarances equal to zero. Each local map stores nformaton n ts own lrf. We assume the control nput of the robot and landmark s observaton have been already ntroduced to obtan the local submaps. The jont dstrbuton resultng from the local map SLAM process s p(x, m ). Thus the m condtonal probablty, as shown n Fg., s gven by, p(m x ) = p(x, m ) p(m ) = N ( ˆm, ˆP x ). (3) Global level. The global level s represented as an adjacency graph n whch orgns of local maps S n wrf are nodes, and the lnks between them are the relatve transformatons s +. Let us defne the global level as the Gaussan state s N {ŝ; P s } of relatve transformatons between sequentally bult local maps, namely ŝ P s ŝ =..., P s =.. (4) P s + ŝ + The global orgns of the maps n the wrf are computed as the compoundng of the prevous global orgn wth the relatve transformaton between sub-maps, S + = S s + (a detaled descrpton of the compoundng and nverson operatons for D and 3D can be found n [4]). The current poston of the robot n wrf s computed as X k = S x k. Also, the global map can be obtaned through, M = S m. (5) Mean and covarances of the Gaussan estmates are obtaned by regular lnear-gaussan propagaton usng the Jacobans of and. The condtonal probablty dstrbuton of S, as shown n Fg., s gven by, p(s S, s ) = p(s, S s p(s s ). (6) ) Consderng the relatve transformatons between local maps as past robot poses, we note that the global level can be vewed as a sparse delayed-state pose-slam [5], where local maps are lke landmarks hangng from robot poses n wrf as shown n Fg.. The man dfference s due to the fact that the state-space n our case contans relatve poses s, nstead of absolute poses S (that wll be naturally correlated) as n the pose-slam case when the map features are margnalzed out. Loop closure. At the global level, a loop closure corresponds to a cycle n the graph, that appears for nstance when a relatve poston estmate between non-consecutve sub-maps s Fg. 3. Bayesan network of the herarchcal SLAM for two robots. Two dsconnected sub-graphs, one for each robot. establshed by a map matchng process. Such a cycle defnes a constrant between a seres of relatve transformatons: h(s) = s s s = (7) = S s =. (8) Gven that h(s) s not lnear due to the angular terms, the enforcement of ths constrant can be formulated as a nonlnear constraned optmzaton problem such that, mn s F(s) = mn(s ŝ) P s (s ŝ) (9) s h(s) =. () For multple l loops h(s) s formed by all the constrants h(s) = [h (s)...h l (s)]. The maxmum a posteror lkelhood soluton for nstance, can be based on the Iteratve EKF as presented n []. As a consequence, the part of the state nvolved n the loop closure at global level becomes correlated, resultng n a nonsparse covarance matrx P s..3. Multple robots A herarchcal/hybrd SLAM approach n the mult-robot case s qute straghtforward; each robot manages a set of sub-maps and a global graph of poses. Ths approach lends tself ncely to the case of multple robots as shown n the Bayesan network of Fg. 3. But the nterests of mult-robot mappng arse of course when the robots exchange mappng or poston nformaton, whch permts the enhancement of the spatal consstency and the constructon of a mult-robot global graph of map poses. Indeed, when robots meet or communcate, they can estmate ther relatve poston or match maps that overlap; these knds of events generate connectons between the ndvdual global graph. The possble events are the followng: robots rendezvous (Fg. 4(a)), match common nformaton wthn sub-maps (Fg. 4(b)), recevng external nformaton that provde absolute localzatons (e.g. a GPS fx (Fg. 4(c)), or feature matches wth an exstng envronment model). The latter s not exactly a drect mult-robot loop closure, but t provdes a lnk between a lrf and a global geo-referenced frame, whch n turn establshes a lnk wth any other robot that has already been absolutely localzed once. Whereas n a sngle robot case a loop closure only occurs when the robot revsts and matches a prevously mapped place, n a mult-robot case these events trgger loop closures; any cycle that appears n the overall graph defned by the concatenaton of each robot graph (the mult-robot graph) s a loop closure. The compoundng of all relatve transformatons that defnes a cycle s equal to zero as n (7), and a nonlnear optmzaton over the transformatons can be performed. Note that to obtan a cycle n the graph defned by the concatenaton of two robots global levels, at least two events between these robots are requred. Robot rendezvous. The event occurs when a robot observes another robot (partal rendezvous) or when both robots observe each

5 658 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () (a) Rendezvous event. (b) Matchng nformaton event. (c) GPS fx. Fg. 4. Loop-closng events for multple robots. Fg. 5. Bayesan network of rendezvous event. The observaton z s the relatve transformaton between the two nodes. other (full rendezvous). We focus on the case when the relatve transformaton between two robots s fully recovered, from the nformaton obtaned through a partal or full rendezvous. That s, whenever ths event occurs, t leads to an mmedate observaton of the relatve transformaton between locatons of the two vehcles. New local maps are created at the nstant of the rendezvous, then the current robot poses are promoted to the global level,.e., s = x k and s j = x j k. In ths way, the observed transformaton z naturally produces a lnk between the maps orgns S and S j on the global level; z = s j. Fg. 5 shows the Bayesan network representaton of ths event. Note that n most of the cases, multple observatons of the relatve transformaton between two robots would be needed to recovered the full pose (poston and orentaton). Ths wll requre a dedcated mplementaton of a delayed-state flter for nstance. Matchng common nformaton. There are two dfferent ways n the lterature to match common nformaton. The frst one uses sgnal nformaton ndependent from the SLAM estmates, e.g. mage s descrptors matchng (SIFT, SURF), mage ndexng technques [6] or scan matchng. A common way to produce a map of poses (Pose SLAM [5]) s to fnd the rotaton and translaton between two robot poses usng one of these technques, as opposed to trackng features. The second way to match common nformaton s usng the avalable nformaton n the SLAM maps (landmarks poston and uncertanty n a global level usng (5)), whch s trggered usually based on the current poston of the robot or robots n the wrf. We refer to the frst approach as data-matchng (magematchng n the vson case), and the second as map-matchng. The mage-matchng produces a lnk drectly between mages, that are assocated to certan poses S and S j, as shown n Fg. 6(a). Ths s a smple, but effectve manner to obtan the relatonshp between two robots, or even to close a loop wth a sngle robot. The observatons are ndependent from the prevous mapped nformaton, but the robot poses have to be part of the global graph when ths event occurs. As n the rendezvous case, ths event produces drectly the mssng lnk z = s j ; practcally, two current mages matchng s equvalent to a rendezvous. The map-matchng requres the transformaton between lrf to be recovered based on prevously bult maps. It could requre both local maps to be transformed to a common frame for dataassocaton, e.g. promoted to the global level as M and M j usng (5) (see Fg. 6(b)). The matchng process happens n the 3D space, beng the wrf or the lrf s. The dsadvantage of ths method s that n the worst case the absolute poston of the two sub-maps must be computed. Moreover, once the maps are matched, they have to be fused nto a sngle one, otherwse t could lead nto nconsstences when mergng all the maps. The reason s that the local maps nvolved turn out not to be condtonally ndependent m m j z, as shown n Fg. 6(b). Absolute localzaton. In an aeral/ground context, t s reasonable to assume that one or both knds of robots mght receve GPS fxes from tme to tme. 3 The relatve transformaton provded by a GPS fx for the pose x k nstant k s smply sg +, where G s the georeferenced frame and new local map s started. Such nformaton provdes a lnk between a lrf and a global geo-referenced frame, and can generate a loop at the graph level for an ndvdual robot. Fg. 7 shows a graphcal model representaton of ths event. Map mergng. Mergng sub-maps nto a global map s not necessary for the robots to operate, as ths s not requred to mantan the consstency on the graph. Should one requre a global map, the map fuson could be delayed untl all possble loop closures are performed, e.g at the end of the mappng process. For that purpose, we explot an approach smlar to the Dvde & Conquer algorthm []. Maps are changed to a common reference frame and merged two by two, and the fuson consders the common map nformaton n covarance form. Impact on the sub-maps. From the pont of vew of a herarchcal SLAM formulaton, the herarchcal nature of ths model manfests tself n the ablty to apply a loop-consstency constrant to a subset of local transformatons (e.g. a sngle loop) wthout takng nto account the local sub-maps. Partcularly, when no nformaton s shared between sub-maps, whch s the case between two sub-maps bult by dfferent robots wth strctly ndependent observatons but an approxmaton for the sub-maps We use to express condtonal dependence and condtonal ndependence, for further detals please refer to [7]. 3 Typcally an aeral robot s often localzed usng GPS nformaton.

6 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () (a) Data-matchng. (b) Map-matchng. Fg. 6. Bayesan network of nformaton matchng event. (a) Independent nformaton of the map s observed to establsh the lnk between the nodes (e.g. mages), wthout the need to recover the global maps. (b) It requres to recover the global maps to perform the data assocaton. sub-maps should be fused nto a sngle sub-map. Ths has the dsadvantage that the sub-maps must be shared among the two robots, but on the one hand ths s a pre-requste for at least one robot to establsh the matches, and on the other hand such events wll occur when the robots are wthn communcaton range. The man advantage to explot a herarchcal map structure n mult-robot mappng s the low communcaton bandwdth requred among the robots; only the ndvdual graphs need to be exchanged to update the mult-robot graph. Most mportantly s that n the general case, only margnal dstrbutons of each node have to be communcated, as opposed to the full jont dstrbutons of the graph..4. Dstrbuted constrants enforcement Fg. 7. Bayesan network of GPS fx event. A lnk s created to the geo-referenced frame G and the current pose. wth non-ndependent observatons, the orgn of the local submap s the only state that changes after the constrant s appled. We are nterested n formulatng ths mportant property n terms of the propertes of the graphcal model. Let us consder the condtonal ndependence property of the graph n Fg.. It can be easly shown that m S s and therefore m m s,.e., gven the relatve transformatons, the consecutve local sub-maps are ndependent, such that p(m, m s ) = p(m s )p(m s ), () where (3) s drectly appled. Note that the global poses S are d- separated [7] from all possble paths between the par of submaps m and m or even M and M. Also, n the mult-robot case t can happen that two dfferent events create a lnk on the same node,.e., f a map-matchng s establshed after a rendezvous. To avod ths problem, new submaps are started after an event occurs for the robots nvolved. In the case of recevng GPS fxes once n a whle, the fact of startng a new local map at the nstant k when the fx s receved, removes the dynamcs aspects of the nternal local maps settng a fx pose at the global level. Note that n practce GPS fxes could be receved at hgh frequences. In ths case not every measurement should create an event. Smlarly, to avod countng nformaton twce f one eventually wants to merge all the sub-maps, after a map-matchng event both The presented approach s dstrbuted n the sense that each robot bulds ts own local sub-maps. The only nformaton that s shared between robots s the set of relatve transformatons between sub-maps s + (the margnals that form the unconstraned global graph s N {ŝ, P s }). Wth ths nformaton, each robot bulds ts own graph of global orgns and enforces ts own constrants separately. In the centralzed case (e.g. wth a central server), the loop constrant can be enforced whenever a cycle n the graph s found. For the dstrbuted case, f a constrant s enforced by one robot and not the others, further global nformaton exchanges wll lead to ncompatble graphs among robots. Therefore, a loop constrant should only be enforced when t s known by all the robots (see Fg. 8). Nevertheless, any constrant can be enforced locally, provded the graph state before the constrant applcaton s memorzed, whch allows to backtrack to a globally consstent graph when new nformaton s receved from other robots. Several cycles mght appear n the graph when an event happens. Our algorthm deals wth ths ssue enforcng only the constrants over the mnmal cycle, as shown n Fg. 8. Ths approxmaton s performed to enforce only one cycle at a tme n order to accelerate the ncremental onlne soluton. The mnmal cycle guarantees fewer lnearzaton errors and focuses only on the current mapped area. The offlne centralzed soluton should use nstead (9) for all cycles n the graph. The smallest cycle between the new connected nodes s searched for rght after the lnk s added. Not only the graph, but also a record of all the new lnks related wth any event are sent to the robots. Ths s done especally for the robots that are not nvolved n the loop closure.

7 66 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () (a) Event r /r and r /r 3. (b) Event r /r 3. (c) Event r /r and LC r and r 3. (d) Event r /r 3 and LC r and r 3. (e) LC r, r and r 3. (f) LC r 3. Fg. 8. Example of the graph for the dstrbuted algorthm wth three robots. The dash-lnes represent events between robots. For the sake of smplcty, we also consder dash-lnes as communcaton lnks. Hghlghted areas represent nodes nvolve n the optmzaton. The algorthm automatcally searches for the mnmum cycle between the new connected nodes after an event occurs. The sequence of steps of the algorthm s as follows:. Buld local maps.. When an event occurs. (a) Broadcast the unconstraned global graph. 4 (b) Append the unconstraned global graph wth the new margnals. (c) Check f all robots nformaton have been receved. (d) Check f there s a cycle. Perform optmzaton over the mnmal cycle. In the example of Fg. 8, three robots are consdered. Each robot searches for cycles n the graph untl the nformaton of the other two has been communcated. For the sake of clarty the robot n s ndcated as the superscrpt of the global orgn.e., S n. Note that n Fg. 8(c) r does not have any cycle yet (step (b)), whle r and r 3 4 In practce the new nodes can be communcated whenever the robots are n communcaton range, e.g., every tme a new local map s created. do, after havng receved the nformaton of the other two robots prevously (step (c)). It s not untl Fg. 8(d), when r receves the nformaton from the mssng robot that can mpose ts own constrant, and therefore re-localze tself. Algorthms complexty. Wthn the local maps, the EKF-SLAM s O(m ), wth m the number of landmarks n each submap. At the global level, fndng the mnmum cycle s O(log n), wth n the number of nodes of the graph, and the optmzaton of the mnmum cycle of g nodes s O(g), wthout the full computaton of P s that s O(n )..5. Smulaton results We use smulatons to benchmark the algorthms under controlled condtons, whch allows us to compare the estmated values aganst perfect ground truth and therefore to conclude on the consstency of the soluton.

8 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () Table Smulaton parameters for the three robots settngs. Experment Rob Real ntal poston Slam ntal poston Intal standard devaton Speed (x, y, z,ψ,θ,φ) (x, y, z, ψ, θ, φ) (σ x,σ y,σ z,σ ψ,σ θ,σ φ ) (u,ω) (m, m, m, rad, rad, rad) (m, m, m, rad, rad, rad) (m, m, m, rad, rad, rad) (m/s, rad/s) Rendezvous r (, 5, 8,,, ) (, 5, 8,,, ) (,,,,, ) (.,.) r (, 5,,,, ) (., 5.5,.3, (.5,.5,.5, (.,.).5,.5,.5).3,.3,.3) Map matchng r (,, 8,,, ) (,, 8,,, ) (,,,,, ) (.,.7) r (, 5,,,, ) (., 4.7,., (.5,.5,.5, (.,.).5,.5,.5).3,.3,.3) Full collaboraton r (5, 5,,,, ) (5, 5,,,, ) (.,.,.,,, ) (.,.) r ( 5, 5,,,, ) ( 5, 5,,,, ) (.,.,.,,, ) (.,.) r 3 (, 4, 8,,, ) (, 4, 8,,, ) (.,.,.,,, ) (.3,.8) Error x(m) Error y(m) Error z(m) Poston Errors Robot Poston Errors Robot Error yaw(rad) Error ptch(rad) Error roll(rad)..5.5 Orentaton Errors Robot (a) Robot paths and global level. (b) Poston errors r and r. (c) Orentaton errors r and r Orentaton Errors Robot 4 6 Fg. 9. Rendezvous sngle run smulaton results for one aeral and one ground robots. In (a) the odometry s shown n green, real and estmated trajectores are shown n red and blue respectvely. 3σ ellpsods are plotted on the bass of each lrf. (b) shows the global poston errors for each robot and ther global 3σ uncertanty bounds. (c) shows the global orentaton errors for each robot and ther global 3σ uncertanty bounds. (For nterpretaton of the references to colour n ths fgure legend, the reader s referred to the web verson of ths artcle.) Smulaton results are analyzed for three settngs: Rendezvous, map matchng, and the dstrbuted full collaboraton of three robots wth several loop closures trggered by the latter events. The world has 3 landmarks spread along a, m surface. Aeral and ground robots buld fxed sze local maps wth pont landmarks. We show robots trajectores and consstency plots before the fnal global map mergng. In each sub-map, the poses and ther uncertantes are expressed n the assocated lrf s: n order to plot the global postons, we compound the poses and the covarances to obtan them n the wrf. The robots make bearng-only observatons: the nverse-depth parametrzaton (IDP) s used to map 3D pont landmarks, wth the parameters ρ nt =.5 m and σ ρ =.5 m [8]. The bearng-only observaton model has a. standard devaton, and the ntal parameters for the three experments are shown n Table. Robots are controlled by lnear u and angular ω veloctes on ther own planes wth the followng odometry nose model: σ u =. m/ s, σ ω = / s, σ ωu = σ uω =. The uncertanty of each robot s consdered for the 6 DOF pose vector (x, y, z,ψ,θ,φ). Rendezvous. For ths settng we use two robots, r (aeral robot) and r (ground robot). We choose to start wth a large uncertanty for r to see the mpact of loop closures n the global localzaton each local map s started wth zero uncertanty, so ths does not affect the local EKF-SLAM performance. The robots meet n the mddle of the envronment where r detects r and computes the transformaton between the two robots. Ths event trggers a loop closure because both robots ntal postons are expressed n the same wrf. The standard devaton of the relatve measure between robots s (.,.,.,.5,.5,.5) n (m, m, m, rad, rad, rad). The robots meet twce, the frst tme about 3 s and then approxmately at 44 s as shown Fg. 9. The frst rendezvous reduces the covarance on the poston of r and the error as well (see Fg. 9(b) at 3 s). As one can see n the plot after the second rendezvous, agan r s re-localzed. To evaluate the consstency of the approach, we performed a 5-runs Monte Carlo analyss of the normalzed estmaton error squared (NEES) of the current robot pose n wrf as explaned n [9]. Fg. (a) shows the average NEES for the global pose of both robots and the sngle-sded 95% regon for the N 6 DOF durng ths settng. The average NEES shows good consstency for the full smulaton, thanks to the use of sub-maps. For robot r, the average NEES s small compared wth the consstency lmts after the loop closure. Ths s because some uncertanty n the orentaton was added by sensng each other (see Fg. 9(c)), whle the estmaton error reman the same. Map matchng. For ths experment the two robots move clockwse n a crcle. Robot r moves n the drecton of the ntal poston of r. Once r reaches that poston t s able to close a loop, as seen n Fg.. Data assocaton s known. One of the man ssues of ths experment s to compute the transformaton between the two local maps whch have common landmarks, because the leastsquare estmaton s very senstve to ponts whose localzaton s not accurate. The problem s even more dffcult for bearngonly observatons, when newly ntalzed ponts can be anywhere on the bearng lne. The egenvalues of each pont s covarance matrx are computed, and ponts whose egenvalues are below a threshold (e.g..m ) are selected to estmate the transformaton. The relatve transformaton s recovered usng the approach n [3]. The results of a sngle run experment are presented n Fg.. A frst loop s closed at 38 s, whch ncreases the precson of the global localzaton of r. At 5 s the two robots close another loop. Note that those two loops have mostly mproved the global localzaton of r. When r reaches ts orgn, t closes a loop wth ts frst map, at around 6 s; that map beng topologcally connected to the maps of r, a slght mprovement on the localzaton of r can be seen through ts error covarance (Fg. (b)).

9 66 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () Error x(m) Error y(m) Error z(m) Poston Errors Robot Poston Errors Robot Error yaw(rad) Error ptch(rad) Error roll(rad)..5.5 Orentaton Errors Robot (a) Robot paths and global level. (b) Poston errors r and r. (c) Orentaton errors r and r Orentaton Errors Robot Fg.. Map-matchng smulaton results for one aeral and one ground robots. See capton of Fg. 9. Robot Robot Average NEES over 5 runs (a) Rendezvous settng. Robot Robot Average NEES over 5 runs (b) Map-matchng settng. Fg.. Average NEES of 5 Monte Carlo runs of the robot pose (poston and orentaton) for (a) the rendezvous settng and (b) the map-matchng settng. The averaged NEES s n a thck lne. The thn lnes represent the NEES for each run. The dashed lne corresponds to a NEES value of 7.8, whch s the sngle-sded consstency lmt of 95% confdence for the 6-dmensonal state vector. Fg. (b) shows the average NEES of the robots poses for ths settng; after a loop closng the average NEES remans pretty much the same, showng consstency for both robots. Dstrbuted collaboraton between three robots. We show here the effects of multple loop closures between three robots that communcate ther unconstraned global maps whenever an event occurs. Two ground robots r and r move along crcles n dfferent locatons on the envronment. They never meet, and ther maps never overlap. The thrd robot r 3 s an aeral robot that moves n a crcular trajectory whch extends from the area covered by r to the one covered by r. Each robot smulaton runs n ts own process. The three robots start at a well known locaton, then at 4 s, r and r 3 have a rendezvous, later at 6 s and 7 s the robot r 3 detects a loop closure wth two maps of r (the vdeo slamnet.mp4 5 shows a run of ths smulaton). The robots communcate the unconstraned global graph when a local map s created. Perfect communcaton s assumed. The uncertantes are expressed n the wrf so that the effects of the loop closure can be seen. Notce the robots only have nformaton about ther own landmarks. The consstency plots for a sngle run are shown n Fg.. The fnal map and global graph before and after mergng (at the end of the smulaton) are shown n Fg Estmatng lne segments n the monocular EKF-SLAM framework Most of the vsual SLAM approaches rely on landmarks defned by nterest ponts detected n the mages, be they Harrs, CenSurE, SIFT, SURF or other features. The estmated state of such landmarks s ther 3D poston, and the resultng maps are sparse collectons of 3D ponts; even though they are spatally consstent and span the world s three dmensons, such maps do not represent useful nformaton to asses volumes, to compute vsbltes, or to establsh matches ether wth maps bult by another robot or wth exstng 3D maps now wdely avalable. To mprove the map s representatonal power, one of the key ponts here s to use, n addton to sparse 3D ponts, lnear landmarks or lne segments. Lnes provde an mproved semantc over ponts: they nherently contan the notons of connectvty (they span from one poston to another) and boundary (they separate one regon from another), whch open the door to potental automatc nterpretatons of the envronment, both at the local and global levels. A 3D model based on lnes can further allow the possblty to buld hgher level enttes (planes, closed regons, objects). By buldng such maps, our goal s to am at buldng a meanngful 3D model, on the bass of whch one can assess vsbltes and match features or maps from dsparate vewponts.

10 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () Error x(m) Poston Errors Robot Poston Errors Robot Poston Errors Robot Error yaw(rad) Orentaton Errors Robot Orentaton Errors Robot Orentaton Errors Robot Error z(m) Error y(m) Error roll(rad) (a) Robot paths and Global level. (b) Poston Errors r, r and r 3. (c) Orentaton Errors r, r and r 3. Fg.. Smulaton results for the 3 robots exploraton; one aeral and two ground robots. See capton of Fg. 9. (a) Before mergng. (b) After mergng. Fg. 3. Fnal map resultng from the 3 robots exploraton centralzng the nformaton. The fgure shows the 3D global map, local map orgns and the fnal robots and landmarks locaton n wrf. These goals, however, are dffcult to acheve and le beyond the scope of ths work. As recognzed by Smth et al. [3], one of the dffcult features to acheve satsfactory SLAM operaton wth lnes s a good lnes extractor and matcher algorthm, whch s not one of our goals n ths paper. We concentrate only on the estmaton of the geometry of 3D lnes from movng monocular platforms, whch consttutes by ts own rght a problem that has not been successfully solved yet, as we expose n the followng secton. Wde baselne map matchng based on lnes s ntroduced, and demonstrated only n smulaton. Real map matchng based on lnes and map nterpretaton are left for future work. The focus s therefore on the ablty of buldng heterogeneous maps wth ponts and lnes. 3.. Related work Most approaches to vsual SLAM wth segments defne the segments based on two endponts that support them, fxng the segment s length so that partally occluded segments reman shorter than they actually are regardless of the evdence gathered wth new observatons. Addtonally, the majorty of these works make use of delayed ntalzaton technques, lmtng the potental performances of monocular or long-range sensng SLAM. These remarks are detaled n the followng paragraphs. Ref. [3] proposes a model-based monocular SLAM system usng lne segments and the UKF, but the use of drect-dstance parametrzaton for the unknown endponts depths leads to delayed ntalzaton. Ref. [3] represents segments wth ther two endponts. The support endponts are coded usng nverse-depth parameters durng the ntalzaton phase, and are converted to Cartesan representatons after convergence. The nverse depth parameters are ntally estmated by an external EKF untl convergence, thus delayng ntalzaton. Ref. [33] makes use of small edge landmarks, named edgelets, assocated to a 3D pont whch s ntalzed n an undelayed manner. Edgelets are typcally 5 pxels long, and longer lnes must be represented by several edgelets. Ths ntrnscally local defnton of edgelets does not possess the connectvty property that we hghlghted above (though t does possess local boundary), compromsng precsely the representatveness that we are seekng. We look here for representatons of arbtrarly long lnes wth the addtonal possblty of ncrementally updatng ther endponts accordng to new observatons. These works have been conceved and demonstrated n ndoor setups, often usng camera motons that have been purposely chosen to maxmze observablty (.e., motons wth a sgnfcant sdeways component lookng at objects close by), thus achevng successful operaton even wth delayed ntalzaton. Our case s radcally dfferent as we are dealng wth outdoors robots whose trajectores are planned and executed wth other prortes n mnd, and where objects to be mapped mght be at large dstances. As we have demonstrated earler [34 36], undelayed ntalzaton allows the robots to use the partal landmark nformaton for localzaton from the very frst observaton of a landmark, regardless of the sensor trajectory and the dstance to the landmark, greatly helpng to constran the camera localzaton. In partcular, ths makes the use of remote landmarks benefcal for long term atttude estmaton, mnmzng angular drft whch s the major cause of nconsstency n EKF-SLAM [9]. Ths consttutes a crucal feature n e.g. aeral vehcles as n our case, especally because, due to the long range sensng requrements and strct energy and payload constrants, only monocular vson seems to be practcable. Our approach draws on [36], whch s n turn based on [37]. Ref. [37] performs SLAM wth segments usng the Eucldean

11 664 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () (a) Plücker lne. (b) Anchored Plücker lne. Fg. 4. Geometrcal representaton of Plücker-based lnes, the sub-vectors n and v and the anchor p. (a) The Plücker lne L and the orgn O defne the support plane π. The lne s sub-vector n R 3 s orthogonal to π. The sub-vector v R 3 s a drector vector of the lne, and les on π. Ths mples n v. The closest pont to O s Q = (v n : v v) P 3. The dstance from L to O s d =n/v, showng that v acts as the homogeneous part of L, thus exhbtng nverse-depth propertes. (b) The anchored Plücker lne Λ s a Plücker lne referred to an anchor p. The closest pont of the lne to the anchor s q = p + (v n)/(v v). Plücker coordnates (a sub-set of the Plücker coordnates where the drecton vector s normalzed) to map nfnte lnes. The major drawback of [37] s a delayed ntalzaton. In [36] we added undelayed operaton by usng the Plücker coordnates drectly, where the Plücker drecton vector s norm exhbts nversedstance behavor. Here we add an anchor to mprove lnearty, equvalently to what t s shown n [38] for the case of ponts. Plücker lnes have also been used n many major vson works wth straght 3D lnes [39 4]. These works, and other ones referenced theren, are based on Structure From Moton approaches solved offlne usng nonlnear optmzaton. Onlne, ncremental SLAM methods based on these technques would have to draw on e.g. [4] (whch works for ponts and edgelets) but they have not yet been reported. 3.. Plücker lnes (PL): ncorporatng the nverse-depth concept to lnes The Plücker coordnates for a lne consst of a homogeneous 6-vector n projectve space, L P 5. In ths vector, one can dentfy two sub-vectors, 6 L = (n : v), wth {n, v} R 3, wth whch an ntutve geometrcal nterpretaton of the lne n 3D Eucldean space s possble [36] (Fg. 4(a)):. The sub-vector n s a vector normal to the plane π supportng the lne L and the orgn O.. The sub-vector v s a drector vector of the lne. 3. The dstance from the lne to the orgn s gven by d =n/v. The basc operatons needed to manpulate Plücker lnes are detaled below: Frame transformaton. Gven a (camera) reference frame C specfed by a rotaton matrx R and a translaton vector T, a Plücker lne L n global frame s obtaned from a lne L C n frame C wth the lnear transformaton [39] R [T] R L = L C. () R Pn-hole projecton. Gven a perspectve camera defned by the ntrnsc parameters k = (u,v,α u,α v ), a Plücker lne n camera frame L C = (n C : v C ) projects nto a homogeneous lne λ P n the mage plane wth the lnear 6 We use a colon (:) to separate the non-homogeneous and homogeneous parts n projectve space. expresson [39] αv λ = K n C α u n C P. (3) α v u α u v α u α v where K s called the Plücker ntrnsc matrx. Its relaton to the regular ntrnsc matrx K s K K. The two most remarkable propertes of the Plücker lne are ts lnear transformaton and projecton equatons and the nversedepth behavor of the sub-vector v (recall property 3. above), somethng that allows us to desgn approprate undelayed ntalzaton methods for EKF. For further detals on the use of Plücker lnes n monocular EKF-SLAM see [36] Anchored Plücker lnes (APL): mprovng on PL lnearty Now, we add an anchor to the parametrzaton to mprove lnearty, as t s done for ponts n the nverse-depth parametrzaton [8]. Anchorng the Plücker lne means referrng t to a pont p n 3D space dfferent from the orgn (Fg. 4(b)). The anchor pont p s chosen to be the optcal center at ntalzaton tme. Thanks to ths, the Plücker lne part (n : v) s correlated to the rest of the map only through the anchor p. As a consequence, on subsequent EKF updates, only the accumulated errors from p to the current camera poston T are consdered, n contrast wth regular Plücker lnes where the error accounts for the absolute moton of the sensor from the orgn of coordnates. The anchored Plücker lne (APL, Fg. 4(b)) s then the 9-vector: p Λ = n R 9. (4) v The operatons needed to manpulate APL are as follows: Frame transformaton. Frame transformaton reduces smply to transformng the pont p and rotatng the vectors n and v. Ths s accomplshed wth the affne transformaton R T Λ = R Λ C +. (5) R Un-anchorng. Gven an anchored Plücker lne Λ = (p, n : v), ts correspondng (un-anchored) Plücker lne L s obtaned by transformng the Plücker part (n : v) from a frame at the anchor (T, R) = (p, I 3 ) to the global frame wth (),

12 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () and removng the anchor. Ths reduces to n + p v L =. (6) v Transformaton and projecton. Transformaton and projecton are accomplshed wth a transformaton to the camera frame (the nverse of (5)), un-anchorng (6), and Plücker projecton (3). The three steps can be composed n one sngle expresson wth: λ = K R (n (T p ) v) P, (7) 3.4. Segment endponts The lne s endponts n 3D space are mantaned out of the flter va two abscssas defned n the local D reference frame of the lne, whose orgn s at the pont q = p + (v n)/(v v), the closest pont to the anchor (see Fg. 4(b)). Gven the lne Λ = (p, n : v) and abscssas {t, t }, the 3D Eucldean endponts are obtaned for {, } wth p = q + t v v = p + v n v + t v v. (8) 3.5. Back-projecton of an APL APL back-projecton conssts n defnng a Plücker lne L from a segment observaton λ, and anchorng t at the camera poston T to obtan an APL Λ. These operatons are detaled below. Back-projecton of a Plücker lne. Here we brefly summarze the development n [36]. In the camera frame, the Plücker sub-vector n C resultng from the observaton λ s smply the nverse of (3), n C = K λ. (9) The second sub-vector v C s not measured and must be obtaned by njectng pror nformaton. Ths pror must specfy exclusvely the DOF that are mssng n the observaton. It s defned n the -dmensonal plane and mapped to the plane orthogonal to the observed n C. Ths plane s spanned by the base matrx E whch s obvously orthogonal to n C, E = e e, n C e e n C. () Then, gven a pror β R, the sub-vector v C s obtaned lnearly wth v C = Eβ. () For convenence, we arbtrarly buld E so that the ponts n the β-plane correspond to lnes easy to nterpret. Wth ths purpose of ntutveness we choose the base vectors {e, e } so that β s exactly nverse-dstance and e s parallel to the mage plane, leadng to n C n C e = n C and e = n C e (n C ) + (n C n C. ) () Anchorng. Ths step s trval as we have an nterest n makng the anchor p concde wth the current camera poston, whch s the orgn when we are n the camera frame, Λ C = n C. (3) v C Back-projecton and transformaton. The operatons above plus the transformaton to the global frame (5) can be composed and wrtten as a sngle-step functon of R, T, λ and β, p Λ = n = T RK λ. (4) v REβ We name ths functon g() and retan t as t s needed for landmark ntalzaton. In EKF-SLAM, the pose orentaton R s often encoded by a quaternon or the Euler angles, whch we denote ndstnctly by Q. The camera pose C s specfed by the vector x = (T, Q) (we neglect here the super-ndex ndcatng the local map, x x, to make the notaton easer to read), n whch case the functon above becomes Λ = g(x, λ, β), (5) whch s smply a functon of the current state x, the current measurement λ and the provded pror β Undelayed APL ntalzaton n EKF-SLAM To ntalze the APL, suppose we have a camera C at locaton x = (T, Q), wth ntrnsc Plücker matrx K. x has uncertantes encoded n the map, 7 whle K s assumed to be determnstc. A newly detected segment, together wth ts uncertanty encoded by a covarances matrx, needs to be transformed (va regular covarance propagaton based on the Jacobans of the transformaton) to a homogeneous lne n P. For example, f the segment s detected n the form of two endponts {u, u }, wth nose covarance R u = dag(σ u,σ u ) each, we obtan the homogeneous lne s pdf λ N {ˆλ; L} by jonng the two ponts ˆλ = u u (6) L = u R u u + u R u u, (7) where u = (u : ), R u = dag(r u, ), and [ ] s the skewsymmetrc matrx assocated to the cross-product (.e., [u] v u v), uz u y [u] u z u x u y u x. (8) Intalzaton of the support APL s done wth the classcal EKF- SLAM method, by lnearzng Λ = g(x, λ, β), Eqs. (4) (5), and provdng β as a Gaussan pror. The camera C s part of the -th local map vector x m, that we rename here smply x m. Its Gaussan pdf x m N {ˆx m ; P m } s decomposed n (), let us rename m m the set of exstng landmarks n the current (the -th) local map. For the pror β we defne a Gaussan pdf β N { ˆβ; B} as shown n Fg. 5. Ths Gaussan n the β-plane wll be convenently mapped to the 3D space va the transformaton matrx E, as we have seen n the prevous secton. We use the non-sotropc Gaussan shown n Fg. 5(b), descrbed by ˆβ = /3dmn, B = We further choose d mn =.75 m. (/3dmn ) (/d mn ). (9) 7 The camera pose C may be encoded n the map ndrectly,.e., va a fxed transformaton related to the robot pose, C = f (x). Wthout loss of generalty we consder ths functon here to be the dentty. In case a non-trval transformaton s to be consdered, we need to compose t wth (5) to get the new functon Λ = g(f (x), λ, β) g (x, λ, β).

13 666 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () (a) Isotropc pdf wth lne at nfnty. (b) Non-sotropc pdf penalzng lnes at the back of the camera. Fg. 5. Defnng a pdf for the pror β. (a) The Gaussan pdf contans all possble lnes at a mnmum dstance of d mn ; t s sotropc n orentaton, t ncludes the orgn whch represents the lne at nfnty, and d mn s at σ. For reference, a Gaussan shape s supermposed on the horzontal axs to evaluate the probablty values at σ and 3σ. (b) An nterestng alternatve that penalzes lnes n the back of the camera s to approxmate just the rght-hand half of the pdf n (a) (here dashed) by a new Gaussan. A good ft s obtaned wth ˆβ = (/3d mn, ) and a non-sotropc covarance B = dag([σβ,σβ ]) wth σ β = /3d mn and σ β = /d mn. We obtan wth (4) the landmark s mean and Jacobans ˆΛ = g(ˆx, ˆλ, ˆβ) (3) G x = g, G λ = g, G β = g x λ β (3) (ˆx,ˆλ, ˆβ). (ˆx,ˆλ, ˆβ) (ˆx,ˆλ, ˆβ) wth whch, beng x, λ and β mutually ndependent, we compute the co- and cross-varances P ΛΛ = G x P xx G x + G λ L G λ + G β B G β (3) P Λxm = G x P xx P xm (33) to fnally augment the map xmλ x m, ˆx m, P ˆxmˆΛ m Pm P Λxm P Λxm. (34) P ΛΛ The endponts are found by retroprojectng both mage endponts {u, u } onto the Plücker estmate s mean, expressng the soluton {p, p } n terms of abscssas {t, t } wth (8), and solvng for them. Detals of these operatons and the necessary algebra can be found n [37,36] Segment EKF correcton The update of the APL s essentally a standard EKF update. The only dffculty, perhaps, s the fact that a proper measure of dstance between two lnes s not avalable n the Eucldean sense, and we need to fnd somethng practcable to defne the nnovaton. We chose the technque descrbed n [37] whch defnes the nnovaton space n polar coordnates (Fg. 6). The observaton functon h() s defned by composng (7) wth the transformaton from homogeneous lnes λ = (λ,λ,λ 3 ) nto polar coordnates (ρ, θ), gven by ρ = λ 3 / λ θ + λ. (35) arctan(λ,λ ) The measured lnes are defned by endponts {u, u } wth a fxed covarance R u each. Convertng them to polar coordnates s just a matter of composng (6) wth (35). We remnd that an nnovaton measure y = z h(ˆx m ) based on a mappng (35) contans some sngulartes and dscontnutes that we need to tackle. See [37] for detals. The currently measured segment s endponts are retro-projected onto the 3D lne, and ts abscssas {t, t } computed (see Eq. (8)). Fg. 6. The polar coordnates of a lne n D space. Durng the lne s convergence phase, the segment endponts abscssas are systematcally replaced wth the current ones. We use some heurstcs to determne when the lne estmate s stable enough, and then apply an extendng-only polcy; each segment s abscssa s updated wth the new one only f ths lengthens the segment Map-matchng wth 3D lnes Assumng the data-assocaton problem for lnes n the 3D Eucldean space s solved, the problem of recoverng the transformaton between two reference frames, based on a set of 3D lnes, s approached usng a closed form least-squares soluton. Frst the APLs are transformed to the Eucldean Plücker coordnates representaton usng (6) and normalzed wth respect to the lne drecton as v = v v and n = n. When removng the v anchor, the normal n s defned wth respect to the orgn of the reference frame. Then () s used to state the least-squares problem to determne R and T. As n the case of pose estmaton from correspondng 3D ponts, the problem s decoupled to frst recover the rotaton matrx and later the translaton vector [3]. From correspondng Eucldean PLs the orentaton s determned through the followng crteron, Err(R) = N v n R(vj n ) (36) n= where N s the number of matched lnes between reference frames and j. The rotaton matrx that mnmzes (36) s obtaned through a sngular value decomposton (SVD) algorthm as descrbed n [3] for 3D ponts. The translaton vector T s obtaned by mnmzng, Err(T) = N n n Rnj n vn T, (37) n= thus the explct translaton s recovered as follows, N N T = v n v n v n (n n Rnj ) n. (38) n= 3.9. Smulaton results n= We smulate a scenaro consstng of a wreframe of a house bult wth 7 segments. Fg. 7 shows the house beng reconstructed. A robot wth one perspectve camera (9 FOV, px resoluton,.5 px error) lookng forward approaches from a certan dstance at. m/s, gatherng mages at 3 fps. In order to observe the scale factor, the robot takes nosy odometry readngs wth. m/ m and.5 / m error. In order to mprove consstency, the measurement nose covarance s multpled by a factor,.e., R u = dag(.5,.5 ) px, as suggested n [9].

14 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () (a) Wthout ground-truth. (b) Wth ground-truth. Fg. 7. The smulated envronment conssts of a robot approachng a wreframe of a house. The fgure shows (a) the partally reconstructed house, after approxmately 4 frames have been processed. Despte the prelmnary state of the map, the structure s already vsble. Wth ths far amount of data, a map made of ponts (wth e.g. one pont at each lne ntersecton) would not convey very much nformaton besdes localzaton, because t lacks structural semantcs such as connectvty and boundares. (b) Fully reconstructed house wth the ground-truth n blue. 5 Average NEES over 5 runs 5 Average NEES over 5 runs Average NEES 5 Average NEES Frames (a) Sngle APL map Frames (b) Multple APL maps. Fg Monte Carlo runs (thn gray lnes) showng the NEES measures of the robot 6D-pose. The averaged NEES s n thck lne. The dashed lne corresponds to a NEES value of 7.8, whch s the sngle-sded consstency lmt of 95% confdence for 6 DOF and N = 5 runs,.e., χ(6 5) (.95)/5 = 7.8, see [9]. (a) APL-SLAM wthout submappng. (b) APL-SLAM usng submappng; 4 maps are created. To evaluate the estmaton s consstency, a Monte Carlo analyss of the normalzed estmaton error squared (NEES) of the robot 6D-pose s made (Fg. 8(a)). The averaged NEES after 5 runs shows good consstency up to frame and a rsker behavor from then on. Ths s n accordance wth [9], whch concludes that long-term EKF-SLAM s always nconsstent, provdng evdence of the necessty of approaches usng multple local maps. Smulaton results wth multple local maps are shown Fg. 8(b), where the consstency has been clearly mproved. In order to evaluate the map-matchng approach presented n Secton 3.8, two robots are deployed n a smulaton settng populated wth lne segments only (see adjont vdeo mapmatchnglnes.av 8 ). The robots trajectores do not ntersect untl the end of the run, however two of ther 5 lnes submaps partally overlap. A map-matchng event occurs at frame 377 (data assocaton s known n ths setup). The event takes place after 6 anchored Plücker lnes from the aeral robot current frame (S 4 ) are matched wth a prevous map bult form the ground robot (S ). Lnes wth large uncertantes are not taken nto account for the least square mnmzaton. The transformaton between the lrf s obtaned wth the proposed approach s ŝ 4 = (.73, 4.4, 6.4,.78,.4,.6) n (m, m, m, rad, rad, rad). The real transformaton between the reference 4th 8 and th frames s s 4 = (.66, 4.56, 6,.69,, ). Note that the orentaton and the translaton are well-estmated, but most mportantly they are consstent. Fg. 9 shows the 3D robots locaton and sub-maps before and after the map-matchng event, and Fg. shows the consstency plots for a sngle run of ths settng. 4. Expermental results 4.. Setup Outdoors data acqustons n a large envronment have been conducted to verfy the performance of the proposed approach, wth the ground robot Dala and the helcopter Ressac (Fg. ). The envronment s an abandoned country vllage n the south of France, now used as a mltary tranng faclty see Fg.. It s sem-structured, n the sense that t does not contan as many buldngs as an urban area, and the buldng themselves do not contan many straght lnes or perfect planar areas. The ground robot Dala s an Robot ATRV platform, equpped wth a calbrated stereo-vson bench made of two cameras wth a baselne of.35 m. The helcopter Ressac s controlled by algorthms developed at Onera [43], and s also equpped wth a calbrated stereo vson bench made of two cameras, wth a.9 m baselne.

15 668 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () (a) Before lne-matchng event. (b) After lne-matchng event. Fg. 9. Event effect n the global map, the sub-maps orgns expressed n the wrf are the large ellpsods 3D lne segments landmarks are also shown here. Error x(m) Poston Errors Robot Poston Errors Robot Error yaw(rad) Orentaton Errors Robot Orentaton Errors Robot Error y(m) Error ptch(rad) Error z(m) Frames Frames Error roll(rad) Frames (a) Robot paths and global level. (b) Poston errors r. (c) Orentaton errors r Frames Fg.. Multple APL maps wth lne-matchng event sngle run smulaton results for ground and aeral robots. In (a) the odometry s shown n green, real and estmated trajectores are shown n red and blue respectvely. 3σ ellpsods are plotted on the bass of each lrf. (b) shows the global poston errors for each robot and ther global 3σ uncertanty bounds. (c) shows the global orentaton errors for each robot and ther global 3σ uncertanty bounds. (For nterpretaton of the references to colour n ths fgure legend, the reader s referred to the web verson of ths artcle.) (a) Dala. (b) Ressac. Fg.. Ground and aeral robots used for the expermental valdaton. Several thousands of mages have been acqured whle the helcopter Ressac s automatcally achevng a swathng pattern at an alttude of about 4 m, and Dala s makng loop trajectores n the north-west group of buldngs under manual control (Fg. 4). 4.. Involved processes The SLAM algorthms ntegrate two types of observatons from only one camera; mage ponts and mage lne segments. The mapped ponts are parametrzed as nverse-depth ponts, and the mapped segments are parametrzed as anchored Plücker lne segments, as presented Secton 3. Unfortunately, because of engneerng ssues encountered durng the data collecton, no nertal or odometrc moton estmates are avalable. 9 As a consequence, we use a vsual odometry approach based on stereo vson for the moton predcton steps of the EKF SLAM algorthms whch n turn s the mean through whch the scale s recovered. 9 GPS ground truth could nether be recorded.

16 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () (a) Dala local map. (b) Ressac local map. Fg.. Local maps bult by the rover Dala (a) and the UAV Ressac (b). (a) Dala local map. (b) Ressac local map. (c) Ressac/Dala combned map. Fg. 3. Local maps supermposed on a aeral vew of the scene; wth the rover Dala (a), the UAV Ressac (b), and the combned submaps (c). Yellow ellpsods represent the endponts covarances. Note that only lne segments wth small covarances have been plotted. At each mage acquston, pont observatons are frstly processed; the resultng updated moton estmate s exploted by the lne segment tracker, and lne landmarks observatons are then processed. Images are sub-sampled by a factor of before beng processed (64 48), and a heurstc s used to select the ponts that wll be used as landmarks; the mage s regularly parttoned n 3 3 regons, n whch one ensures that at least landmarks are beng tracked one or two new nterest ponts are selected as a landmark every tme ths s not satsfed. As for the lnes, only the ones whose length s greater than 6 pxels are retaned as landmarks Landmark detecton, trackng and deleton Pont landmarks are Harrs nterest ponts that are matched from one vew to the other wth the group based matchng procedure descrbed n [44]; a frst canddate match between two nterest ponts s establshed usng sgnal nformaton (the two prncpal curvatures of the auto-correlaton functon), and confrmed f matches of neghborng ponts that satsfy geometrc constrants on ther locaton n the mage are found. We use dfferent ntalzaton parameters for nverse depth pont parametrzaton wth Dala and Ressac. Dala s parameters are ρ nt =. m and σ ρ =. m, whle Ressac s parameters are ρ nt =.5 m and σ ρ =.5 m (the ponts are ntalzed at 4 m, whch s the helcopter average elevaton over the terran). To extract and track lne segments, we use a model-drven approach, that s very robust to scene and llumnaton varatons, and that does not requre the settng of any senstve parameter (detals can be found n [45]). For the estmaton part, the a pror parameters used n the experment for the APL are β = (.5, ), σ β =.5 and =.375 for both robots. The predcton of the lne segment σ β poston n the mage, requred by the segment tracker, s done usng the projecton of the 3D lne segment nto the mage frame. Smart landmark deleton s crucal for mantanng relable maps consstng of only stable and consstent landmarks. We perform t at the local-map level, based on a test usng three counters assocated to each landmark: N s, the number of match attempts (searches); N m, the number of matches performed; and N, the number of valdated matches (nlers). A landmark s deleted whenever the condton D(N s, N m, N ) = (N s > ) [(N m /N s <.5) (N /N m <.5)] holds true. D() bascally ensures suffcent evdence wth (N s > ), and then checks f the landmark appearance OR vsblty are unstable, wth (N m /N s <.5), or f the landmark estmate s nconsstent wth the observatons, wth (N /N m <.5). The three thresholds (,.5,.5) could be optmzed n some way, however, wth these ntal values the mpact on map qualty and estmaton consstency s notable Local maps New local maps are created when landmarks (combnng ponts and lne segments) are n the map. Immedately after, the current robot s pose s the new relatve transformaton n the global graph. Fgs. and 3 shows examples of local maps bult by Dala and Ressac: The maps consst of a set of 3D lne segments and 3D ponts. Fg. 4 shows the estmated trajectores of the robots, supermposed to the aeral mage of the area. For ths run, no events that lnk the two robots graphs are consdered Enforcng loop closures Wth ths dataset, only very few lne segments are mapped by both robots, whch precludes the segment map matchng. To defne

17 67 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () (a) Dala trajectory n open loop. (b) Ressac trajectory n open loop. Fg. 4. SLAM trajectores for Dala and Ressac n open loop, drawn on an aeral vew of the experment area (obtaned on The actual trajectory of Dala fts the road, whereas here the trajectory estmate does not, although ts orgn as been set at the good poston over the aeral vew. loop closng events, we therefore take advantage of the avalablty of stereovson data, and emulate wth 3D pont matches two types of events, rendezvous and data-matchng (mage to mage): The rendezvous s emulated usng matches of nterest ponts perceved by the two robots usng ther current mage frames. The 3D coordnates of the ponts obtaned by stereovson yeld the possblty to provde an estmate of the relatve robot poston. The mage-matchng event recovers the relatve transformaton between the current robot pose and a past pose from a dfferent robot (orgn of a local sub-map, such as n Fg. 6(a)) usng also matches of nterest ponts between ther respectve frames. Only key-frames correspondng to the orgns of local sub-maps are used for the matchng. Once an event arses, a modfed A-star algorthm s used to detect the occurrence of a cycle n the graph, and n partcular the mnmum cycle on whch the loop constrant s appled (the algorthm searches for the mnmal length path between the two nodes lnked by the event, the exstence of ths lnk beng gnored for the search). Fgs. 5 8 show results obtaned by the ntegraton of events between Dala and Ressac. Dala starts at the entry of the northwest group of buldngs, wth no uncertanty n ts local map, but also n the wrf :.; the frst Dala sub-map s the orgn of the world. Ressac starts above Dala, and heads towards the south-east. A frst rendezvous event occurs mmedately after the start, and Ressac s localzed n Dala s reference frame. A second event occurs after Ressac comes back from the southeast vllage, passng above a place prevously vsted by Dala. The effects of the mage-matchng event are shown n Fg. 5. The fgure also shows the mage frames that were evaluated for the matchng; new local maps are ntated afterward for both robots. Note that Ressac s uncertanty n heght s pretty large, especally before the second event: the vsual odometry used as predctons s ndeed not very precse n the vertcal drecton, because the Ressac stereo baselne s small wth respect to the depth of the perceved ponts and the ntegraton of ponts and lnes n the sub-maps does not greatly reduce the elevaton estmates. However, after the datamatchng event, the elevaton of Ressac and the orgns of all the bult maps are strongly corrected. No map-mergng s done. Fg. 6 shows n the same plot the robots trajectores as estmated by vsual odometry, SLAM wthout any loop closure event ntegrated, and wth the ntegraton of the events. The fnal global graph (orgns of local maps wth assocated uncertantes n the wrf ), and the robots trajectory n wrf are shown Fg. 7. Ths fgure shows how the orgns of Ressac s submaps around the same area (sub-map 5 and 7) have a smlar uncertanty, as an effect of the loop closng event when t comes back. Note also that Ressac s 3D fltered path s off the trajectory defned by the sub-maps orgns (mean of the ellpsods); the path shown s the result of the EKF at each tme nstant, that s not corrected by the optmzaton algorthm. However, the full topology (orgns of the local maps) s readjusted, as n a smoothng algorthm. Fnally, Fg. 8 presents qualtatve results of the proposed approach. Each robot has processed over mages mappng approxmately an area of 3 3 m. In the second part of the adjont vdeo arground.mov, the robots trajectory n wrf are shown. The south-east group of buldngs s easy to spot n the vdeo, t s the place where Ressacc s turnng 8. Also, the vdeo shows the processed mage sequence for Dala and Ressac, along wth the 3D landmarks, and local maps orgns n wrf. 5. Dscusson The contrbutons proposed n ths paper ft well wth the objectve of deployng cooperatve aeral / ground robotc systems; on the one hand, the dstrbuted mult-map approach handles communcaton losses and can cope wth large areas, and on the other hand, maps made of ponts and lnes represent better the envronment geometry than only ponts, and thus yeld the possblty to match and fuse data acqured by both knds of robots. The proposed soluton to the localzaton and mappng problem s based on a combnaton of flterng (wthn the local maps the robots poses are fltered) and smoothng (the orgns of the local sub-maps are past robots poses and whenever an event occurs the past poses are corrected). The mappng problem s therefore relegated wthn the local sub-maps, and s decorrelated from the global localzaton problem, makng our approach akn to a cooperatve localzaton approach, where robots mprove ther localzaton usng the nformaton provded by other robots [5]. In terms of cooperaton, the margnals of graph level s the sole nformaton that must be exchanged between the robots or

18 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () (a) Image frame for Dala. (b) Image frame for Ressac. (c) Before an event. (d) After an event. Fg. 5. Top: Image frames from both robots before the event, approxmately correspondng to the mddle column mages of Fg.. Green squares represent nterest pont currently consdered as landmarks, yellow squares represent nterest ponts just ntalzed as landmarks. The lne segments are n blue, wth endponts n red. Yellow ellpses are the uncertanty n the mage vew. Bottom: Event effect n the global map, the sub maps orgns expressed n the wrf are the large ellpsods only 3D lne segments landmarks are shown here. (For nterpretaton of the references to colour n ths fgure legend, the reader s referred to the web verson of ths artcle.) Fg. 6. Comparatve trajectory plots; odometry n dash-dot lne, open loop run n dashed lne and cooperatve run for Dala (left) and Ressac (rght).

19 67 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () Fg. 7. Fnal global graph (the global level) wth Dala s trajectory n blue and Ressac s trajectory n red n the wrf. (For nterpretaton of the references to colour n ths fgure legend, the reader s referred to the web verson of ths artcle.) Fg. 8. The thck-lnes are the estmated trajectores for Dala and Ressac after the ntegraton of two rendezvous/matchng events. Ressac s trajectory s shown n red, and Dala s trajectory s shown n blue. Note that Dala s SLAM trajectory s now ftted the road. (For nterpretaton of the references to colour n ths fgure legend, the reader s referred to the web verson of ths artcle.) to a central server. Future work consders the mplementaton of the dstrbuted approach n a real scenaro. The approach s well suted to a mult-robot context, and t can n partcular handle all the possble localzaton means, from odometry to absolute localzaton wth respect to an ntal model. The fact that no nformaton s shared between sub-maps leads to a loss of nformaton, as faraway ponts and lnes that are beng tracked for long perods of tme and between sub-map need to be re-ntalzed after a new sub-map s created. The condtonal ndependent maps approach (CI-SLAM [46]) pallates ths problem n the sngle robot case. But n the mult-robot case, ths soluton rases ssues that reman to be studed n partcular, sharng nformaton between the robots can not be straghtforwardly done at the graph level. In order to obtan the best localzaton for each robot n the ncremental SLAM soluton and to avod any graph ncompatblty between robots, an alternatve effcent mplementaton of a posteror maxmum lkelhood soluton should be consdered nstead of the mnmal cycle optmzaton. The map mergng process s not requred for the robots to operate, but should one requre a global map, t can be done at the end of the mappng process. In our smulatons, we appled a fuson n covarance form; fuson n nformaton form [6] has however more nterestng propertes for decentralzed systems, because nformaton n the same reference frame s just added. Moreover, the sparseness of the nformaton matrx s structure can be exploted to reduce communcaton bandwdth. Combnng covarance and nformaton forms mght reduce the computaton complexty, as shown n [47] whch explots the advantages of both methods. In order to buld landmark maps that are nvarant to the vantage pont, we have proposed to use 3D ponts and lne segments. An mportant contrbuton of ths paper s the new lne segment parametrzaton for undelayed ntalzaton, the anchored Plücker lne, wth whch we exploted the two key concepts of nverse-dstance and anchorng. The outcome s promsng, but not fully satsfactory yet. Ongong work s a more detaled analyss of dfferent lne parametrzatons wth the am of further mprovng lne estmaton performances, most partcularly wth regard to flter consstency but also to other mportant aspects such as endponts management [48]. The bult maps combne nverse-depth ponts and anchored Plücker lne segments. Essentally, as we stated the problem, detected lne endponts do not provde useful nformaton as there s no guarantee that they represent stable 3D ponts. Therefore, one lne n the mage conveys the same amount of nformaton as one pont: DOF. In general, a set of N 3D lnes n general confguraton provdes the same amount of nformaton as a set of the same number of ponts, also n general confguraton. Havng both ponts and lnes means that such an amount of nformaton can be gathered wth, say, N/ ponts and N/ lnes. Wthn a complex scenaro, there may be places where mostly ponts are vsble, but other places where mostly lnes are vsble and trackable (structured scenaros for example). An algorthm that s able to explot both types of landmarks s therefore n advantage n front of sngle-type ones. The use of lne segments s very promsng, but yelds a wreframe model that remans prelmnary. Further post-processes can allow the buldng of a whole surfaces model, e.g. by collapsng pont landmarks or by usng homographes to verfy plane hypotheses generated on the bass of pars or trplets of coplanar segments. Fnally, much work remans to be done at the control level n order to effcently deploy these mappng processes among a fleet of heterogeneous robots. For nstance, creatng a new submap after every detected loop closure becomes totally neffcent when GPS s avalable, and besdes sharng graphs every-tme a connecton between robots occurs, one must defne what addtonal nformaton has to be transmtted e.g. whch maps should be assocated to trgger a loop closure? Mappng s an actve process that must be controlled and supervsed; ths s even more necessary when t s dstrbuted among several robots.

20 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () Acknowledgments Ths work has been partally funded by the Acton Project (acton.onera.fr). Cyrlle Berger s funded by a Cfre contract wth Thales Optroncs. We would lke to especally thank the Onera Ressac team for all ther help for the experments. References [] C. Estrada, J. Nera, J. Tardós, Herarchcal SLAM: real-tme accurate mappng of large envronments, IEEE Transacton on Robotcs (5) [] T. Vdal-Calleja, C. Berger, S. Lacrox, Even-drven loop closure n mult-robot mappng, n: IEEE Int. Conf. on Intellgent Robots and Systems, Sant Lous, USA, 9. [3] A.I. Mourks, S.I. Roumelots, Predctng the accuracy of cooperatve smultaneous localzaton and mappng (C-SLAM), Internatonal Journal of Robotcs Research 5 (6) [4] X. Zhou, S. Roumelots, Mult-robot SLAM wth unknown ntal correspondence: The robot rendezvous case, n: IEEE Int. Conf. on Intellgent Robots and Systems, Bejng, Chna, 6, pp [5] C. Estrada, J. Nera, J. Tardós, Fndng good cycle constrants for large scale mult-robot SLAM, n: Proceedngs of the Internatonal Conference on Robotcs and Automaton, Kobe, Japan, 9, pp [6] S. Thrun, Y. Lu, Mult-robot slam wth sparse extended nformaton flers, n: Internatonal Symposum on Robotcs Research, Sena, Italy, 3. [7] E. Nettleton, S. Thrun, H. Durrant-Whyte, S. Sukkareh, Decentralsed slam wth low-bandwdth communcaton for teams of vehcles, n: 4th Internatonal Conference on Feld and Servce Robotcs, Combra, Portugal, 6. [8] R. Smmons, D. Apfelbaum, W. Burgard, D. Fox, M. Moors, S. Thrun, H. Younes, Coordnaton for mult-robot exploraton and mappng, n: Proceedngs of the AAAI Natonal Conference on Artfcal Intellgence, AAAI, Austn, TX,. [9] A. Howard, Mult-robot smultaneous localzaton and mappng usng partcle flters, Internatonal Journal on Robotcs Research 5 (6) [] S. Thrun, A probablstc onlne mappng algorthm for teams of moble robots, Internatonal Journal of Robotcs Research () [] A. Gl, O. Renoso, M. Ballesta, M. Julá, Mult-robot vsual slam usng a Rao- Blackwellzed partcle flter, Internatonal Journal of Robotcs Research 5 (6) [] D. Fox, J. Ko, K. Konolge, B. Lmketka, D. Schulz, B. Stewart, Dstrbuted multrobot exploraton and mappng, Proceedngs of the IEEE (6). [3] A. Howard, G. Sukhatme, M. Matarc, Multrobot smultaneous localzaton and mappng usng manfold representatons, Proceedngs of the IEEE Specal Issue on Mult-robot Systems 94 (6) [4] F. Dellaert, A. Kpp, P. Krauthausen, A multfrontal QR factorzaton approach to dstrbuted nference appled to multrobot localzaton and mappng, n: Proceedngs of the AAAI Natonal Conference on Artfcal Intellgence, Pttsburgh, Pennsylvana, 5, pp [5] A. Bahr, M.R. Walter, J.J. Leonard, Consstent cooperatve localsaton, n: IEEE Internatonal Conference on Robotcs and Automaton, Kobe, Japan, 9. [6] S.I. Roumelots, G.A. Bekey, Dstrbuted multrobot localzaton, IEEE Transacton on Robotcs and Automaton 8 () [7] H.J. Chang, C.G. Lee, Y.C. Hu, Y.H. Lu, Mult-robot SLAM wth topologcal/metrc maps, n: Proceedngs of the Internatonal Conference on Robotcs and Automaton, Kobe, Japan, 9, pp [8] R. Madhavan, K. Fregene, L. Parker, Dstrbuted cooperatve outdoor multrobot localzaton and mappng, Autonomous Robots 7 (4) [9] S.B. Wllams, H. Durrant-Whyte, G. Dssanayake, Constraned ntalzaton of the smultaneous localzaton and mappng algorthm, Internatonal Journal of Robotcs Research (3) [] J.D. Tardós, J. Nera, P.M. Newman, J.J. Leonard, Robust mappng and localzaton n ndoor envronments usng sonar data, Internatonal Journal of Robotcs Research () [] L.M. Paz, P. Jensfelt, J.D. Tardós, J. Nera, EKF SLAM updates n O(n) wth dvde and conquer SLAM, n: IEEE Int. Conf. on Robotcs and Automaton, Rome, Italy, 7. [] J.L. Blanco, J. González, J.A. Fernández-Madrgal, Subjectve local maps for hybrd metrc-topologcal SLAM, Robotcs and Autonomous Systems 57 (9) [3] U. Frese, Treemap: a O(log n) algorthm for ndoor smultaneous localzaton and mappng, Autonomous Robots (6) 3. [4] R.C. Smth, P. Cheeseman, On the representaton and estmaton of spatal uncertanty, Internatonal Journal of Robotcs Research 5 (986) [5] R. Eustce, H. Sngh, J. Leonard, Exactly sparse delayed-state flters for vewbased SLAM, IEEE Transacton Robotcs (6) 4. [6] M. Cummns, P. Newman, FAB-MAP: probablstc localzaton and mappng n the space of appearance, The Internatonal Journal of Robotcs Research 7 (8) [7] C.M. Bshop, Pattern Recognton and Machne Learnng, Sprnger, 6. [8] J. Cvera, A. Davson, J. Montel, Inverse depth parametrzaton for monocular SLAM, IEEE Transacton on Robotcs 4 (8). [9] T. Baley, J. Neto, J. Guvant, M. Stevens, E. Nebot, Consstency of the EKF-SLAM algorthm, n: IEEE/RSJ Int. Conf. on Intellgent Robots and Systems, Bejng, Chna, 6, pp [3] R. Haralck, H. Joo, C. Lee, X. Zhuang, V. Vadya, M. Km, Pose estmaton from correspondng pont data, IEEE Transactons on Systems, Man, and Cybernetcs 9 (989) [3] P. Smth, I. Red, A. Davson, Real-tme monocular SLAM wth straght lnes, n: Brtsh Machne Vson Conf., vol., Ednburgh, 6, pp [3] A.P. Gee, W. Mayol, Real-tme model-based SLAM usng lne segments, n: LNCS proceedngs of the nd Internatonal Symposum on Vsual Computng, Lake Tahoe, USA, 6. [33] E. Eade, T. Drummond, Edge landmarks n monocular SLAM, n: Brtsh Machne Vson Conf, Ednburgh, Scotland, 6. [34] J. Solà, A. Monn, M. Devy, T. Lemare, Undelayed ntalzaton n bearng only SLAM, n: IEEE/RSJ Int. Conf. on Intellgent Robots and Systems, Edmonton, Canada, 5, pp [35] J. Solà, A. Monn, M. Devy, T. Vdal-Calleja, Fusng monocular nformaton n mult-camera SLAM, IEEE Transacton on Robotcs 4 (8) [36] J. Solà, T. Vdal-Calleja, M. Devy, Undelayed ntalzaton of lne segments n monocular SLAM, n: IEEE Int. Conf. on Intellgent Robots and Systems, Sant Lous, USA, 9, pp [37] T. Lemare, S. Lacrox, Monocular-vson based SLAM usng lne segments, n: IEEE Internatonal Conference on Robotcs and Automaton, Rome, Italy, 7. [38] J. Solà, Consstency of the monocular EKF-SLAM algorthm for 3 dfferent landmark parametrzatons, n: IEEE Int. Conf. on Robotcs and Automaton, Anchorage, USA,. [39] A. Bartol, P. Sturm, The 3D lne moton matrx and algnment of lne reconstructons, n: IEEE Computer Socety Conference on Computer Vson and Pattern Recognton, vol., Colorado Sprngs, USA,, pp [4] B. Kamgar-Pars, B. Kamgar-Pars, Algorthms for matchng 3D lne sets, IEEE Transactons on Pattern Analyss and Machne Intellgence 6 (4) [4] A. Bartol, P. Sturm, Structure from moton usng lnes: representaton, trangulaton and bundle adjustment, Computer Vson and Image Understandng (5) [4] G. Klen, D. Murray, Parallel trackng and mappng for small AR workspaces, n: Proceedngs of the 6th IEEE and ACM Internatonal Symposum on Mxed and Augmented Realty, IEEE Computer Socety, Nara, Japan, 7, pp.. [43] P. Faban, V. Fuertes, A. Pquereau, R. Mampey, F. Techtel-Kgsbuch, Autonomous flght and navgaton of vtol uavs: from autonomy demonstratons to out-of-sght flghts, Aerospace Scence and Technology (7) [44] T. Lemare, C. Berger, I.K. Jung, S. Lacrox, Vson-based slam: stereo and monocular approaches, IJCV- IJRR 6. [45] C. Berger, S. Lacrox, DSeg: Drect lne segments detecton, Techncal Report, LAAS/CNRS, 9. [46] P. Pnés, J.D. Tardós, Scalable slam buldng condtonally ndependent local maps, n: IEEE/RSJ Int. Conf. on Intellgent Robots and Systems, San Dego, CA, 7. [47] C. Cadena, J. Nera, SLAM n O(log n) wth the combned Kalman-nformaton flter, n: IEEE Int. Conf. on Intellgent Robots and Systems, Sant Lous, USA, 9. [48] J. Solà, T. Vdal-Calleja, J. Cvera, Impact of landmark parametrzaton on monocular EKF-SLAM wth ponts and lnes, Techncal Report, LAAS-CNRS,. note In preparaton. Avalable: URL /fr/. Teresa A. Vdal-Calleja receved the M.E. degree from the Unversdad Naconal Autónoma de Méxco (UNAM), Mexco Cty, Mexco, the M.S.E.E. Degree from Centro de Investgacón y de Estudos Avanzados del Insttuto o Poltécnco Naconal (CINVESTAV-IPN), Mexco Cty, and the Ph.D. Degree from Unverstat Poltéctca de Catalunya (UPC), Barcelona, Span, n 7. Durng her Ph.D. studes, she was Vstng Scholar wth the Actve Vson Lab, Unversty of Oxford, Oxford, U.K., and the Australan Centre for Feld Robotcs (ACFR), Unversty of Sydney, Sydney, NSW, Australa. In 8, she was a Postdoctoral Fellow wth LAAS-CNRS, Toulouse, France. She was on leave from the Insttut de Robótca Informtca Industral, CSIC-o UPC, Barcelona to ACFR n 6. She currently s a research fellow at ACFR. Her current research nterests nclude vsual SLAM, place recognton, co-operatve aeral and ground vehcles, actve sensng, and navgaton. Cyrlle Berger graduated from École Centrale de Pars and obtaned the Master n Artfcal Intellgence from the Unversty Pars VI n 5. He obtaned hs Ph.D. enttled Percepton of the envronment geometry for autonomous navgaton at LAAS-CNRS n 9, whle beng supported by Thales Optroncs. He s currently on a post-doctoral stay at the Unversty of Lnkopng. Hs man nterests are related to computer vson and robotcs, wth a focus on localzaton and on the buldng of envronment structured geometrc representatons.

21 674 T.A. Vdal-Calleja et al. / Robotcs and Autonomous Systems 59 () Joan Solá comes from Barcelona, Catalona. He obtaned hs Masters Degree n electroncs n 995 from the Unverstat Poltécnca de Catalunya and worked for several years n Barcelona as a desgner of power electroncs equpment for the emergng renewable energes sector. He later started a Ph.D. on robotcs at LAAS-CNRS n Toulouse, France, focusng on vsual smultaneous localzaton and mappng. Durng hs post-doctoral perod at SRI Internatonal and LAAS-CNRS he has moved from vsual odometry to humanod walkng, and recently to nertal sensng and sensor fuson. He recently moved back to Barcelona and entered a small company wth the objectve of buldng a scentfc manned submarne to vst the ocean s depths, m below the sea level. Smon Lacrox graduated as an engneer at École Centrale de Pars n 99. He completed hs Ph.D. n Robotcs at LAAS/CNRS n 995, enttled Moton and percepton strateges for autonomous rover navgaton n un- structured envronments. After a one year post-doc n the Center for Intellgent Machnes, McGll Unversty (Montréal), he joned the Robotcs and Artfcal Group at LAAS/CNRS n 996 as a research scentst. Snce, he anmates the actvtes related to robotcs n a natural unstructured envronment n the robotcs group. He ntated n ths context aeral robotcs actvtes n the lab, and ts current focus s on ar/ground collaboratve autonomous systems. Hs work orgnally concerned percepton and navgaton for autonomous outdoor moble robots; envronment modelng, localzaton, percepton control and exploraton strateges, and has shfted to hgher level decson makng processes, especally wthn dstrbuted robotcs systems, but stll grounded on the realty of robot percepton.