A Flexible Solution for Modeling and Tracking Generic Dynamic 3D Environments*

Transcription

1 A Flexble Soluton for Modelng and Trang Gener Dynam 3D Envronments* Radu Danesu, Member, IEEE, and Sergu Nedevsh, Member, IEEE Abstrat The traff envronment s a dynam and omplex 3D sene, whh needs aurate models to represent t and relable algorthms to pereve t. Ths paper presents a new model for representng the dynam 3D envronment of the traff sene, the partle based dynam elevaton map enhaned wth gray level nformaton. A new trang algorthm, based on the measurement ues extrated from dense stereovson, s employed for estmatng the state of the proposed model. The multmodal probablty densty of a map ell s state onernng the ell s speed, heght and gray level, s approxmated by a populaton of partles, whh an mgrate from one ell to another usng ther speeds. The partle populaton s updated usng a weghtng-resamplng mehansm based on the partle s ftness wth the measurement data, whh onssts of the raw heghts obtaned from stereovson and the pxels of the graysale left mage. The measurement model used for partle weghtng s desgned by tang nto aount the spefs of the stereovson sensor. The dynam elevaton map trang system s able to provde a dense and aurate representaton of the observed sene, to mprove the densty and auray of the stereo-based envronment perepton, and to enhane t wth dynam nformaton. The fnal result s an aurate vrtual representaton of the pereved 3D sene. I. INTRODUCTION A omplex and dynam 3D sene, suh as the ones enountered n the urban traff, may be hard to model wth orented boxes [1, 2]. Some shapes are not well suted for box representaton, or the olusons and sensor lmtatons prevent a suessful box ft. The traff sene s omposed of obstales, whh are stat or dynam, road surfae, urbs, sdewals, gener 3D strutures that sometmes defy our attempt to model them wth smple geometral prmtves. Many researhers try to overome ths problem by desgnng gener solutons for representaton and trang of 3D envronments, models that are not bound to partular objet types. The smplest approah s to store raw sensoral ponts, and use them to dsrmnate aganst aessble areas and obstales. These ponts an also be used for map buldng, f they are hghly aurate, delvered by a hgh auray hgh densty laser sanner [3]. An ambtous attempt to model and tra the gener dynam 3D envronment s the 6D vson, *Ths wor was supported by a grant of the Romanan Natonal Authorty for Sentf Researh, CNCS UEFISCDI, projet number PN- II-ID-PCE R. Danesu s wth the Tehnal Unversty of Cluj-Napoa, Cluj- Napoa, 114 Romana (phone: ; fax: ; e-mal: radu.danesu@s.utluj.ro). S. Nedevsh s wth the Tehnal Unversty of Cluj-Napoa, Cluj- Napoa, 114 Romana (e-mal: sergu.nedevsh@s.utluj.ro).. eah pont of the sene havng poston and a 3D speed vetor [4]. A more ompat representaton s the dynam stxel set [5], whh models the vsble sdes of obstales as a set of dynam vertal strutures, the speeds beng onfned to the road surfae. For the purpose of navgaton, omplex 3D envronments an be modeled as dgtal elevaton maps (DEM). The map s a 2D grd, eah ell n the grd havng a heght above a globally or loally defned zero level. The DEMs an be large data strutures, when they are used for terran mappng [6], a funton whh maes them extremely useful for planetary exploraton tass [7], but they an also be more ompat strutures, used for the navgaton of autonomous robots [8] or ntellgent vehles [9]. The elevaton map s nature of envronment representaton maes t well suted to be estmated n real tme, usng sensors that are apable of delverng dense 3D data, suh as the lasersanner [2] and the stereovson [9] []. For real-tme navgaton assstane, the ells of the elevaton map an be quly analyzed and lassfed nto traversable, obstales and others [8] [9]. The elevaton map s not a omplete desrpton of the 3D envronment brdges and tunnels, for example, annot be fully represented, and for ths reason ths nd of representaton s sad to be 2.5 dmensonal [11]. Some extensons, suh as the Mult-Level surfae maps [12] or the mult-volume oupany grd [13], offer some worarounds for ths problem, by storng multple heghts for the same map ell. Oupany grds are another way of representng the pereved envronment n the form of a 2D, brd-eye vew map. However, nstead of heghts, the oupany grd stores n eah ell a probablty of ths ell to be ouped by an obstale. Many oupany grd solutons provde a probablst reasonng framewor for usng unertan sensoral data [14], and reent ones an also handle the presene of dynam obstales [2][15]. The elevaton map and the oupany grd both have desrable propertes that mae them useful for drvng envronment representaton and trang, and thus a ombnaton of the two seems to be a natural extenson. In [16], we fnd a stereovson-based soluton whh mantans two maps, one for oupany and one for heght, whle n [17] the elevaton map s used as an ntermedary proessng step towards ahevng the oupany grd representaton.

2 II. OVERVIEW OF THE PROPOSED SOLUTION Ths paper presents a further extenson of the oupany grd/elevaton map ombnatons, a soluton for dret modelng and trang fully dynam elevaton maps. Whle n [12] and [13] we an fnd sophstated tehnques for desrbng omplex stat 3D envronments, and n [16] and [17] the elevaton maps are used along wth oupany grds, ths paper presents a unfed soluton for modelng and estmatng both the stat and the dynam haratersts of the envronment. Eah ell n the dynam elevaton map has a probablty dstrbuton of heght and speed, and these dstrbutons wll be updated aordng to the measurement data, whh s avalable from dense stereo proessng. Also, due to the fat that the stereovson system delvers not only 3D data, but mage gray level data as well, we shall model, for eah ell, a probablty dstrbuton of gray values, whh wll be useful n the state update proess, allowng us to use an addtonal gray level ue, and wll nrease the fdelty of the estmated 3D world model. The proposed dynam elevaton map modelng and trang soluton s based on movng partles. Eah partle has a heght, a speed and a gray value, and s loated n a ell of the map. The partle set of a ell forms a mult-modal probablty dstrbuton over a 4-dmensonal state spae (heght, forward speed, lateral speed, and gray value). The partles an move from one ell to another, provdng an elegant and ntutve mehansm for predton, and an be reated and destroyed based on ther agreement wth the measurement data (the state update). The same partle-based mehansm was used n [18], for modelng and trang dynam oupany grds. However, the movng partle an arry more than poston and speed nformaton. Thus, the soluton desrbed n ths paper s based on partles arryng nformaton about poston, speed, heght and gray level, leadng to a very flexble probablst model of a fully dynam, gray level enhaned elevaton map. The partle populaton s ontrolled by the measurement data, whh onsst of the nstantaneous, stat elevaton map onstruted from dense stereo, usng a method desrbed n [9], and the mage gray levels assoated to the elevaton map ells. The trang algorthm ontnuously estmates the probablty densty of the heght, speed and gray value for eah ell n the map. As these probablty denstes are represented by partles, the purpose of the trang algorthm s to manage the partle populaton, usng the measurement data. A blo dagram of the soluton s presented n Fg. 1. The frst step of the trang yle s the Partle Drft, whh apples moton equatons to the partles past states n order to predt ther poston at the present tme. Ths step auses the partles to move from one ell to another. The drft s followed by the stohast Dffuson, where the parameters of the partles (poston, heght, speed and gray level) are altered by random amounts, refletng the unertantes of the real world transformatons. Together, Drft and Dffuson form the predton. The measurement data s represented by the Raw Elevaton Map, derved from dense stereo data proessng (and affeted by stereo reonstruton spef errors), and by the Graysale left mage aptured from the left amera. The raw elevaton map s ombned wth the graysale mage to produe a Raw Graysale Map, whh assgns a gray level value for eah ell n the raw elevaton map that has a vald heght. Fg. 1. Overvew of the proposed trang algorthm. Due to the fat that the host vehle an pth randomly and sometmes abruptly between observaton tmes, the heght of the predted partles has to be adjusted to ompensate for ths (Pth Compensaton). The pth dfferene s estmated by omparng the predted heghts and the heghts n the raw map, under the assumpton that most of the sene s made up by stat elements, whose heghts should not hange. After the pth-based heght adjustment, the partles are subjeted to Partle Weghtng. The partles are ompared to the measurement data, n terms of heght and gray value. Ths proess must tae nto aount the unertanty of the stereovson proess. For speedup purposes, weght Loo-Up Tables (LUTs) are omputed for eah map ell, for eah possble partle heght and gray level, n the proess of Heght Weght Computaton and Gray level Weght Computaton. Eah partle wll get a heght weght and a gray level weght from the LUTs, and the fnal weght wll be the produt of the two. After weghtng, a new populaton of partles s generated for eah map ell, usng the proess of Resamplng, the weght of the partle nfluenng the hanes of t beng sampled. If a ell n the grd has very few partles (or none), the measurement data, already pre-proessed n the form of a heght weght LUT and a gray level weght LUT, wll be used to reate new partles, wth random speeds, and heght and gray level dstrbutons onsstent wth the LUTs. After the partle populaton s updated, t reflets the updated probablty denstes for the state of the envronment, and the system s ready for a new yle. A dynam elevaton map ontanng an estmated value of the

3 heght, speed and gray level for eah ell s the result of the Estmaton and Output step. III. THE WORLD MODEL Assumng the oordnate system wth the orgn on the ground n front of the host vehle, the axs pontng forward, Y axs pontng to the rght, and Z axs pontng up, the horzontal plane OY s dvded nto ells of m x m, eah ell beng dentfed by a row oordnate r and a olumn oordnate. Eah ell has an assoated heght value h, and a gray level value g. Also, sne the 3D sene we want to model s dynam, eah ell has an assgned speed vetor, whh we ll onfne to the horzontal plane, and therefore t wll have two omponents, for the and Y axes, or, n terms of rows and olumns, v r, (r, ) and v, (r, ) the row speed and the olumn speed for eah ell n the map (Fg. 2). Eah ell n the dynam gray level elevaton map an be desrbed by four values, h, g, v r, and v,. Due to the fat that perfet sensng of a real traff sene s mpossble, one annot have exat nowledge of these values. These lmtatons lead to unertantes, whh have to be nluded n the world model as probablty denstes. A ell n the dynam elevaton map s assoated to a T four-dmensonal random varable h, g, v, v ). ( r,, The objetve of the trang algorthm s to ompute the probablty densty of, for eah ell n the map, based on a sequene of measurements Z() Z(t), t beng the urrent observaton tme. Fg. 2. The dynam, gray level elevaton map. The dynam elevaton map wll be desrbed, at tme t, by a set of partles S(t){x (t) x (t)( (t), r (t), h (t), g (t), v, (t), v r, (t)) T, 1 N S (t)}, eah partle beng loated n the 2D grd, n the ell havng the row r, and the olumn. The grd s a map of 2 rows x 1 olumns, and eah ell n the grd s a retangle of m x m. Eah partle represents a hypothess of the state of the ell: a possble heght h (t), a possble gray level g (t), a possble forward speed v r, (t) and a possble lateral speed v, (t), as depted n Fg. 3. The row, olumn and speed of a partle are expressed as multples of the ell sze D and D Y (urrently m), and the heght s expressed as a multple of the heght element of sze D H (urrently D H 1 m). The probablty densty of the state of a ell s approxmated by the speed, heght and gray level values of the partles loated nsde ell, as shown by equaton (1). P( ( t) Z(), Z(1),..., Z( t)) { x ( t) S( t) r ( t) r, ( t) } Fg. 3. Partle approxmaton of the unertan state of the world. IV. ALGORITHM DESCRIPTION A. Predton Drft and Dffuson The state transton probablty model s mplemented by the determnst drft and the stohast dffuson. The determnst drft hanges the state of the partles by tang nto aount two fators: the movement of the host vehle, whh auses a relatve movement of the whole sene n the vehle s oordnate systems, and the movement of the moble partles, aordng to ther speed. The host vehle s movement an be omputed from ts speed and ts yaw rate, whh are read from the CAN bus and ntegrated over the tme nterval between two measurement frames. A detaled desrpton of ths proess an be found n [18]. After drft, the partles are subjeted to dffuson. The state of eah partle s altered by random quanttes δ (t), δ r(t), δ h(t), δ g(t), δv (t) and δ v r (t), drawn from Gaussan dstrbutons of zero mean and expermentally adjusted ovarane matrx Q (t). B. The measurement data The dense stereovson data s transformed nto a raw elevaton map. A detaled desrpton of the proess an be found n [9]. The raw map s desrbed by two 2D arrays: Measured heght of eah ell, denoted by z (r, ); (1) Measured gray level of eah ell, denoted by b (r, ). The gray level of a map ell s found by transformng the row, olumn and heght oordnates of the raw heght map nto YZ oordnates, and projetng them nto the left mage plane. Data avalablty for eah ell, denoted by d (r, ). d (r, )1 means heght for ths ell s avalable, and d (r, ) means that no measurement data s avalable for ell. A ell may have no vald measured heght due to the fat that the stereovson engne may not be able to provde a 3D value for all areas n the mage, or due to olusons and

4 feld of vew lmtatons. The trang algorthm s made aware of ths stuaton by d (r, ). If the ell has no measured heght, we ll assume that t has no measured gray level, ether. Fgures 4-6 show the stages of the proess of measurement data extraton. Fg. 4. Orgnal graysale left mage (top), and dense stereo reonstruted 3D ponts (bottom). Fg. 5. Measurement data. Left heghts of eah ell (a brghter value means a greater heght of a ell); Mddle gray level values for the map ells; Rght Data avalablty for eah ell whte means measurement data s avalable. Fg. 6. Measurement data, represented as a 3D surfae. The ells wthout data avalablty are shown n green. C. Pth angle ompensaton The world we observe wll not hange abruptly between measurements, therefore the heghts of the map ells are expeted to reman, on the average, onstant. Abrupt hanges of ell heghts are expeted when the host vehle s pthng, the effet of a pth angle varaton α ausng a heght dfferene between the partles n the ell and the measurement data: ( h z ( r, )) D tan 1 D r H α (2) Equaton (2) s vald only when the partle belongs to a stat struture, and ts heght resembles the true heght of the struture n the sene. These ondtons are not vald n the general ase, but we an assume that most of the partles belong to stat strutures and the errors average out. By averagng the pth anddate value of eah partle, a pth dfferene value s estmated for the entre sene. After the pth dfferene value s estmated, the heght of eah partle s orreted. Even f ths orreton s not perfet, as heght hanges may our also from host vehle rollng, and the pth angle may be sometmes norretly estmated, t s enough to ompensate for the most rtal stuatons. D. Partle weghtng The proess of partle weghtng s the partle flter nstantaton of the measurement (observaton) model P( Z( t) ( t) x ( t)). Ths model desrbes the ondtonal probablty densty of the measurement Z(t) gven a possble ell state value ( t) x ( t). A partle s a state hypothess, and the probablty of the measurement gven ths state wll be enoded as a partle weght, whh wll desrbe how well the partle hypothess mathes the measurement data. The raw elevaton map s just a onvenently modfed verson of the stereovson-derved 3D pont data, therefore the probablst observaton model wll be derved from the observaton model of stereovson, a three-dmensonal normal dstrbuton entered n the real world 3D oordnate, and havng a ovarane matrx defned by the dstane error standard devaton, the lateral error standard devaton and the vertal error standard devaton. Y The expeted error standard devatons of the three oordnates omputed by a stereo reonstruton proess depend on the system s baselne (dstane between ameras) b, the foal dstane n pxels f, and dsparty omputaton unertanty (mathng unertanty) d. The error standard devaton for the dstane oordnate an be omputed as [19]: 2 bf (3) d Z

5 The error standard devatons for the lateral oordnate Y and for the vertal oordnate Z, Y and Z, an be omputed usng equatons (4) and (5). Y Y (4) Z Z (5) The unertantes of, Y and Z are then onverted n unertantes of row, olumn and heght: + r D r Y DY h Z DH h (6) (7) + + (8) The offsets r, and h are added so that other measurement errors, besdes mathng unertanty, an be aounted for. These offsets are tuned expermentally. A partle wll reeve two weghts, one whh wll reflet the agreement between the partle heght and the measurement heghts n the raw elevaton map, and the other refletng the agreement between the partle s gray level and the measurement gray levels. Due to the unertantes of the measurement, the best heght and gray level measurement for a real world ell may not be found n the orrespondng measured ell, but n neghborng ells. Thus, for a partle loated n a ell, ts heght and gray level must be ompared to measured values n an unertanty regon of 4 r x4 around the entral poston (r, ). In order to mae ths proess more effent, Loo-Up Tables (LUTs) are reated for eah ell, for eah possble value of heght and gray level, n the proess of Heght Weght Computaton and Gray level weght omputaton. For reatng the heght weght LUT, eah measured heght z nsde the unertanty regon for a ell votes n ts orrespondng heght hstogram poston, but the weght of ts vote wll be generated by a Gaussan ernel Γ entered n (r, ), havng the standard devatons () and ). Ths s a straghtforward applaton of the Gaussan observaton model n the horzontal plane. Formally, the hstogram value for a heght anddate h, at oordnates (r, ), for the ell, s omputed usng equaton (9): H ( h) r r τ r 2 r κ 2 r ( (9) d( Γ ( τ r, κ ) δ ( z( h) The proess of heght hstogram reaton s depted n Fg. 7. We an apply the same reasonng for the measured gray values b(r, ), and reate a weght hstogram G, for eah anddate gray level g, usng equaton (): G ( g) r r τ r 2 r κ 2 () d( Γ ( τ r, κ ) δ ( b( g) Fg. 7. Buldng the heght hstogram for a ell. A anddate heght, loated nsde the stereo unertanty regon, wll have a vote weghted by the value of the 2D Gaussan ernel entered n the urrent ell. In order to aount for the heght unertanty, the heght hstogram H wll be onvolved wth a 1-dmensonal Gaussan ernel K H,, of standard devaton H (), obtanng the LUT for partle weghtng wth respet to heght, W H,. W K * H H, H, (11) Smlarly, the gray level hstogram G wll be onvolved wth a Gaussan ernel K G,, of standard devaton G (), aountng for the unertanty of gray level estmaton (ths nludes uneven lghtng, refletons, et), obtanng the weght LUT for partle s gray levels, W G,. W K * G G, G, (12) After the weght LUTs are reated, the weghts of the partles an be easly omputed. A partle, n a ell, wll get the followng weght: w W h ) W ( g ) (13) H, ( G, E. Resamplng and reaton of new partles In order to update the state of the pereved envronment n aordane wth the measurement data, resamplng s appled for eah ell, after the partles are weghted. The total number of allowed partles for a ell s N C, a onstant whh s urrently set at. The real number of partles n the ell, N R,, resulted by drft and dffuson, may be lower than N C,, but never hgher, as exess partles resulted from drft and dffuson are dsarded. For re-samplng, we ll assume that the ell holds a hgher number of partles, N A 1.25 N C. The dfferene between the real number of partles n the ell, N R,, and the augmented maxmum number of partles N A s the number of empty partles, partles whh are n fat empty plaes. The re-samplng mehansm wll perform the followng steps: 1. Weght the empty partles wth a default low weght, whh we hose to be the average value of the heght weght LUT, W H,, multpled by the average value of the gray level weght LUT, W G,. The number of empty partles multpled by the default weght s the assumed probablty of a omplete msmath between the predted state and the measured data.

6 2. Normalze the weghts of all N A partles so that ther sum beomes Perform N C random extratons from the total partle populaton, real and empty, the weght of the partle ontrollng ts hanes of beng seleted []. The mehansm for dereasng the partle populaton of a ell when most of these partles do not math the measurement data s desgned to speed up the onvergene of the state estmaton n the presene of movng obstales. A movng obstale s partles must replae ground partles, but f a ell s already full wth ground partles, the movng obstale partles have no plae to go, most of them wll be lost, and the movng obstale wll not be suessfully traed. If a map ell has a low number of partles, the measurement data s used to reate new partles to populate the ell. The speeds of the new partles wll be sampled from a normal dstrbuton entered n, the heghts wll be sampled from the mult-modal dstrbuton represented by the weght LUT W H, and the gray levels wll be sampled from the gray level LUT W G,. The proess of reatng new partles s appled after eah resamplng step. V. RESULTS In order to assess the world modelng and trang apabltes of the proposed soluton, multple sequenes aqured n the urban traff of Cluj-Napoa, Romana, were used. In the absene of a defnte ground truth, we made a subjetve evaluaton of how well the trang system was able to provde a vrtual 3D representaton of the pereved envronment, n omparson wth the raw map used as measurement. Some results are shown n Fg. 8. The trang system was able to onsderably mprove the densty of the raw map, to flter out the reonstruton errors, espeally n the ase of the road surfae and to orretly dentfy the movng elements n the sene and ther dreton of moton. a.1. a.2. a.3. a.4. b.1. b.2. b.3. b Fg. 8. Qualtatve results n real urban traff senaros: a.1.-a.4, orgnal graysale mage; b.1.-b.4, raw elevaton map wth graysale nformaton;.1.-.4, traed dynam elevaton map. The apablty of the system to estmate the movng objets speed was evaluated usng sequenes reorded n ontrolled senaros. A test vehle approahes at a 45 degrees angle from the left, passes through the feld of vew, and dsappears towards our rght (Fg. 9). The observed vehle was reorded four tmes, wth four dfferent speeds:,, and m/h. Fg. 9. Test senaro for speed measurement evaluaton. As the sene onssts only of the test vehle and the road, we analyze the speed of the partles that have a heght hgher than m, whh we ll assume to belong to the obstale. From the speeds of the partles, the speed of the movng obstale s estmated. In fgure, the estmated speed (n m/h) s plotted aganst the frame number. The system s able to quly get an estmate of the objet s speed, when the objet beomes vsble, but, as expeted, the hgher the magntude of the speed, the more dffult t s to get the partles to onverge to the orret value. In Table I, the root mean square error (RMSE) of the speed estmaton s shown. The estmated speed s ompared to the ground truth only for the frames where the obstale s n the feld of vew. The numbers onfrm the results shown n the plots, the derease of auray for

7 the hgher speeds. Ths behavor s to be expeted, as the partle populaton of a ell s ntalzed wth random speeds from a dstrbuton of zero mean. Speed (m/h) Speed (m/h) Speed (m/h) Speed (m/h) Fg.. Estmated objet speed, for,, and m/h ground truth speeds. TABLE I. SPEED MEASUREMENT EVALUATION. Ground truth Mean estmated RMSE (m/h) speed (m/h) speed (m/h) VI. CONCLUSIONS AND FUTURE WORK Ths paper desrbes a flexble method for modelng and trang omplex 3D envronments - the partlebased dynam elevaton map enhaned wth gray level nformaton. The entral element of the soluton s the dynam partle, havng poston, heght, speed and gray value, whh an mgrate from one map ell to another. A partle populaton n a ell s a mult-modal probablty densty approxmaton for a 4-dmensonal state spae, an approxmaton that s updated by ontrollng the partle populaton usng the ues obtaned from measurement. The prelmnary results show a system apable of relably estmatng the 3D stat and dynam haratersts of the observed traff senes, able to sgnfantly nrease the densty and auray of the raw, stereovson-extrated elevaton map, and to add dynam nformaton for eah map ell. The future wor wll be foused on nreasng the tme performane of the soluton, as the urrent system s not yet a real-tme algorthm. However, as the system s entral onept s the partle, massve parallelzaton at the partle level and at the ell level an be employed, and the deployment on a CUDA-ompatble GPU an lead to real-tme performane. REFERENCES [1] B. Gassmann, L. Frommberger, R. Dllmann, and K. Berns, "Realtme 3D map buldng for loal navgaton of a walng robot n unstrutured terran," n IEEE/RSJ Internatonal Conferene on Intellgent Robots and Systems (IROS 3), 3, pp , vol.3. [2] C. Coué, C. Pradaler, C. Lauger, T. Frahard, and P. Bessère, "Bayesan Oupany Flterng for Multtarget Trang: An Automotve Applaton," Internatonal Journal of Robots Researh, vol. 25, pp. 19-, 6. [3] C. Brennee, O. Wulf, and B. Wagner, "Usng 3D laser range data for SLAM n outdoor envronments," n IEEE/RSJ Internatonal Conferene on Intellgent Robots and Systems (IROS 3), 3, pp , vol.1. [4] U. Frane, U. Frane, C. Rabe, H. Badno, and S. K. Gehrg, "6D- Vson: Fuson of Stereo and Moton for Robust Envronment Perepton," n DAGM-Symposum, vol. 3663, ed: Sprnger, 5, pp [5] D. Pfeffer and U. Frane, "Effent representaton of traff senes by means of dynam stxels," n IEEE Intellgent Vehles Symposum (IV ),, pp [6] D. F. Huber and M. Hebert, "A new approah to 3-D terran mappng," n IEEE/RSJ Internatonal Conferene on Intellgent Robots and Systems (IROS '99), 1999, pp vol.2. [7] M. Vergauwen, M. Pollefeys, and L. Van Gool, "A stereo-vson system for support of planetary surfae exploraton," Mahne Vson and Applatons, vol. 14, pp. 5-14, 3. [8] S. Larox, A. Mallet, D. Bonnafous, G. Bauzl, S. Fleury, M. Herrb, and R. Chatla, "Autonomous Rover Navgaton on Unnown Terrans: Funtons and Integraton," Internatonal Journal of Robots Researh, vol. 21, pp , 2. [9] F. Onga and S. Nedevsh, "Proessng Dense Stereo Data Usng Elevaton Maps: Road Surfae, Traff Isle, and Obstale Deteton," IEEE Transatons on Vehular Tehnology, vol. 59, pp ,. [] M. Harvlle, "Stereo person trang wth adaptve plan-vew templates of heght and oupany statsts," Image and Vson Computng, vol. 22, pp , 4. [11] P. Pfaff, R. Trebel, and W. Burgard, "An Effent Extenson to Elevaton Maps for Outdoor Terran Mappng and Loop Closng," Internatonal Journal of Robots Researh, vol. 26, pp , February 1, 7. [12] R. Trebel, P. Pfaff, and W. Burgard, "Mult-Level Surfae Maps for Outdoor Terran Mappng and Loop Closng," n IEEE/RSJ Internatonal Conferene on Intellgent Robots and Systems (IROS 6), 6, pp [13] I. Dryanovs, W. Morrs, and. Jzhong, "Mult-volume oupany grds: An effent probablst 3D mappng model for mro aeral vehles," n IEEE/RSJ Internatonal Conferene on Intellgent Robots and Systems (IROS ),, pp [14] A. Elfes, "Usng oupany grds for moble robot perepton and navgaton," Computer, vol. 22, pp , [15] C. Chen, C. Tay, C. Lauger, and K. Mehnaha, "Dynam Envronment Modelng wth Grdmap: A Multple-Objet Trang Applaton," n 9th Internatonal Conferene on Control, Automaton, Robots and Vson (ICARCV '6), 6, pp [16] M. Kumar and D. P. Garg, "Three-Dmensonal Oupany Grds Wth the Use of Vson and Proxmty Sensors n a Robot Worell," ASME Conferene Proeedngs, vol. 4, pp , 4. [17] H. Lategahn, W. Derendarz, T. Graf, B. Ktt, and J. Effertz, "Oupany grd omputaton from dense stereo and sparse struture and moton ponts for automotve applatons," n IEEE Intellgent Vehles Symposum (IV),, pp [18] R. Danesu, F. Onga, and S. Nedevsh, "Modelng and Trang the Drvng Envronment Wth a Partle-Based Oupany Grd," IEEE Transatons on Intellgent Transportaton Systems, vol. 12, pp , 11. [19] O. Faugeras, Three-Dmensonal Computer Vson: A Geometr Vewpont: Mt Press, [] M. Isard and A. Blae, "CONDENSATION Condtonal Densty Propagaton for Vsual Trang," Internatonal Journal of Computer Vson, vol. 29, pp. 5-28, 1998.