Globally Consistent Mosaicking for Autonomous Visual Navigation

Transcription

1 Globally Consstent Mosackng for Autonomous Vsual Navgaton Ranjth Unnkrshnan CMU-RI-TR Submtted n partal fulfllment of the requrements for the degree of Master of Scence n Robotcs The Robotcs Insttute Carnege Mellon Unversty 5000 Forbes Avenue Pttsburgh, PA 523 September Carnege Mellon Unversty The ves and conclusons contaned n ths document are those of the author and should not be nterpreted as representng the offcal polces or endorsements, ether expressed or mpled, of Carnege Mellon Unversty.

2 Keyords: mosackng, localzaton, mappng, cyclc netorks, constraned optmzaton, fundamental cycles, constrant bass, non-lnear state estmaton, Kalman flter

3 Abstract Moble robot localzaton from large-scale appearance mosacs has been shong ncreasng promse as a lo-cost, hgh-performance and nfrastructure-free soluton to vehcle gudance n man-made envronments. The feasblty of ths technque reles on the constructon of a hghresoluton mosac of the vehcle s envronment. For relable poston estmaton, the mosac must be locally dstorton-free as ell as globally consstent. The problem of loop closure n cyclc envronments that plagues ths process s one that s commonly encountered n all map-buldng procedures, and ts soluton s often computatonally expensve. Ths document nvestgates the problem of map-buldng th observatons havng lo spatal and temporal persstence from sensors havng a short sensory horzon. By explotng the topology of sensor observatons, the problem of mosackng can be formulated as one of constraned optmzaton hose soluton can be obtaned effcently even for the problem scale typcal to the applcaton of nterest. A bass of the space of constrants n spatal relatonshps beteen observatons can be easly extracted. Extrnscally avalable nformaton n the form of survey data can be treated dentcally to these constrants and ncorporated n the map-buldng process. The developed frameork can also be extended to accommodate ncremental constructon for onlne mplementaton. Thess supervsor: Dr. Alonzo Kelly Research Scentst, Robotcs Insttute

4

5 Acknoledgements The ork presented n ths document s the fruton of nnumerable enjoyable dscussons th my advsor, Dr. Alonzo Kelly. Hs nvaluable gudance, endless nspraton and broad perspectve on research have taught me a lot and have greatly shaped the ay I conduct my ork. I have often shed that I had the beneft of orkng th hm much earler n my academc career. Much credt for the softare mplementaton surroundng ths ork goes to the members of the Automated Materal Transport System (AMTS) project at the Natonal Robotcs Engneerng Consortum, n partcular Mke Happold, Bryan Nagy, Patrck Roe and Ethan Frantz, for ther tmely help. Thanks also to my offce mates, Davd Steck, Matt Troup and Surya Sngh, for ther creatve suggestons and advce, and for lendng patent ears to my questons even at odd hours of the day.

6

7 Table of Contents Secton : Introducton Secton 2: Mosac-based Localzaton Secton 3: Implementaton Secton 4: Issues th Mosac Constructon Secton 5: Pror Work Pror mosackng ork Map Buldng n Cyclc Envronments Secton 6: Observatons at an Image Overlap Dfferental Relatonshps Pont Jacoban Compound Pont Jacoban Sequence Jacoban One Overlap Absolute Pose Observer Relatve Pose Observer Compound Pose Observer Secton 7: Lnear Mosackng Total Resdual of a Lnear Mosac Smooth Dstorton of a Lnear Mosac Formulaton based on the Compound Pose Observer Secton 8: Netork Mosackng Loop Analyss Secton 9: Mosackng an arbtrary netork of mages Lockdon Graph Constructon Extracton of Fundamental Cycles Computaton of the Jacoban for Pose Resduals Secton 0: A Constraned Optmzaton Approach Prors for Non-temporally Adjacent Overlaps

8 0.2 Resdual Equatons for all overlaps Uncertanty Modelng Loop Analyss Constrant Equatons Formulaton as a Constraned Estmaton Constraned Estmaton as a Projecton Operaton Secton : Incorporatng Survey Informaton Secton 2: Extenson to an Incremental Mappng Algorthm Secton 3: Results Secton 4: Summary and Conclusons Secton 5: Future Work Secton 6: References

9 Lst of Fgures Fgure : Normal camera mountng on vehcle... 2 Fgure 2: Industral AGV... 3 Fgure 3: Lghtng and Imagng Module... 4 Fgure 4: Mappng rg...4 Fgure 5: Porton of a segment th one consttuent mage hghlghted...4 Fgure 6: Outlne of mosac bult usng only dead-reckonng estmates... 6 Fgure 7: Pont Jacoban... 0 Fgure 8: Compound pont Jacoban... Fgure 9: Sequence Jacoban... 2 Fgure 0: One overlap... 3 Fgure : Compound pose observer... 6 Fgure 2: Total resdual mnmzaton n lnear mosackng... 7 Fgure 3: Sparse structure of feature observer dervatves... 9 Fgure 4: Lnear mosac dstorton... 9 Fgure 5: One-cycled netork of 4 mage segments Fgure 6: Lockdon graph correspondng to the one-cycled netork Fgure 7: To-cycled netork of 5 mage segments Fgure 8: Topology graph correspondng to the to-cycled netork Fgure 9: To-cycled netork of 5 mage segments Fgure 20: Sparse structure of Jacoban of feature observer H and the matrx of Lagrangan dervatves Fgure 2: Mosac th to surveyed fducals and assocated augmented topology graph Fgure 22: Mosac th three surveyed fducals and assocated augmented topology graph Fgure 23: Mosac constructed usng only ntal pose estmates from dead-reckonng, and mosac after consstency enforcement Fgure 24: Outlne of ground truth, observed and corrected trajectores Fgure 25: Plot of mage poston resdual th respect to true orentatons Fgure 26: Image orentaton resdual th respect to true orentaton Fgure 27: Comparson of corrected trajectores...48 Fgure 28: Comparson of mage poston resdual beteen mosacs corrected th and thout feature resdual mnmzaton Fgure 29: Comparson of mage orentaton resdual beteen mosacs corrected th and thout feature resdual mnmzaton Fgure 30: Map of ndoor vehcle gudepath... 50

10

11 . Introducton Navgatng from magery s a common technque n robotcs, and a consderable amount of research nterest has been drected toards usng vson to provde localzaton and moton estmaton capabltes to autonomous vehcles, both on land and n the sea. Mosac-based localzaton s one such technque that s motvated by the fact that many man-made envronments contan substantally flat, vsually textured surfaces of persstent appearance. Images from a donard-lookng camera are used to track vehcle moton over a prevously obtaned hghresoluton mosac of the envronment floor. The problem of vehcle localzaton s then reduced to that of to-dmensonal pose recovery through template matchng. The repeatablty of ths technque reles largely on the pror generaton of an accurate mosac that must be both locally smooth and globally consstent to be relable. Ths requrement s one that s common to all mapbuldng procedures, and typcal solutons to the assocated problem are often computatonally expensve and do not scale ell to the large datasets typcal to the applcaton of nterest. Ths report examnes the nature of geometrc couplng that exsts n the problem of map-buldng, and develops a scalable soluton for mappng large cyclc envronments usng data th lo spatal and temporal persstence. A method for ncorporaton of optonal external survey data nto the map-buldng process s also developed. Fnally, an extenson to a frameork facltatng onlne mplementaton s also presented, along th results usng smulated data as ell as data from real envronments.

12 2. Mosac-based Localzaton When cameras are used for vson-based localzaton, the ablty to render a scene permts navgaton from real-tme magery. Consder the case of a camera mounted on the undercarrage of a vehcle th ts axs perpendcular to the floor mmedately beneath the vehcle. The observed envronment, consstng of the floor of the area navgable by the vehcle, can be represented n ts photorealstc rchness as an appearance model n the form of a mosac. The constructed mosac of the floor s rendered globally consstent and stored n persstent memory. The vehcle subsequently tracks moton over the mosac usng a vsual tracker hch computes camera pose. The mosac thus serves as a map, lnkng observed features to vehcle poston. We thus convert the problem of &DPHUDPRWLRQ )LJXUH 6LPSOHVW FDVH RI D FDPHUD PRXQWHG QRUPDO WR WKH IORRU DW D FRQVWDQWKHLJKWIURPLW%\DVVXPSWLRQ RI IORRU SODQDULW\ DQG PRXQWLQJ SDUDPHWHUV IRUHVKRUWHQLQJ RI WKH LPDJHGRHVQRWRFFXU vehcle localzaton nto a smple nstance of the camera pose recovery problem of computer vson and thereby ntroduce a feld-relevant form of vson-based localzaton. Ths technque dffers from vsual odometry n that consderable effort s expended to create a globally consstent model. It dffers from landmark-based localzaton n that the scene s represented n an conc form rather than as a lst of landmark locatons. The smplcty of the concept underlyng ths technque offers several advantages both n terms of mplementaton as ell as commercal deployment:. The smple scene geometry smplfes processng. Snce man-made floors are predomnantly flat, the scene geometry s knon, and trackng algorthms need not recover shape but only recover vehcle moton. The dstorton of conc features due to moton can also be predcted. 2. The use of globally consstent pror models mparts repeatablty and enables hgher trackng veloctes. Snce e are not trackng pxel regons beteen postons but trackng poston through observed features n a persstent model, defntve locatons can be extracted as long as some part of the model remans n ve. If the pror model s globally consstent, reported poston becomes a one-to-one functon of locaton and the result s nether tme nor pathdependent. Such a system s guaranteed to be as repeatable as the fundamental resoluton of the model used. 3. No nfrastructure such as res or reflectve beacons s requred. Ths makes the system harder to sabotage and easer to nstall. 4. Captal cost s lmted to that of a camera, lghtng, off-lne storage, and a capable processor - hch may be requred for other reasons. 5. Installaton cost s lmted to the labor and tme requred to map the envronment by drvng over all necessary gudepaths or areas only once. 2

13 Our applcaton makes and explots several smplfyng assumptons:. Persstent Appearance: Whle a persstent shape assumpton s commonly made of scene geometry, the use of a persstently stored model of scene appearance assumes that the actual appearance of the scene ll not change sgnfcantly over sgnfcant perods of tme. Exceptons to ths assumpton are common, but the appearance change needs to be sgnfcant and extensve n order to render the present technque noperable. It s also feasble to ncorporate a learnng process to accommodate sloly changng appearance. 2. Substantally Flat 2D Scene: Whle completely general 3D polygon models are certanly )LJXUH,QGXVWULDO$*9 possble, e ll assume that the scene can be represented by a mosac mapped onto a 2D surface. Whle the assumpton of flatness can be completely relaxed n general (e.g. n computer graphcs), e ll assume that the scene s flat enough that self occluson and depth dscontnutes cannot occur. These to assumptons apply, at least locally, to most man-made ndoor and outdoor envronments. Such envronments often consst of flat surfaces punctuated by lne ntersectons th other flat surfaces. 3. Restrcted Camera Moton: Whle arbtrary camera moton s computable, e ll restrct moton to be consstent th beng mounted under a terran-follong vehcle. That s, the camera moves n a plane parallel to the flat scene hle beng orented parallel to the terran normal. Under these condtons, the general problem of renderng the scene s reduced lterally to that of extractng the pxels n the rectangular regon predcted to be n ve. 4. Restrcted Mosac Topology: Whle not necessary n general, e ll confne our attenton to envronments here vehcle moton s restrcted to roadays or gudepaths, rather than regons der than an mage n more than one drecton, except at ntersectons. To do so smplfes consderably the problem of constructng globally consstent mosacs. Confnement to gudepaths s requred for safety reasons n commercally relevant automated guded vehcle (AGV) applcatons. 5. Prmary Poston Estmate: Snce e ll confne the applcaton to that of vehcles, t s useful and not overly restrctve to assume the avalablty of an ndependent estmate of camera moton beteen frames. Ths prmary poston estmate can be used to ncrease relablty, trackng performance and therefore vehcle speed. 3

14 3. Implementaton )LJXUHD/LJKWLQJDQG,PDJLQJ0RGXOH$ FXVWRP VROXWLRQ WR SURYLGLQJ ORZSRZHU HQJLQHHUHG OLJKWLQJ DQG LPDJH FDSWXUH IDFLOLWLHVWKDWFDQEHPRXQWHGXQGHUQHDWKD YHKLFOH )LJXUH E $ FXVWRP PDSSLQJ ULJ HTXLSSHGZLWKDGRZQZDUGYLVLRQPRGXOH IRU LPDJLQJ DQG RGRPHWU\ V\VWHP IRU SURYLGLQJUHODWLYHPRWLRQHVWLPDWHV A custom engneered lghtng and magng module s mounted on the undersde of a mappng rg, and used to capture mages sequentally as the vehcle s drven over the desred gudepath. The mappng rg s equpped th to odometers on the front left and rght heels to provde dfferental-headng based dead-reckonng nformaton and thus prmary vehcle-mage poston and headng estmates from odometry. The mages n each lnear sequence, termed a map segment, are fused to obtan the appearance of a floor sath as de as the vehcle tself. Several )LJXUH 3RUWLRQ RI D VHJPHQW ZLWK RQHFRQVWLWXHQWLPDJHKLJKOLJKWHG 4

15 such segments n combnaton descrbe the appearance of the gudepath netork thn hch the vehcle s desgned to operate. Regons of overlap beteen to segments, or parts of the same segment are dentfed by means of fducals, termed lockdon ponts. Such fducals are ndcatve of ntersecton ponts n the gudepath here one segment connects to another, potentally closng a loop n the gudepath netork. Our current mplementaton provdes a user nterface usng hch a human operator can defne or dentfy such fducals to ndcate potental regons of overlap thn or beteen segments. Plausble extensons to ths process nclude usng dstnct markers to serve as fducals at ntersecton ponts n the gudepath netork and automate the search for them n the captured mages, or removng the need for fducals altogether and attemptng to explot rchness n floor texture to detect prevously vsted regons hle traversng the gudepath netork. 5

16 4. Issues th Mosac Constructon 5m )LJXUH2XWOLQHRIPRVDLFEXLOWXVLQJRQO\GHDGUHFNRQLQJHVWLPDWHV7KHKLJKOLJKWHG UHJLRQLOOXVWUDWHVFORVXUHLQFRQVLVWHQF\LQDSRUWLRQRIWKHWUDMHFWRU\WKDWFORVHVLQRQLWVHOI Map constructon usng a moble robot requres a soluton to the dual problems of recoverng the moton hstory of the robot, as ell as constructng a model of ts envronment based on observatons made durng the course of the moton. For the constructed map, n our case the mosac, to be relable n the context of localzaton, t must satsfy to major condtons:. Local smoothness, hereby the regstraton error n adjacent mages s bounded to some acceptably small value. The resdual n the poston of features common to to temporally adjacent mages can, n prncple, be reduced to a value lmted only by the nose floor and fnte samplng rate assocated th the process of mage generaton. For hgh speed vsual trackng to be possble, t s desrable to have the resdual as lo as possble for the features observed by the tracker to be consstent th ther vsual representaton n the mosac. 2. Global consstency, hereby the poston reported at a partcular locaton becomes nether tme nor path dependent, and s unquely represented n the constructed model. Ths requrement becomes apparent hen the gudepath hose mosac s to be constructed contans loops. Due to the accumulaton of error n dead-reckonng estmates over large dstance, a sngle feature present on the floor s represented n to dstnct locatons n the constructed mosac. Ths renders the poston reported by the vsual tracker erroneous, as the poston becomes dependent on the path taken by the vehcle and nconsstent th the true locaton n the reference frame of the factory. 6

17 These requrements are common to any envronment model used for vsual navgaton, ndependent of the type of sensor used for the observaton. The observatons made by the sensor n mosac-based localzaton, a calbrated donard-lookng camera, dffer from those of other dense sensors lke laser range-fnders and those used n beacon-based localzaton/navgaton systems n that the features observed have lo persstence n both spatal and temporal domans. The problem typcally nvolves the manpulaton of thousands of mages for mappng loop-rch vehcle gudepaths n factores, and demands a tme-effcent soluton to be tractable. 7

18 5. Pror Work 5. Pror mosackng ork The feld of mage mosackng s a relatvely old one, th no dearth of research n automated mosackng or ts applcatons. Several methods have been proposed, ncludng the soluton of a lnear system derved from the collecton of par-se regstraton matrces [3], or the frame-tomosac scheme [0]. Only more recently have near real-tme [2] and globally-consstent solutons emerged [20]. Recently, Kang et al. [2] presented a method usng a graph representaton of the topology of the saths to represent spatal and temporal adjacences. Although the complexty for global regstraton s O(mn) n the scheme, here s n s the number of mages and m s the maxmum degree of a node n the topology graph, the qualty of the resultant mosac seems to depend mplctly on the proxmty of each frame to ts fnal poston, and on the relatvely large number of non-temporal mage overlaps th respect to the number of mages to be mosacked. 5.2 Map Buldng n Cyclc Envronments Consderable research has been done n the feld of real-tme vdeo mosackng of the ocean-floor for navgaton, exploraton and reckage vsualzaton. Gracas and Santos-Vctor [7] present several algorthms based on a projectve geometry frameork, and usng robust matchng technques for frame-to-frame algnment. Work by Flescher et al [6] used teratve smootherfolloer technques to reduce errors accumulated over an mage chan, but no menton as made of the tractablty of ts extensons to netorks. In the publcaton by Roe and Kelly [9] related to constructon of mosacs for our current applcaton, an teratve scheme as used, hereby each segment n turn as arped to confrm to the pose and poston requrements at ts endponts, n that order. Lu and Mlos [5] recognzed the need for a smultaneous soluton n ther ork on automated mappng th a laser range-fnder. They dstngushed spatal relatonshps beteen scan poses nto those suppled by odometry and those estmated from rgorous scan matchng done hen revstng areas. They then solved an overconstraned system of measurements to compute the left-pseudonverse least-squares perturbaton to absolute scan poses to enforce global consstency. The complexty of the method as hoever On ( 3 ) n the number of scans, far too prohbtve for our purposes. Ther scheme also had the defcency of usng hll-clmbng approxmatons that ere very senstve to ntal estmates of poses. Furthermore, ther suggested ncremental mplementaton essentally performed the same computaton on all poses accumulated up to the current tme nstant thout computatonal savngs. Gutmann and Konolge [9] presented a real-tme autonomous mappng system producng accurate metrc maps n large cyclc envronments. A local regstraton step lnked scans n a K- neghborhood of the last scan to generate a locally consstent patch, Loops ere closed ncrementally henever a patch correlaton scheme returned a hgh match score for revsted regons, th lo varance and ambguty. Whle the frst step as of computatonal cost dependng solely on K, the latter as essentally the same On ( 3 ) operaton of Lu and Mlos, th margnal cost reductons acheved by usng sparse (perhaps band) matrx technques. The methodology of perturbaton of absolute pose s one that s common to most relevant lterature that the authors have come across. Lnearzaton of observaton equatons, formulatng 8

19 the state varable to be estmated as a concatenaton of perturbatons to absolute poses, dscards second order effects by defnton. These effects are apprecable for problems of the scale of our mosackng applcaton, because the magntude of changes n absolute poses, unlke relatve ones are large. Furthermore, they contrbute to volaton of the fundamental assumpton n lnearzaton, that the Jacoban of the observaton equatons evaluated at the current (ntal) estmate s approxmately equal to the same evaluated at the true state value. Work by Neman [7] on the problem of smultaneous localzaton and mappng (SLAM) recognzed that observatons of relatve postonal relatonshps beteen landmarks had uncertantes that ere uncorrelated to uncertantes n robot poston. An overcomplete, and possbly nconsstent set of such observatons formed the state vector n a relatve map, that could be updated n tme lnear n the number of observatons. When a consstent map as explctly requred, geometrc projecton flterng as done to repeatedly project the relatve map estmates onto a constrant space descrbed by the topologcal relatonshps beteen the observatons. The constrant equatons ere chosen heurstcally amongst a large but fnte set, such that they spanned a large subset of the observatons. In the context of our applcaton, t s clear that hgh qualty mosacs cannot be constructed thout accountng for the coupled nature of the problem. Here, couplng means that the geometrc error at one overlap s a drect functon of the poston of several mages and an ndrect functon of the poses of all mages n the mosac. Ths document ams to formulate a model for ths couplng, and analyze the memory and computatonal requrements assocated th a tangble soluton to the mosackng problem. A technque s presented for cost-effectvely mappng cyclc netork envronments from lovsblty observatons, that enforces global consstency hle preservng exstng local contnuty to gve usable results. The method orks best n topology here the number of loops s small th respect to the number of observatons, hch n practce s commonly encountered and realstc n areas of lmted vsblty. We descrbe ho the spannng set of constrant equatons requred to enforce global consstency can be easly extracted, and present an algorthm for the same. Each loop equaton n ths set corresponds to an element of the fundamental cycle bass of the topologcal graph descrbng the netork of observatons. On the assumpton of good local estmates from odometry, an effcent scalable soluton to the problem of map-buldng n large cyclc envronments s developed for observatons th a short sensory horzon. We then present a formulaton that systematcally relaxes the assumpton of good ntal estmates, hle preservng the lo tme complexty of the prevous algorthm. Our method employs the approach of total resdual mnmzaton beteen observatons, hle ncorporatng knoledge of pror state uncertanty and enforcng topologcal constrants to mantan map consstency. A frameork that facltates ncremental onlne mplementaton s also proposed. 9

20 6. Observatons at an Image Overlap The process by hch mosackng takes place can be expressed as an observer process here features n overlappng mages that should be n the same locaton are actually slghtly msalgned. When ths stuaton s expressed mathematcally, t s possble to solve for the manner n hch the mage locatons can be slghtly changed n order to acheve better algnment. 6. Dfferental Relatonshps Ths secton consders the manner n hch small changes n mage poses affect the regstraton of features hch are common to to or more mages. 6.. Pont Jacoban The Pont Jacoban s a name for the relatonshp beteen dfferental output pose and the parameters of a homogeneous transformaton. It s analogous to the manpulator Jacoban n robotcs. Consder a model frame denoted m and a orld frame denoted. The to frames nvolved can be any arbtrary frames. Let a pont r m be expressed n the model frame. Let the pose relatng the model frame to the orld frame be denoted. We r r m T ρ m a m b m θ m term poses referenced th respect to the orld frame as absolute poses. Let the homogeneous transform correspondng to ths ŷ m absolute pose be denoted T m. For the remander of ths document, the absence of an explct superscrpt ll mply the value s beng ŷ xˆ m expressed th respect to the orld frame. The coordnates of the xˆ pont n the orld frame are gven by: )LJXUH3RLQW-DFRELDQ r m T mr hch can be rtten out as: x y cθ m sθ m a sθ m cθ m b 0 0 x m y m No the Pont Jacoban s the vector partal dervatve: J( ρ m ) x x x m a r m b m θ m 0( x sθm y m cθ m ) ρ m y y y 0 ( x m cθ m y m sθ m ) a m b m θ m 0 ( y b m ) 0 ( x a m ) Ths relatonshp gves the amount by hch the orld coordnates of the pont change n response to small changes n the pose relatng the to frames m and. It s a central relatonshp n mosackng because t quantfes the change n the poston of a pont caused by movng an 0

21 mage a small amount. r J( ρ m ) ρ m 6..2 Compound Pont Jacoban ŷ j r j ŷ ŷ m r m xˆ m Consder no a slght varaton on the above here the model frame s postoned th respect to some ntermedate frame, hch tself s postoned th respect to another arbtrary frame j. Let a pont r m agan be expressed n the model frame. Let the pose relatng the model frame to the ntermedate frame be denoted xˆ a m b m θ m xˆ j )LJXUH&RPSRXQGSRLQW-DFRELDQ We term poses expressed th respect to frames other than the orld frame as relatve poses. Let the homogeneous transform correspondng to ths relatve pose relatng frames m and be denoted j T m, and that beteen frames and j be T. Let the relatve pose of the ntermedate frame th respect to frame j be expressed as. j ρ j j j T a b θ ρ m T The coordnates of the pont n frame j are gven by: r j T j Tm r m hch can be rtten out as: x j y j cθ j sθ j j j sθ a cθ j b j 0 0 cθ m sθ m sθ m a m cθ m b m 0 0 x m y m x j y j cθ m j sθ m j j j sθ m cθ am j j sθ bm + a j cθ m j j j sθ am + cθ bm + b 0 0 x m y m x j y j cθ m j sθ m j x m j sθ m y m j cθ am + + j j sθ bm a x m j cθ m y m j j j + + sθ am + cθ bm + b here θ m j θ j + θ m

22 No the Compound Pont Jacoban s the vector partal dervatve j J( ρ ) x j x j x j j j j m j k a r b θ 0 x sθm y m j j j ( cθ m ) + ( a m sθ b m cθ ) j ρ y j y j y j 0 x m j cθ m y m j j j ( sθ m ) + ( a m cθ b m sθ ) j j j a b θ J ρ j ( ) x j a j x j b j x j k 0 ( y ) r j ρ y j y j y j 0 x j j a j j j a b θ θ j j b j Ths relatonshp gves the amount by hch the coordnates of the pont change n response to small changes n the pose relatng the to frames and j. The relatonshp s mportant n mosackng because t quantfes the results of a small change n the relatve pose of an ntermedate frame Sequence Jacoban The prevous result can be used to represent ho a feature moves hen every mage n a sequence s subject to small changes. We adopt a relatve pose representaton here the pose of every mage s referenced th respect to ts mmedate predecessor n the sequence. Let each mage frame be assgned a unque ndex th numerc value ncreasng n sequence order. Hence the pose assocated th any mage of ndex s a relatve pose of the form ρ. Let the pose of the frst mage n the sequence be referenced th respect to the orld frame. We can consder ths pose of mage 0 th respect to the orld frame as a relatve pose n ths frameork. Consder a stuaton here a feature f s vsble n a partcular mage th assocated frame ndex k. The poston of the feature n the orld frame r r k s gven by: ŷ xˆ ŷ ŷ 2 ŷ k ŷ 0 xˆ xˆ xˆ 2 0 )LJXUH6HTXHQFH-DFRELDQ xˆ k r f 0 T 0 T Tk k 2 k k Tk r The essence of a partal dervatve s to compute the effects of changes n one varable hle holdng any others fxed. Applyng ths prncple here, t s clear that the total Jacoban relatng changes n the feature locaton to changes n 2

23 each ndvdual mage pose th respect to ts predecessor s: r f here J 0 k s the sequence Jacoban and s the sequence pose dfferental. The ndvdual Jacobans are of the form: here the rotaton matrx th θ θ may be nterpreted as convertng the Jacoban of the form seen n Secton 6..2 on page to the reference of frame. ( a, b ) s the poston of the orgn of frame th respect to frame -, and ( x, y ) s the poston of the feature th respect to the frame One Overlap a 0 b 0 θ 0 T a 0 b 0 θ 0 T J 0 J J k J k θ 0 θ 0 T k 2 k 2 k 2 a k b k θ k k a k k b k R 0 k k θ k b 0 ( y ) J R 0 x a cθ R sθ sθ Consder a par of overlappng mages denoted and j. Each mage has ts on model frame poston th respect to the orld frame. Let a feature n the regon of overlap be denoted m. Unless these mages are perfectly regstered, each ll have a dfferent estmate m of here the feature s: ŷ ŷ j j r m ( ρ ) T ( ρ )r m r m ( ρj ) T j ( ρj )r m xˆ xˆ j cθ T J 0 k R 0 k 6.2. Absolute Pose Observer Hence, the resdual, or dfference beteen them s: z m z m r m ( ρ ) r m ( ρj ) T ( ρ )r m j T j ( ρj )r m ŷ xˆ )LJXUH2QHRYHUODS Ths s of the form of the standard observer z h( x) f e generate a state vector by adjonng 3

24 the poses of the to mages nto a sngle state vector thus: x ρ T ρj T T Such a formulaton enables computng a relatonshp beteen the resdual and dfferental changes n ether mage absolute pose. m m Let J and J j denote the pont Jacobans of the feature m th respect to each mage pose respectvely. Then on dfferentatng the above expresson, the change n the resdual can be rtten as a matrx tmes the change n the adjoned state: z m m m J ρ J j ρj H x m J ρ m J j ρ j here the matrces nvolved are parttoned and have been rtten to expose ther component matrces. Ths fnal equaton can be rtten for a number of features n an overlap regon. It s clear that a mnmum number of 2 unque feature ponts are need to over-constran the pose of one of the mages th respect to the other, snce any one feature pont provdes to constranng equatons n ts (x,y) poston. Provded enough unque features are used, the system of equatons can be solved to mnmze the resdual and regster the mages together as close to perfectly as s possble. To do ths, smply rte one equaton for each feature and gather all equatons together nto a large matrx equaton: The notaton means the pont Jacoban relatng changes n the poston of feature m to the assocated changes n the pose of mage. The feature locatons are dstnct so: 2 J J J m x y x 2 y 2... J J 2 J j 2 J j ρ ρ j The mage poses are also dstnct so: J m Jj m By keepng the absolute pose of one of the mages as a reference, the absolute pose of other can be determned, n a least-squares sense, by solvng the system of overconstraned equatons. The advantage of ths formulaton s that changes n the absolute poses of mages are avalable drectly - they do not need to be computed from changes n relatve poses as n the next secton Relatve Pose Observer The resdual can be expressed to depend only on the relatve pose of the to mages n an overlap. 4

25 Recall from above that the resdual, z m We can premultply ths expresson by z m, or dfference beteen feature postons s: r m ( ρ ) r m ( ρj ) z m T ( ρ )r m T T zm j T j ( ρj )r m to express the result n the mage model frame: r m T j ( )r m j ρ j Ths s also of the form of the standard observer z of the to mages: x ρ j h( x) f the state vector s the relatve pose Such a formulaton also enables computng a relatonshp beteen the resdual and dfferental changes n the relatve mage pose. Note that there s no only one mage pose nvolved - the relatve pose relatng j to. Let mage frame functon as the orld frame and mage pose j functon as the model frame. Let J j denote the pont Jacoban of the feature th respect to the pose of mage j relatve to mage. On dfferentatng the above expresson, the change n resdual can be rtten as a matrx tmes the change n state: z m Ths fnal equaton can be rtten for a number of features n an overlap regon. Provded enough unque features are used, the system of equatons can be solved as before to mnmze the resdual and regster the mages together as close to perfectly as s possble. To do ths, smply rte one equaton for each feature and gather all equatons together nto a large matrx equaton: J x y x 2 y 2... J j ρj H x J j J 2 j... The notaton 2 jmeans the pont Jacoban relatng changes n the poston of feature 2 (expressed n the frame of mage ) to the assocated changes n the pose of mage j relatve to mage. The feature locatons are dstnct so: J j J 2 j Ths approach requres an orderng of poses n that each mage, except from one dstngushed mage, ll be postoned relatve to some earler mage. To mages may share the same predecessor but no mage has more than one predecessor. Under these condtons, a relatve pose can be defned for every mage except the root of the predecessor tree. Reverson to an absolute pose representaton can be done by smply assgnng an absolute pose to an arbtrarly chosen base mage, and then sequentally computng absolute poses for each neghborng mage. ρ j 5

26 6.3 Compound Pose Observer Consder no a stuaton here to sequences of mages, each comprsng a knematc chan, are knon to overlap at a gven feature. Let each sequence have a dstngushed frame (p and q n the fgure) assocated th t hose pose s knon th respect to the orld. Ths may be a partcular mage frame or any other frame hch s affxed to the sequence. Let a feature m be vsble n mage of the frst sequence and mage j of the second sequence. Ultmately all mage poses ll be placed n a sngle vector so ther ndces must be dstnct. The pose of the feature m can be computed from each sequence and they are lkely to dsagree: ŷ ŷ p xˆ The resdual, xˆ p ŷ j xˆ xˆ j ŷ ŷ q xˆ q )LJXUH&RPSRXQGSRVHREVHUYHU z m z m, or dfference beteen them s: r m ( R p ) r m ( R q j ) T ( R p )r m p r m ( R p ) T p T ( R p )r m q j r m ( R q j ) T q Tj ( R q j )r m here R p s the vector of relatve poses p of all mages from to p+ and T s the compound homogeneous transform relatng frame to frame p. We can consder the pose of the mage p relatve to the orld as ts relatve pose n ths frameork. j T j ( R q j )r m Ths s of the form of the standard observer z h( x) f e generate a state vector by adjonng the poses of the to mages nto a sngle state vector thus: x R p R q j Such a formulaton enables computng a relatonshp beteen the resdual and dfferental changes n any mage relatve pose n ether sequence. Lets dfferentate the above expresson. Let J p and J q j denote the sequence Jacobans of the feature th respect to each mage sequence respectvely. Then, the change n the resdual can be rtten as a matrx tmes the change n the adjoned state: z m J p R R H x p J q j q j J p J q j R p R q j here the matrces nvolved are parttoned and have been rtten to expose ther component matrces. 6

27 7. Lnear Mosackng Lnear mosacs are the smplest mosac to construct. A smooth lnear mosac can be constructed smply by sequentally mnmzng the feature poston resdual for each overlap regon n the mosac as t s traversed from start to fnsh. Ths problem s solvable n nsgnfcant tme because the unknons are not coupled at all. The problem can hence be parttoned nto smaller ndependent subproblems of number equal to the number of mages nvolved. The lnear mosac, or segment, can then be termed as locally (or nternally) smooth, hereby the maxmum dstorton error due to ncorrect pose fts s bounded to an arbtrarly small value. In practse, a reasonably ell-calbrated dfferental-headng odometer provdes accurate estmates of relatve pose over short dstances. On the platforms used n our project, ths error has been found to reflect on dstorton that s less than a pxel beteen successve mages. The problem of nterest, hoever, s that of the correcton of ths error on ts apprecable accumulaton over large dstances. 7. Total Resdual of a Lnear Mosac ŷ xˆ xˆ ŷ 5 )LJXUH7RWDOUHVLGXDOPLQLPL]DWLRQ LQOLQHDUPRVDLFNLQJ ŷ 5 xˆ 5 A slghtly more dffcult problem than lnear mosackng s the problem of dstortng a lnear mosac so that ts endpont s moved a small amount n poston and orentaton. Consder the stuaton outlned n fgure () here a lnear sequence of mages overlap such each mage contans a porton of the scene n common th both the mage before and the mage after t n the sequence. Let each mage be assgned a unque ndex, and let each mage pose be expressed th respect to the orld frame n an absolute pose representaton. Suppose t s necessary to move mage frame 5 to pose 5 n a manner hch mnmzes the overall resdual of the features at each mage overlap to the exstng smooth lnear mosac. Ths can be accomplshed th the left pseudonverse. Let at least to features be dentfed and ther ne locatons be used to constran the pose of frame 5 relatve to frame 5. For each feature n each overlap, e can express the change n ts resdual as a functon of ts Jacoban, taken th respect to ts absolute pose, and the change n the adjoned pose vector. We defne overlap to be that beteen mages and +. Let z m refer to the resdual of the m-th feature n the -th overlap, and J + m refer to the Jacoban relatng the change n orld coordnates of the m-th feature n the -th overlap, th changes n the absolute pose of mage +. Then the equaton for each overlap feature can be rtten as: z m J + m ρ + J m ρ J J For the features relatng the frames 5 and 5, the resdual of each feature s gven by the dfference of ts poston and a desred orld poston. Alternately the features can be thought of m + m ρ ρ + 7

28 as lyng n an overlap th 5. Then, the change n ther resduals can be related as: z m 5 J m 5 5 ρ 5 J m 5 5 ρ 5 Concatenatng the equatons for all features n a gven overlap gves: z 0 z z m 0 J 0 J J J J m J + m 0 ρ 0 ρ ρ + ρ 5 here m refers to the number of feature ponts n the -th overlap. Wrtng such a set of equatons for all overlaps results n a system of the form: z here x s a column vector of adjoned absolute pose dfferentals, and z s a column vector of adjoned changes n feature resduals. The equatons are overdetermned, and ll therefore be solvable by the left pseudonverse: x If the pose of the frst mage s kept fxed to ts ntal value, and that of the last mage s set to be equal to ts desred value, the frst and last columns of J s reduce to zero and can be removed from the matrx altogether. Ths leaves the H matrx to be of sze 2M 3( N 2), here M s the total number of feature ponts, and N s the number of mages. Memory Usage and Computatonal complexty It may be observed that for a lnear mosac of n mages th n f features per overlap, the H matrx s of sze 2n f n 3n ( ) and contans approxmately n 2n f 2n f n non-zero elements. Wth careful mplementaton, the Hessan H T H s computable th O( n f n ) cost. For lnear mosacs, H T H s of a block trdagonal form th each block of sze 3-by-3. Postve defnte matrces of ths form can be effcently nverted through Cholesky factorzaton n tme sub-cubc n. n H x [ H T H] H T z 8

29 H T H T H H )LJXUH 6SDUVH VWUXFWXUH RI IHDWXUH REVHUYHU GHULYDWLYHV *UD\HG UHJLRQV FRUUHVSRQG WR QRQ]HUR HOHPHQWV RWH WKH EORFNWULGLDJRQDO VWUXFWXUH RI WKH +HVVLDQDSSUR[LPDWLRQH T H Hoever a soluton of ths form does not scale ell to a netork of mages. Overlaps beteen non-temporally adjacent mages ntroduce off-dagonal block elements n the Hessan matrx. These entres are crucal to the generaton of a globally consstent mosac, but destroy the blockdagonal structure of the H T H matrx makng nverson expensve. Ths n turn makes the problem of mosackng several thousands of netorked mages n ths manner computatonally ntensve. The next subsecton attempts to formulate the problem n a manner that utlzes and preserves the contnuty of temporally adjacent mages to reduce the dmensonalty of the problem, hle enforcng consstency at the end-ponts. 7.2 Smooth Dstorton of a Lnear Mosac Suppose t s necessary to move mage ŷ 5 frame 5 to pose 5 n a manner hch causes mnmal overall dstorton to an ŷ xˆ ŷ 5 exstng smooth lnear mosac. We adopt xˆ m 0 xˆ a relatve pose representaton, here the 5 n pose of each mage n the sequence s m' expressed th respect to ts mmedate n' predecessor n the sequence. )LJXUH/LQHDUPRVDLFGLVWRUWLRQ Let to features, m and n, be dentfed and ther ne locatons be used to constran the pose of frame 5 relatve to frame 5. For feature m, e can use the sequence Jacoban from frame to frame 5 to constran the poston of frame 5 as: 2 3 z m J 5 R 5 The second feature pont, n, can be nterpreted as constranng the pose of frame 5. The change n 9

30 orentaton resdual can be expressed as: θ θ n here the terms + on the rght-hand sde represent the change n relatve orentaton beteen 5 successve frames, and the term θ n represents the change n relatve orentaton of the observed feature pont n frame 5, hch s zero snce frame 5 s fxed. The adjoned equatons can be represented as: θ 2 + θ 3 + θ 4 + θ 5 + θ n x m y m θ n J 2 m J m J 5 m R 5 The left subscrpt on the Jacobans s used to denote the fact that the feature coordnates used n the Jacoban come from the feature hose ndex appears. Ths set of equatons s of the form: z H x The equatons are underdetermned, and ll therefore be solvable by the rght pseudonverse: x H T [ HH T ] z The resultng value of x gves the mnmum norm perturbaton to the relatve poses of the mages that s requred to acheve the shft of frame 5 to the desred end frame poston of 5. In ths formulaton, t s mportant that the number of equatons, or the number of ros of H be at most 3, as any more ould result n overconstranng the pose of frame 5, hereby the matrx H ould lose ro rank. Memory Usage and Computatonal complexty It may be observed that for a lnear mosac of n mages, the approxmate sze of the H matrx s only 3 3n 9n elements, hch s far less than that requred by the prevous subsecton. The cost of computng HH T s only On ( ), and beng of sze 3-by-3 can be nverted n constant tme. Hence the order of the hole process s a comfortable On ( ). 7.3 Formulaton based on the Compound Pose Observer An mportant case s here a large number of lnear mosacs overlap at dscrete ntersecton ponts. If the lnear mosacs are already smooth, the rght pseudonverse agan provdes an effcent soluton to the mosackng problem. For ths problem, e: enumerate all ntersectons n the mosac for each ntersecton dentfy the to segments hch have mages partcpatng n the ntersecton for each segment, dentfy the mage partcpatng n the ntersecton and another key mage as far as possble from t thout crossng another ntersecton, hose pose relatve to the orld s knon. The coordnate frame assocated th the key mage s termed the key frame. 20

31 for the mage partcpatng n the ntersecton, locate at least to features hch can also be located n an mage of the other segment. When all of ths s done, e can rte compound pose observers for each feature by projectng the observed feature poston onto all of the relatve poses of both mage sequences. The equatons for one feature look lke: z m R R H x J p p J q j q j Multple equatons are rtten by respectng the unque ndex allocaton of all mages n the netork and placng the ndvdual sequence Jacobans (th the correct sgns) n the correct columns of the overall coeffcent matrx. It s also necessary to shft the base frame th respect to hch a feature resdual s expressed, to one of the mages hch contan that feature, n order to correctly represent the resdual vector to be mnmzed. Ths set of equatons obtaned ll agan be of the form: The equatons are underdetermned, and ll therefore be solvable by the rght pseudonverse: x z If the pose of the orld frame s left out of the state vector x, t should not be necessary to strke an mage pose to preserve nvertblty of the pseudonverse. A major dsadvantage of ths approach s that the key frame of each segment s requred to be suffcently proxmal to ts expected fnal pose, as the formulaton grants t only the same level of freedom of movement th respect to the orld frame as that gven to any other mage hose pose s expressed th respect to ts predecessor. Ths can be rectfed usng a eghted rght pseudo-nverse formulaton, th hch the degree of moton of the key frames th respect to that of other mages can be arbtrarly set by choosng approprate eghts. Ths, hoever, makes the soluton susceptble to naccuraces n the suppled eght values. Solutons obtaned for dfferng eghts, although satsfyng the requred constrants n feature resduals, ll not be the same. Hence a more general scheme for buldng mosacs th multple overlappng sequences s requred. H x H T [ HH T ] z J p J q j R R p q j 2

32 8. Netork Mosackng 8. Loop Analyss The problem of generatng a consstent mosac of a netork of nternally smooth lnear segments can be solved by explotng the topology of the netork to generate end-pose constrants at the ntersectons. By representng the poses of all mages relatve to ther predecessor n a segment, e convert ther representaton nto one that can be used to preserve local contnuty. By adoptng any one mage as our base, and fxng ts frame (or pose), e essentally fx the pose of all other mages that partcpate n an ntersecton th t. Hence, by representng the pose of these partcpatng mages n terms of the relatve poses of all mages, as encountered on traversng any knematc path from the base to the overlap, e can solve for mnmal changes n those relatve poses for a suppled change n end pose, or end pose thereof. Example : One-cycle topology Consder the smple sngle-cycled netork shon n fgure (4). The mages pars (h,a), (b,c), (d,e) and (f,g) form four overlaps. Each mage n a par s regstered to ts counterpart.e. the pose of one mage relatve to the other n each par s knon ether through manual specfcaton (by specfyng the poston and orentaton of a common feature termed a lockdon pont), or through a pror stage of automatcally checkng for and regsterng every par of mages that are n suffcent proxmty of each other to consttute a potental overlap. Each lockdon pont s assocated th ts on coordnate frame, termed a lockdon frame. In the example, the frames are assgned labels L through L4. In our analyss, e shall refer to each overlap as contanng a lockdon pont. Ths does not lead to any loss of generalty, as the case of regstraton of to mages to determne the pose of one th respect to the other can be nterpreted as havng determned a common lockdon pont n the to mages, hose observed poses th respect to each of the to mage frames are related by the prevously determned relatve pose beteen the to mage frames. Let us assume for no that mage ndces correspondng to the letters a, b, c and d are n order of ncreasng magntude. As per the conventon e ll adopt, every mage pose s referenced th respect to ts mmedate predecessor havng a smaller mage ndex. We also adopt the conventon that homogenous transforms represented by loer-case t refer to constant quanttes, or relatonshps beteen mages that are determned a pror by the regstraton process descrbed n the earler paragraph. Transforms represented by upper-case T refer to unknons hch are to be determned. 22

33 xˆ g ŷ f xˆ f ŷˆ g ŷ e ŷ d xˆ e xˆ d 4 g f 3 d e ŷ c xˆ h ŷ a ŷ b ŷ h xˆ a xˆ c xˆ b h 2 a b c ŷ xˆ )LJXUH2QHF\FOHGQHWZRUNRILPDJHVHJPHQWV Hence the homogenous transform relatng frame b to a can be rtten as: T b a a T a+ a+ a+ 2 b T a+ 2Ta + 3 Tb and smlarly for d to c, etc. We refer to such an expresson as a transform chan to descrbe the sequence of homogenous SE(2) transformaton matrces that equate to the pose relatonshp on the left hand sde of the equaton. It s clear that each transformaton matrx n the sequence s an explct functon of the relatve pose descrbed by t. Fgure (5) shos the lockdon graph (LG) correspondng to the above netork. We defne a lockdon graph as a collecton of a vertex set υ, and an edge set ε. Each vertex v υ s composed of a lockdon frame L v unque to each lockdon pont, and a set of subnodes Φ. Each subnode n Φ belongng to a vertex v represents a unque footprnt of the lockdon frame assocated th v,.e. there exsts a subnode correspondng to each mage of the overlap set that contans the lockdon pont. Edges beteen sub-nodes correspond to pose relatonshps beteen the assocated mages as observed through a part of an mage sequence. The LG of a set of mage observatons thus descrbes the topology of the geometrc relatonshps thn the dataset as determned by lockdon ponts common to overlappng non-temporally adjacent mages. From fgure (5), t s clear that the lockdon graph correspondng to the example netork contans one cycle. By traversng the knematc chan correspondng to ths cycle, e can represent the frame of any mage th respect to any arbtrarly chosen base frame n to dstnct ays, correspondng to the to dstnct mage sequence paths that can be taken to that mage startng from the mage contanng the base frame. It s clear that the homogenous transform expressng the base frame relatve to tself, as determned by the transform chan correspondng to the cycle, should equal dentty. 23

34 L4 ŷ f xˆ f f ŷ e xˆ e e L3 ŷ d xˆ g g ŷ g d xˆ d xˆ h L a h ŷ h xˆ a ŷ a b ŷ c c xˆ b xˆ c ŷ b L2 )LJXUH /RFNGRZQ JUDSK FRUUHVSRQGLQJ WRWKHRQHF\FOHGQHWZRUN Mathematcally, or T a a I 3 3 T b a t c b Td c t e d Tf e t g f T h g t a h T a a T b a b L t L2 t 2 L c t L3 t 3 L e t L4 t 4 L g t L t e I3 3 Td c d Tf e f Th g h here the t elements, as defned earler, are knons determned from the lockdon ponts from the four overlaps. Takng the dervatve of the above equaton th respect to all the relatve pose varables yelds 3 constranng dfferental equatons that comprse a system of the form z here x s the augmented vector of change n relatve poses of all the mages n the netork, and z s the observed pose resdual. Ths system s underconstraned and can be solved by the rght pseudo-nverse as x Example 2: To-cycle topology Consder Fgure (6) here an addtonal segment has been ncorporated so as to generate another fundamental cycle n the lockdon graph. Ths cycle addton generates another equaton of the form of that n example. H x H T [ HH T ] z 24

35 xˆ g ŷ f xˆ f ŷ k xˆ l ŷ l xˆ k ŷ e ŷ d xˆ e xˆ d g f 4 6 k l 3 d e ŷ g xˆ h ŷ a ŷ xˆ j ŷ c ŷ b ŷ h xˆ a ŷ j xˆ xˆ c xˆ b h j 5 2 a b c ŷ xˆ )LJXUH7ZRF\FOHGQHWZRUNRILPDJHVHJPHQWV Let the overlaps formed by the pars (,j) and (k,l) contan lockdon ponts th assocated frames L5 and L6 respectvely. Hence the equaton set conssts of: T a a T a L t L5 t 5 L j t L6 t 6 L l t L4 t 4 L g t L t e I3 3 Tk j k Tf l f Th g h as ell as the equaton T a a T b a b L t L2 t 2 L c t L3 t 3 L e t L4 t 4 L g t L t e I3 3 Td c d Tf e f Th g h as before, yeldng a total of 6 constrant equatons n poston and orentaton. Dfferentatng ths equaton system as before yelds an underconstraned system of equatons n dfferental relatve pose that can be solved as before usng the rght pseudo-nverse. It may be argued that the lockdon graph contans a total of 3 cycles th sequences gven by (a-j-k-l-f-g-h-a), (a-b-c-d-e-f-g-h-a), and (-b-c-d-l-k-j-); and that equatons are to be rtten for all three. In fact, the equatons correspondng to the knematc chans descrbed by the three cycles do not form an ndependent set. Mathematcally, the H matrx formed by usng all three equatons ould be rank-defcent n ts ros, and hence the rght pseudonverse ould not exst. Resultngly, any to of the three cycles may be chosen n formng the H matrx for the system to be solvable. In other ords, the set of cycles that may be used must form an ndependent and complete cycle cover - a set that, n graph theory, s termed a fundamental cycle bass. 25

36 9. Mosackng an arbtrary netork of mages 9. Lockdon Graph Constructon Gven the mage locaton of lockdon ponts n each segment, constructon of the lockdon graph correspondng to the netork s smply as descrbed belo: Start th empty graph For each segment s Set lastseenimageid lastseenlockdonid - For each mage th ndex If mage does not contan a lockdon pont, proceed to next mage n segment If there does exst a vertex th ndex of the current lockdon pont Create a ne vertex th ndex of the current lockdon pont Add a subnode th current mage number and segment number to the lst of subnodes of the current vertex If (lastseenimageid! -) Create an undrected edge to the vertex th ndex lastseenlockdonid and th subnode n the current segment. Set lastseenimageid, and lastseenlockdonid currentlockdonid 9.2 Extracton of Fundamental Cycles There s consderable amount of lterature on fndng a fundamental cycle bass of a gven graph, gven ts several applcatons, ncludng solvng electrcal netorks [], processng of survey data [2] and others. Deo et al [4] have descrbed several polynomal-tme heurstc algorthms for generatng a set of fundamental cycles n a graph and have analyzed ther performance on the bass of mean fundamental-cycle-set length and executon tme for a number of graphs. In our applcaton e smply use a BFS routne, modfed along the suggestons n [4], to fnd a cycle bass for graphs, hch n our applcaton are permtted to have multple self-loops and multple edges beteen vertces. If e eght the edges of a lockdon graph by the number of mages consttutng the part of the segment beteen the to lockdon ponts (or more approprately, the subnodes on hch the to lockdon ponts le), t can be seen that the amount of computaton nvolved n our mesh-analyss algorthm s proportonal to the length of the cycle set. Unfortunately, Deo et al [4] proved the problem of fndng a spannng tree hose fundamental set of cycles has total shortest length to be NP-complete. Horton [] descrbed a polynomal tme algorthm for fndng the shortest cycle bass of a graph th m number of edges and n vertces n Om ( 3 n) operatons, th a formdable orst case of On ( 7 ). The analyss n [] as restrcted to 2-connected graphs thout loops or multple edges. Snce then, Thomassen [24] ent on to prove the problem of fndng a cycle cover of smallest total length for an arbtrary graph to be NP-hard. The soluton to least norm perturbaton s, hoever, not dependent of choce of cycle bass, snce the lnearzed equatons correspondng to any cycle bass form an ndependent set n the same space of constrant equatons. The extracton of the smallest bass s therefore not crucal to the algorthm, and s not pursued. 9.3 Computaton of the Jacoban for Pose Resduals Consder a fundamental cycle L represented by a sequence of lockdon pont ndces, say, L L 2 L L j L n L. Consder the porton of the chan beteen lockdon ponts L and L j. Let 26

37 both lockdon ponts have a footprnt on mages n a common segment s, n mages and respectvely. The expresson relatng the pose of th respect to ll then be of the form or dependng on hether k < k j or k > k j respectvely. (Recall our conventon that the pose of every mage s expressed relatve to ts mmedate predecessor n the same segment, havng an mage ndex one less than that of the current mage, and that the pose of the frst mage n each segment s expressed relatve to the orld frame.) Also note that the superscrpt n the constant t terms denotes both the segment ndex s and the mage number. The segment numbers have been dropped n the super- and sub-scrpts of the T terms for clarty. We denote the former case of k < k j as a path of forard traversal, and the latter case as that of backard traversal of the knematc chan. The dervaton of the Jacoban n both cases s elucdated belo. Case A: Forard traversal The knematc expresson contanng s of the form L j s, k ( t L ) k T k k + Tk + 2 sk, ( t L ) k T k k Tk 2 T + + Tkj Tkj k j k j + s, k j ( ) t Lj sk, j ( ) t Lj L k k j or the pose resdual L T T + R Z p Z 0 T LN + R p Q Q 0 R + p + 0 R P p P 0 R Q R + R P R Q R + p + P R Q p +p + Q 0 from hch e get to constrants n poston as: p Z p + R Q and p Z R Q R θ + + p P here and the thrd constrant n orentaton as snθ R + + θ + cosθ + cos snθ + θ Z θ + 27

38 Case B: Backard traversal The knematc expresson contanng + T s of the form.e. the pose resdual hch can also be rtten as L T + + T TLN R Z p Z 0 R Q p Q 0 + R + p 0 R P p P R Q R RP R Q R pp + R Q p +p Q 0 R Z p Z R Q R RP R Q R pp + R Q R p p + + Q 0 From ths, e get to constrants n poston as: p Z p + + R Q R and p Z θ + + R Q R pp R Q R + p + here and the thrd constrant n orentaton as + snθ R + cosθ + cosθ + snθ + θ Z θ + Concatenatng the dervatve terms of all the constrant equatons yelds the underconstraned system of the form descrbed earler, and the dfferental pose ncrement requred s solved by the rght-pseudo nverse. Memory Usage and Computatonal complexty The approxmate sze of the H matrx s 3n l 3n here n l s the number of loops and n s the number of mages. Cost of computng HH T s On 2 3 ( l n ) and cost of nverson s On ( l ). In typcal netorks, n l «n and the computatonal cost of the algorthm for medum to large netorks s bounded by a comfortable On ( ). 28

39 0. A Constraned Optmzaton Approach The prevous secton descrbed an effcent technque for map-buldng n large cyclc envronments for observatons th small spatal feld of ve. Each element of the fundamental cycle bass of the topologcal graph descrbng the netork corresponded to a constrant equaton called a loop equaton. On the assumpton of local smoothness th good ntal poston estmates from odometry, a soluton to the requred perturbaton n relatve pose estmates could be obtaned at lo computatonal cost. Ths secton presents a formulaton that systematcally relaxes the above assumpton, hle preservng the lo tme complexty of the prevous algorthm. The proposed soluton employs a total resdual mnmzaton approach beteen observatons, hle ncorporatng knoledge of pror state uncertanty and enforcng topologcal constrants to mantan map consstency. It s clear that the use of relatve poses to descrbe postons of observatons n the map-buldng problem has several advantages. In moble robotcs applcatons, t provdes a convenent frameork to embed ntal estmates of relatve moton beteen temporally adjacent observatons from a prmary poston estmaton system such as dead-reckonng. Use of relatve poses nherently decouples the covarance n non-systematc error beteen locatons of nontemporally adjacent observatons, so that the ntal state covarance matrx P 0 s of a blockdagonal form. Iteratve least-norm perturbaton of relatve poses to correct map nconsstences can be done cheaply hle fully explotng topology, hle the converson of observaton locatons to an absolute reference frame s postponed tll after the map-buldng process s completed. In a set of observatons hose spatal relatonshps can be descrbed by a tree-lke netork, the problem of recoverng both robot postons and feature locatons becomes trval. Ths s because the state equatons are nherently decoupled, rrespectve of the choce of absolute or relatve poses for representaton, and the state vector can be solved for n tme lnear n the number of observatons. On the ntroducton of non-temporally adjacent observatons, cycles are formed n the topology graph, and the choce of representaton becomes crucal n determnng tme complexty. As seen n Secton 7. on page 7, the choce of absolute poses requres the nverson of a Hessan matrx havng a near block trdagonal structure th off-dagonal elements correspondng to the ponts of loop-closure. Whle ths can be acheved n sub-cubc tme complexty, ncluson of the dense pror state covarance matrx from a model of odometry makes the problem prohbtvely expensve to solve n large scale problems. At frst glance, the choce of relatve poses too offers lttle n the form of favorable matrx structure. On concatenatng feature resdual observatons for tree-lke netorks, the assocated Hessan matrx s block dagonal and nvertble n tme lnear n the number of observatons. Hoever, loop-closng observatons fll the matrx completely. Ths s because the absolute pose of one of the mages partcpatng n the loop-closng overlap has to be expressed n terms of all the relatve poses that precede t n the sequence around a cycle n the topology graph. Ths causes one ro of the Jacoban n the lnearzed observer equaton to fll up, and subsequently the Hessan to be nverted s completely flled. Ths trend of fllng of matrx sub-blocks suggests the possblty of several avenues for computng estmates of relatve pose constraned by the topology of the observatons. One possblty, as analyzed earler, conssts of optonally computng the relatve poses of temporally 29

40 adjacent mages hle gnorng the loop-closng observatons, and then enforcng the topologcal constrants as a least-norm rght-pseudo nverse soluton to the bass loop equatons. The algorthm as tme-effcent but suffered from relance on accurate values of relatve poses beng specfed at non-temporally adjacent overlaps. Other possbltes nclude orkng th a projecton of the state space thn a smaller state space n hch the constrants are automatcally enforced, performng SVD on the entre matrx to compute a pseudo-nverse, and enforcng the condtons as eghted artfcal observatons along th the actual sensor observatons. Hoever, all these approaches destroy the nherent structure of the Hessan matrx and make the soluton prohbtvely expensve. The soluton proposed n ths secton s a Lagrange multpler formulaton of the map-buldng problem, here consstency-enforcng loop equatons are posed as hard constrants n the system. The state vector conssts of elements expressng the change n relatve poses beteen overlappng temporally adjacent mages as ell as those expressng the change n relatve poses beteen overlappng non-temporally mages at loop-closure ponts, and the Lagrange multplers assocated th the constrant equatons. 0. Prors for Non-temporally Adjacent Overlaps Equatons descrbng feature resduals at each overlap are expressed n terms of the relatve pose connectng the to mages partcpatng n the overlap. For nstance, n the overlap beteen mages and j, the resdual of the m-th feature as observed n both mages s gven by z j( m) p ( m, ) j R jp( jm, ) t j here z j( m) R 2 s the feature resdual, p ( m, ) R 2 s the poston of the m-th feature n the -th mage (subscrpt) represented n the coordnate frame of mage (superscrpt), R j s the 2-by-2 rotaton matrx relatng the coordnate frame of mage j th respect to, and t j R 2 s the correspondng dsplacement beteen the to frames. On lnearzng, ths can be expressed as R 2 3 z j( m) J ( jm, ) ρ j here J ( jm, ) s the Jacoban relatng the change n poston of feature m n mage j th change n relatve pose of mage j th respect to mage, and ρ j R 3 represents the change n relatve pose Collectng the terms for feature resduals n non-temporally adjacent overlappng mages z j( m) J ( jm, ) ρ j here j +, gves a subsystem of the form z j M j x j hch can be solved teratvely by the least-norm left-pseudonverse soluton as x j T ( M jmj ) T M j zj Here the matrx to be nverted for each par of overlappng mages s a 3-by-3 matrx, and hence ts computaton s nexpensve. Each such soluton provdes an ntal estmate for the relatve 30

41 pose beteen non-temporally overlappng mages from feature matchng. These obtaned estmates of relatve pose are hence ndependent of the relatve pose estmates of all other mages n the mosac. The values of these estmates are used to evaluate the Jacobans used n the overall system matrx descrbed next. Note that ths formulaton leaves open the scope of usng robust estmators, such as the teratvely reeghted least squares (IRLS) technque commonly encountered n computer vson lterature for outler rejecton. There exsts a large body of lterature on the topc, and further dscusson s avoded th the understandng that the technques can be sutably ncorporated n each step nvolvng feature resdual mnmzaton usng sutable eghtng terms. 0.2 Resdual Equatons for all overlaps Concatenatng resdual equatons for each feature n each overlap gves a system of equatons of the lnearzed observer form z H x here the concatenated vector of change n resdual s z z 0 0 T ( ) z 0( ) z j( m) z N N( M) and the state vector of change n relatve pose s gven by x 0 N ρ ρ2 ρj ρn T Note that the relatve poses beteen temporally adjacent mages have ntal values from odometry, and those for non-temporally adjacent overlaps are from the feature matchng technque descrbed earler. 0.3 Uncertanty Modelng The errors n estmate of feature resduals arse prmarly from mage nose hch lead to naccuracy n specfcaton of correspondng ponts. Ths error can be treated as uncorrelated beteen measurements. We model ths error n a loop-closng overlap beteen mages and j as a dagonal matrx th entres σ 2, hch s our estmate of varance of pxel nose. Then the 3-by-3 error covarance matrx n estmate of relatve pose beteen mages and j at loop-closure s gven by P j σ 2 T ( M jmj ) For temporally adjacent mages, the 3-by-3 error covarance matrx correspondng to ntal estmate of relatve pose from odometry s sutably chosen from an approprate odometry model [3]. Hence the a pror covarance of the state vector error s gven by the block-dagonal matrx as P 0 dag( P 0, P 2,, P j, P N N ) The matrx s block-dagonal and non-sngular, as errors n relatve pose are nherently decoupled, and the uncertanty n estmate of non-temporally adjacent relatve poses s derved solely from feature matchng and not from the poses of other mages n the cycle. P 0 3

42 0.4 Loop Analyss xˆ xˆ f g ŷ f ŷ g ŷ k xˆ l ŷ l xˆ k ŷ e ŷ d xˆ e xˆ d ŷ f xˆ f xˆ g g ŷ ˆ g f ŷ l l ŷ k xˆ l k xˆ k ŷ e xˆ e e ŷ d xˆ d d ŷ xˆ h ŷ h xˆ a ŷ a ŷ j ŷ xˆ j xˆ ŷ c ŷ b xˆ c xˆ b xˆ )LJXUH D 7ZRF\FOHG QHWZRUN RILPDJHVHJPHQWV xˆ h a h ŷ h y xˆ a a ŷ j ŷ j b ŷ c c xˆ b xˆ c ŷ b )LJXUHE7RSRORJ\JUDSKFRUUHVSRQGLQJ WRWKHWZRF\FOHGQHWZRUN xˆ j xˆ Consder the to cycled netork shon n fgure (7a), composed of 5 overlappng segments. The mage pars (a,h), (,j), (b,c), (d,e), (k,l) and (f,g) form 6 non-temporally adjacent overlappng mage pars. Let us assume that ndces a through h are n ncreasng order of magntude. As per our conventon, every mage pose s referenced th respect to ts mmedate predecessor havng a smaller mage ndex. Transforms represented by upper-case T refer to unknons to be determned, but hose ntal estmates are avalable ether from odometry, n the case of temporally adjacent mages, or from feature matchng descrbed earler, n the case of overlaps beteen non-temporally adjacent mages. A subscrpt ndex refers to the frame beng referenced, and a superscrpt ndex refers to the frame th respect to hch the reference s made. Hence the homogenous transform relatng coordnate frame of mage b to that of mage a s gven by the transform chan T b a a T a+ a+ a+ 2 b T a+ 2Ta+ 3 Tb Fgure (7b) shos an alternate representaton of the netork, termed a topology graph (TG), havng the property of havng the same number of cycles as the orgnal netork of observatons. We defne a topology graph as a combnaton of a vertex set υ, and an edge set ε. Each vertex v υ s composed of a set of sub-nodes Φ. Each sub-node n Φ belongng to a vertex v represents a unque footprnt of features assocated th v,.e. the mages correspondng to each subnode of a vertex consttute non-temporally adjacent overlaps th common features. Edges beteen sub-nodes correspond to observed pose relatonshps. Lke the lockdon graph, the topology graph serves as an ntermedate representaton of the geometrc relatonshps beteen mage observatons for the purpose of extractng constrants mpled by them. The major dfference beteen the lockdon graph and the topology graph s the omsson of lockdon frames n the latter representaton. In the ne representaton of state, e desre to nclude relatve pose terms beteen overlappng non-temporally adjacent mages n loop-closng overlaps thout makng a reference to the actual features n common to the overlappng mages. In the topology graph, each vertex corresponds to one such overlap, th the 32

43 mages partcpatng n the overlap appearng as subnodes of the assocated vertex. The subnode of one mage n the overlap s marked as a key subnode. The addtonal relatve poses consdered for the overlap are the poses of all other mages referenced th respect to the key mage/subnode. Ths markng scheme enables us to avod redundant ncluson of all relatve pose relatonshps beteen overlappng mages, hle at the same tme nclude a mnmal but representatve number of non-temporally adjacent relatve poses for each overlap. These selected relatve poses are augmented to the state vector of temporally adjacent relatve poses to form the fnal state vector for the map. An algorthm for constructng ths graph for an arbtrary netork of mages s as follos: Start th empty graph For each segment s Set lastseenimageid lastseenlockdonid - For each mage th ndex If mage does not contan a lockdon pont, proceed to next mage n segment Set value of currlockdonid to the ndex of the lockdon pont contaned by the current mage If vertex th ndex currlockdonid exsts Create subnode th ndex and add t to the subnode lst of the vertex else Create vertex th ndex currlockdonid Create subnode th ndex and add t to the subnode lst of the vertex Mark subnode th ndex to be the key subnode for the vertex, th respect to hch the relatve poses of all other subnodes of the vertex are to be referenced. If (lastseenimageid! -) Create an undrected edge to the vertex th ndex lastseenlockdonid and th subnode n the current segment. Set lastseenimageid, and lastseenlockdonid currentlockdonid 0.5 Constrant Equatons As th the lockdon graph, the homogenous transform expressng a base frame relatve to tself, as observed along any cycle n the topology graph, say (a-b-c-d-e-f-g-h-a), should equal the dentty. Mathematcally, T a a I 3 3 T b a Tc b Td c Te d Tf e Tg f Each equaton s a functon of the relatve poses beteen mages partcpatng n the assocated cycle. The set of such equatons consttutes the constrant equatons that are to be satsfed for consstency. In our example, there are three such equatons correspondng to the three possble cycles (a-b-c-d-e-f-g-h-a), (a--j-k-l-f-g-h-a), and (-b-c-d-l-k-j-) n the graph. The lnearzed forms of any to of these three equatons consttute an ndependent bass n the space of constrants. 0.6 Formulaton as a Constraned Estmaton Let each equaton n the chosen set of L constrant equatons be of the form g l ( X) b l here X s the concatenated set of relatve poses beteen temporally adjacent mages and non- T h g Ta h 33

44 temporally adjacent overlappng mages. The maxmum lkelhood soluton to X s then a mnmzer of the Lagrangan hch can be represented n matrx form as JXλ (, ) --z( X) T zx ( ) -- ( X Xˆ ) T + P 2 2 o ( X Xˆ ) + λ T [ gx ( ) b] The frst term n J penalzes the sum of square resduals of feature observatons n all overlaps, hle the second term penalzes nverse covarance eghted perturbatons of relatve poses from ther estmates. These to terms together consttute a full non-lnear optmzaton that facltates lnearzaton of the estmaton problem at every step usng all avalable observatons and the current best estmate of the entre hstory of the robot trajectory. The thrd term contanng the vector of Lagrange multplers λ enforce the loop equatons as hard constrants that are to be met by the posteror estmate upto frst order accuracy. The true X s a combnaton of the estmate value Xˆ and a perturbaton X. Hence, the cost functon can be rertten as: Lnearzng gves Dfferentatng to extract the Lagrange condtons at pont of constraned mnma, gves J H T zxˆ ( ) H T T + H X + P X o X+ G λ 0 and JXλ (, ) J( X, λ) J( X, λ) T -- z 2 j( m), j z j( m) hch can be rtten n matrx form as m -- ( ρ 2 j ρˆ j) T + P j ( ρ j ρˆ j) + j, --z( Xˆ + X) T zxˆ ( + X) 2 T λ l [ gl ( X) b l ] Note that ths s smlar to the general form of the Varable State Dmenson Flter (VSDF) [6], and that recursve parttonng can be used to take advantage of the sparseness of the system L l -- X T T + P 2 o X + λ [ gxˆ ( + X) b] -- [ zxˆ ( ) + H X] T [ zxˆ ( ) + H X] -- X T T + P 2 2 o X + λ [ gxˆ ( ) + G X b] J( X, λ) -- zxˆ ( ) T zxˆ ( ) 2z( Xˆ ) T H X X T H T [ + + H X] -- X T + P 2 2 o X + λ T [ gxˆ ( ) + G X b] J λ H T H+ P o G T gxˆ ( ) + G X b 0 G 0 X λ H T zxˆ ( ) b g( Xˆ ) 34

45 H T H+ P 0 G T H G 0 )LJXUH6SDUVHVWUXFWXUHRI-DFRELDQRIIHDWXUHREVHUYHU+DQG WKHPDWUL[RI/DJUDQJLDQGHULYDWLYHV matrx to be nverted. By frst solvng for λ n GH T ( H+ P o ) G T λ [ gxˆ ( ) b] GH T ( H+ P o ) H T zxˆ ( ) the soluton for X can be obtaned by solvng H T ( H+ P o ) X H T zxˆ ( ) G T λ Note that the matrx H T H+ P o s block dagonal th a block sze of 3, and can hence be nverted n tme lnear n the number of mages. The above update s performed teratvely untl convergence s acheved accordng to some sutable crteron. The Levenberg-Marquardt procedure can be used to modfy ths update rule by addng a value ε to the dagonal elements of the lnearzed system before nverson. Ths value of ε s ncreased to smoothly stch toards gradent descent, or decreased to stch toards Gauss-Neton update, dependng on hether the value of the cost functon J ncreases or decreases respectvely. Computatonal complexty The state vector X conssts of n relatve pose terms beteen n temporally adjacent mages, and n k terms correspondng to k non-temporally adjacent overlaps. We abbrevate the total number of relatve poses n the state vector to be n. Let the total number of loops be l, and the average number of features consdered n each overlap be p. Wth careful mplementaton, t can be shon that λ s computable n Onp ( + nl+ nl 2 + l 3 ) tme. Subsequently X can be computed n O( np + nl) tme. Hence, the computatonal complexty of each teraton of the algorthm s bounded by Onp ( + nl+ nl 2 + l 3 ) here typcally l«n 0.7 Constraned Estmaton as a Projecton Operaton An nterestng observaton may be made by settng the Jacoban matrx H to be zero n the lnearzed Lagrange cost functon. Ths corresponds to performng a batch-step consstng purely of topologcal constrant satsfacton th knoledge of pror state error covarance. The P 0 35

46 resultng system of equatons then takes the form P 0 G T G 0 X λ 0 b g( Xˆ ) Solvng for X, e get X P 0 G T ( GP 0 G T ) [ b g ( Xˆ )] Ths s exactly the eghted form of the Moore-Penrose nverse, or least-nverse-covarance eghted norm rght pseudo-nverse soluton. so that the ne estmate becomes X' Xˆ + K [ b g( Xˆ )] here K P 0 G T ( GP 0 G T ) Ths s the non-lnear constraned estmator form of the Extended Kalman flter, here K s the Kalman gan matrx of the system. The geometrc nterpretaton of ths operaton, as ponted out by Neman [7], s that the posteror estmate X' s obtaned by projectng the pror estmate Xˆ onto a hyper-plane approxmaton or flat of the true constrant surface gx ( ) b computed at Xˆ. Due to lnearzaton assumptons, the constrant equatons are met to frst-order accuracy by the posteror estmate. No consder the orgnal cost functon th non-zero H. The expresson for λ s obtaned by solvng GH T ( H+ P o ) G T λ [ gxˆ ( ) b] GH T ( H+ P o ) H T zxˆ ( ) On substtutng the expresson for λ, the soluton for X takes the form X [ I KG] H T [ H+ P 0 ] H T zxˆ ( ) + K[ b g( Xˆ )] here K H T [ H+ P 0 ] G T GH T H P [ [ + ] G T ] 0 Recall that the estmate of requred perturbaton n relatve pose solely for feature resdual mnmzaton s gven by the eghted left pseudo-nverse soluton X r H T [ H+ P 0 ] H T zxˆ ( ) th error covarance of X r gven by P r H T [ H+ P 0 ]. By nspecton, t can be seen that the constraned estmate of requred perturbaton n relatve pose s of the form X [ I KG] X r + Kb [ gxˆ ( )] hch s the constraned estmator form of the Kalman flter, th K P r G T [ GP r G T ] beng the Kalman gan matrx. 36

47 Hence the Lagrange formulaton of the map-buldng problem s a constraned estmator that. projects the feature resdual vector zxˆ ( ) nto the column space of H to yeld a least-squares unconstraned left pseudo-nverse estmate of perturbaton X r, and 2. projects the unconstraned estmate X r havng covarance P r onto a lnearzed hyperplane approxmaton of the non-lnear constrant surface gx ( ) b. Ths sequence of steps thus provdes a geometrc nterpretaton of constraned optmzaton n the map buldng process. 37

48 . Incorporatng Survey Informaton ŷ d xˆ e ŷ e xˆ e ŷ e xˆ d ŷ d xˆ d ŷ 2 xˆ 2 ŷ 2 xˆ 2 xˆ f ŷ a ŷ xˆ ŷ c ŷ b xˆ f ŷ f ŷ xˆ ŷ c xˆ c ŷ f xˆ a xˆ c xˆ b xˆ aŷ a xˆ b ŷ b ŷ xˆ )LJXUH0RVDLFZLWKWZRVXUYH\HGILGXFLDOVDQGDVVRFLDWHG DXJPHQWHG WRSRORJ\ JUDSK &RRUGLQDWH IUDPHV DQG FRUUHVSRQGLQJWRWKHVXUYH\HGPHDVXUHPHQWVDUHVKRZQ It s often requred to ncorporate pror nformaton n the map buldng process, e.g. data from manual surveyng, n order to algn nternally assocated reference frames n the map th some convenent external reference. Ths secton examnes the case here surveyed measurements of poston and headng are made at fducals that are observable by the robot. The measurements are assumed to be made th respect to an external orld reference frame that s not drectly knon to the map-buldng algorthm. Each poston n the robot trajectory s represented by a relatve pose th respect to the poston at the prevous tme nstant. Wthn ths frameork of relatve poses, map nconsstences are resolved subject to 3 gauge freedoms - 2 n translaton along the orld X/Y axes and n rotaton. When one surveyed measurement s suppled, the mage contanng the surveyed fducal can be assgned the approprate value of absolute pose to confrm th the measurement. Absolute poses of other mages temporally adjacent to ths mage can then be set n accordance th the relatve poses obtaned through loop-analyss after the nconsstences have been resolved. Consder the map llustrated n fgure (9). It conssts of 3 segments overlappng at 3 locatons to form one loop. To surveyed ponts labeled and 2 are also shon, along th ther assocated frames. The labeled ponts are ncorporated to form the augmented topology graph shon on the rght. The dfference beteen the augmented graph and the topology graph seen earler s that nodes n the augmented graph can correspond to free (or relatve) lockdon ponts or surveyed ponts. Assume that the surveyed measurements of frames and 2, as represented th respect to the (as 38

49 yet) unknon orld frame, are gven by ρ l x l y and ρ 2 l θ l x2 l y2 l θ2 respectvely. These constrants can be equvalently represented as the combnaton of a constrant n absolute poston of frame, n conjuncton th a constrant n relatve poston of frame 2 th respect to frame..e. ρ l x l y and ρ 2 l θ here the vrtual relatve measurements are easly computable through the relaton: l x2 l y2 l θ2 l x2 l y2 l θ2 ( l x2 l x ) cosl θ ( l y2 l y ) snl θ ( l x2 l x ) snl θ + ( l y2 l y ) cosl θ l θ2 l θ The relatve poston can be expressed n terms of the relatve poses n the map, as a sequence of transformatons leadng from frame to frame 2. Ths sequence s expressed as a path equaton, as the relatve transforms n such a sequence ould le on a path n the lockdon graph startng from node and endng at node 2. Ths path equaton consttutes the extra set of constrants n relatve poston and headng of frame 2 th respect to frame, and s a functon of the relatve poses of all the mages consttutng the path. Consder for example the path -b-c-2. The correspondng constrant equatons can be rtten as: and ρ 2 T b Tc b T2 c 0 0 θ b θ c b correspondng to to constrant equatons n relatve x-y poston and one constrant equaton n headng, respectvely. Hence by augmentng the matrx of constrant dervatves G, hose ros consst of the Jacoban of the loop equaton, th another 3-ro consstng of the Jacoban of the path equaton above, the system of relatve poses can be solved for as before th Lagrange multplers. Ths s the soluton θ 2 c l x2 l y2 + + l θ2 39

50 ŷ d xˆ e yˆ e xˆ e ŷ e xˆ xˆ d ŷ 3 b ŷ3 ŷ 2 xˆ xˆ 2 xˆ 3 3 ŷ 2 ŷ d xˆ d xˆ 2 xˆ f ŷ f xˆ a ŷ a ŷ xˆ ŷ c ŷ b xˆ c xˆ f ŷ f xˆ aŷ a ŷ xˆ xˆ b ŷ c ŷ b xˆ c ŷ of the augmented system here xˆ )LJXUH0RVDLFZLWKWKUHHVXUYH\HGILGXFLDOVDQGDVVRFLDWHG DXJPHQWHG WRSRORJ\ JUDSK &RRUGLQDWH IUDPHV DQG FRUUHVSRQGLQJWRWKHVXUYH\HGPHDVXUHPHQWVDUHVKRZQ H T H+ P o T G aug G aug 0 X λ H T zxˆ ( ) b g( Xˆ ) G aug G loop G path At the end of the teraton, the converson from relatve to absolute poses s ntalzed by settng frame to that specfed by the survey data. It may be noted that the survey pose for both frame and 2 ll then be conformed to exactly. No consder the stuaton n fgure (20) hen a thrd fducal s surveyed and ts measured poston and headng nformaton s to be ncorporated n the mosac generaton. Ths thrd fducal, labeled 3, s shon n the map above th ts assocated frame, as ell as n the augmented topology graph as node 3. Let the surveyed pose of ths fducal be gven by: ρ 3 l x3 l y3 Ths nformaton can be equvalently represented as a constrant n absolute pose of frame, along th to constrants n relatve pose of frames 2 and 3 th respect to frame. Ths can be l θ3 40

51 rtten as ρ l x l x2 l y and ρ 2 l and ρ y2 3 l θ l θ2 l x3 l y3 l θ3 The constrant n relatve pose of frame 3 th respect to frame can also be represented as a path equaton. If the path to be consdered s -a-f-3, ths path equaton can be rtten as and T a Tf a T3 f θ a θ f a 0 0 correspondng to to constrant equatons n relatve x/y poston and one constrant equaton n headng, respectvely. The G matrx can then be augmented as before, no th to path equatons, and the system of relatve poses can be solved th the Lagrange multpler frameork. 2 It may be noted that there exsts another constrant n relatve pose ρ 3 of the frame 3 th respect to frame 2 (or vce versa). Hoever, gven the equatons expressng ρ 2 and ρ, the relatve pose 2 3 ρ 3 s no longer an ndependent varable. The lnearzed forms of the path equatons constranng ρ 2 and ρ 3 form an ndependent bass for the constrants mposed by the surveyed poses. In fact, the equatons correspondng to any to of these three varables consttute the largest representatve set of constrant equatons mantanng full ro rank n the G matrx. These equatons correspond to the edges of the spannng tree contanng the nodes n the topology graph correspondng to the surveyed nodes. Hence n the general case, for observatons assocated th a topology graph contanng n l ndependent loops, n s surveyed fducals th n total observatons, there are n l pose constrants n loop equatons (global consstency constrants) and n s pose constrants n spannng tree equatons (survey constrants). Both types of constrants are treated smultaneously and dentcally thn the map-buldng frameork. 2 3 It follos that the complexty of the mappng algorthm becomes On ( cn + n c ) here n c n l + n s and n l «n for sensors th lmted FOV. θ 3 f l x3 l y3 + + l θ3 4

52 2. Extenson to an Incremental Mappng Algorthm The ablty to decouple the total resdual mnmzaton and consstency enforcement steps usng a Kalman flter suggests the possblty of an ncremental map-buldng strategy. At each step of ncorporaton of a ne sensor readng there are a fnte number of cases to be evaluated, dependng on the change enforced n the topology graph. Let us assume, thout loss of generalty, that at the current tme nstant, there are n elements n the state vector representng the pose relatonshps beteen observatons n current (consoldated) map. A (possbly empty) subset n d of these n elements ll be a consttuent member of one or more loops n the topology graph, and the remander n elements ll not be a part of any loop. What ths means s that dependences ll exst beteen n d relatve poses, and the relatonshps ll be descrbed by the loop equatons n the current map. The remander n elements ll be ndependent of each other. The state vector can then be parttoned as X x d x here subscrpts assocate the partton th the set of dependent or ndependent elements, and all vectors are taken by notaton to be column vectors. Let the current estmate of state error covarance be gven by P () -. Because of the nature of the partton, ths can be rtten as () P 0 d 0 P P - here P d s a dense 3n d -by-3 n d matrx representng the covarance of the n d elements of relatve poses n the dependent set, and P s a block dagonal 3n -by-3 n matrx th 3-by-3 blocks representng the covarance of the n elements of relatve poses n the ndependent set. P s block dagonal as the random errors n temporally adjacent relatve poses that are not part of a loop are uncorrelated. Let a ne observaton be made, and the state element correspondng to the relatve pose beteen the current observaton and the temporally precedng observaton be x n+ Case A: No feature s observed that s present n a non-temporally adjacent observaton. In ths case there s no change n the topology graph. The current observaton s then smply an extenson of a sequence of observatons hose assocated relatve poses are ndependent. The analyss s done by augmentng the state vector X th a term correspondng to the ne relatve pose and augmentng P th another block dagonal element correspondng to the state covarance of the ne relatve pose, as determned from odometry. Case B: A feature s observed that s present n a non-temporally adjacent observaton th no ne loop formed. Ths happens hen an exstng map s augmented th nformaton obtaned sequentally n a separate mappng run. The relatve poses beteen observatons reman ndependent, but an addtonal node s created n the topology graph at a locaton correspondng to the overlap of the to non-temporally adjacent mages. The analyss s done by augmentng the state vector X th to terms, the frst correspondng to the relatve pose beteen the current 42

53 observaton and the observaton temporally precedng t, and the second correspondng to the relatve pose beteen the to non-temporally adjacent mages partcpatng n the nely formed overlap. P s augmented th a block dagonal element correspondng to the state covarance of the ne relatve pose, as determned from a model of odometry, and another block dagonal element correspondng to the error covarance n estmate of the relatve pose beteen the overlappng non-temporally adjacent mages as determned by the separate feature matchng procedure descrbed earler. Case C: A feature s observed that s present n a non-temporally adjacent observaton and a cycle s formed. Ths has the effect of addng a ne node and a ne edge n the topology graph, hence formng a ne cycle. In ths case, the state vector and covarance matrces are augmented as descrbed n case B, but a separate step of consstency enforcement s carred out. The equaton descrbng the ne constrant corresponds to the nely generated cycle n the graph. The ne element of the fundamental cycle bass n the modfed topology graph can be chosen to be any cycle that contans the nely added edge. As ll be shon, greater computatonal savngs are obtaned by choosng the smallest such cycle. Ths smallest cycle s trvally obtaned by temporally removng the nely added edge, fndng the shortest path beteen the nodes of the edge n the reduced graph, and declarng the concatenaton of the edge and the found path to consttute the shortest cycle. The state vector can then be reordered and parttoned n the follong manner x m X x l x a x b here x m conssts of the n m relatve pose elements that are part of a prevously assmlated cycle n the topology graph,.e. part of a cycle other than the one nely created, x l conssts of the n l relatve pose elements that are part of both, a prevously assmlated cycle n the topology graph, as ell as the nely created cycle, x a conssts of the other, and n a relatve pose elements that are only part of the nely created cycle and none x b conssts of the n b relatve pose elements that are not part of any cycle. It can be observed that before the generaton of the ne cycle, x m and x l together consttuted the elements of the dependent set x d, and x a and x b together consttute the elements of the ndependent set x. The correspondng structure of the state covarance matrx can then be expressed as here and are both block dagonal elements, th each block beng a 3-by-3 matrx. P aa P bb The constraned posteror estmate of the state vector can then be expressed as a projecton onto a lnearzed subspace of the constrant equaton.let the constrant correspondng to the loop equaton be gx ( ) b. Snce g s not a functon of state elements n x m and x b, ts Jacoban th 43

54 0 0 0 P bb respect to the state s then gven by the form G 0G l G a 0 The Kalman gan matrx s then gven by The term to be nverted can be rtten as or T 2 It can be observed that the frst term G l P ll G l can be computed n On ( l ) tme and the second T term G a P aa G a can be computed n On ( a ) tme. Snce S s a 3-by-3 matrx, nverson can be done trvally. The K matrx can then be computed as 2 n On ( m n l + n l +n a ) tme. The updated state estmate s then computed as n a total of P - tme. P P 0 0 mm ml () P lm P ll P aa 0 K S GP (-) G T 0G l G a 0 P (-) G T [ GP (-) G T ] S G l P lm G l P ll G a P aa 0 K 2 On ( m n l + n l +n a ) 0 G l T G a T 0 P mm P ml 0 0 P lm P ll P aa P bb 0 G l T G a T 0 T T [ G l P ll G l + G a P aa G a ] T S P mm P ml P G ml l T P lm P ll 0 0 G l S T P ll G l S 0 0 P aa 0 T G T a P aa G a S P bb 0 0 X (+) X (-) + Kb [ gx ( (-) )] 44

55 The updated state covarance s computed as P (+) P (-) KGP (-) and s computable n O( ( n m + n l + n a ) 2 ) tme. Although expensve, ths computaton need only be performed at ponts of loop closure n the robot s trajectory. 45

56 3. Results )LJXUHOHIW0RVDLFFRQVWUXFWHGXVLQJRQO\LQLWLDOSRVHHVWLPDWHVIURPGHDGUHFNRQLQJ RWH WKH GLVFUHSDQF\ LQ DOLJQPHQW RI WKH FLUFOHG IHDWXUH LQ WKH ORZHU OHIW FRUQHU DW WKH FURVVRYHUSRLQWULJKW0RVDLFDIWHUFRQVLVWHQF\HQIRUFHPHQW Fgure (2) shos results th a toy problem of 22 mages, usng a smulated vehcle movng at 0.5m/s. The actual mages used n ths experment are extracted from a larger aeral photograph. Intal estmates of vehcle pose are obtaned from a smulated dfferental headng based deadreckonng system. Systematc error s ntroduced by corruptng the velocty measurements of the odometers on the left and rght heels th a scale error of magntude equal to 5% of the true value measured. The pcture on the left llustrates the ndcaton of a rghtard drft by deadreckonng measurements, leadng to dstorton and nconsstency n the generated map on traversng a loop. The pcture on the rght shos the map after consstency as enforced th our algorthm. Fgure (22) shos the outlnes of the observed vehcle trajectory and the estmated trajectory after map correcton through consstency enforcement, as compared to the ground truth trajectory. It can be seen that on the scale of the fgure, the corrected and true trajectores are nearly ndstngushable for most of the path. On plottng the mage poston and orentaton resduals n the orld reference frame (Fgures (23) and (24)), t can be seen that the resdual n mage headng s bounded to less than.5 degrees and that no mage devates from ts true poston by more than 6 pxels. Fgures (25)-(27) compare the soluton obtaned th the proposed technque th the soluton that enforces only global consstency by loop closure and does not smultaneously enforce local smoothness through feature resdual mnmzaton on the assumpton of good ntal estmates. Fgure (25) shos the trajectores correspondng to the to solutons, clearly ndcatng the soluton from the constraned optmzaton formulaton to be more n conformty th the true trajectory. It can also be seen that overall mage poston resdual s loered (Fgure (26)), and that mage orentaton resduals are comparable and both acceptably small (Fgure (27)). 46

57 )LJXUH2XWOLQHRIJURXQGWUXWKREVHUYHGDQGFRUUHFWHGWUDMHFWRULHV )LJXUH3ORWRILPDJHSRVLWLRQUHVLGXDOZLWKUHVSHFWWRWUXHRULHQWDWLRQV 47

58 )LJXUH,PDJHRULHQWDWLRQUHVLGXDOZLWKUHVSHFWWRWUXHRULHQWDWLRQ )LJXUH&RPSDULVRQ RI FRUUHFWHG WUDMHFWRULHV ZLWK DQGZLWKRXWIHDWXUHUHVLGXDOPLQLPL]DWLRQ 48

59 )LJXUH &RPSDULVRQ RI LPDJH SRVLWLRQ UHVLGXDO EHWZHHQ PRVDLFVFRUUHFWHGZLWKDQGZLWKRXWIHDWXUHUHVLGXDOPLQLPL]DWLRQ )LJXUH &RPSDULVRQ RI LPDJH RULHQWDWLRQ UHVLGXDO EHWZHHQ PRVDLFVFRUUHFWHGZLWKDQGZLWKRXWIHDWXUHUHVLGXDOPLQLPL]DWLRQ 49

60 )LJXUH0DSRILQGRRUYHKLFOHJXLGHSDWK5HJLRQVRIWKHILQDOPRVDLFDW WKHLQWHUVHFWLRQVDUHKLJKOLJKWHG Fgure (28) shos results th a 70m long vehcle gudepath n our ndoor laboratory, mapped usng our test vehcle th a total of 836 mages. The mosac bult usng only ntal pose estmates from dead-reckonng had 4 ntersectons that ere ntally unresolved. Although our mplementaton of the algorthms presented n ths document ere not geared toards optmal computatonal effcency, t s orth notng that the algorthm n Secton 7. based on total resdual mnmzaton th absolute pose representaton took several hours on a 700Mhz Pentum-III processor, hle the algorthm n Secton 0.6 based on constraned optmzaton took less than 0 mnutes. 50