Modeling and Tracking the Driving Environment with a Particle Based Occupancy Grid

Transcription

1 Moelng an Trakng the Drvng Envronment wth a Partle Base Oupany Gr Rau Danesu, Florn Onga, an Sergu Neevsh, Membe IEEE Abstrat Moelng an trakng the rvng envronment s a omplex problem, ue to the heterogeneous nature of the real worl. In many stuatons, moelng the obstales an the rvng surfaes an be aheve by the use of geometral objets, an trakng beomes the problem of estmatng the parameters of these objets. In the more omplex ases, the sene an be moele an trake as an oupany gr. Ths paper presents a novel oupany gr trakng soluton, base on partles, for trakng the ynam rvng envronment. The partles wll have a ual nature they wll enote hypotheses, as n the partle flterng algorthms, but they wll also be the bulng bloks of our moele worl. The partles have poston an spee, an they an mgrate n the gr from ell to ell epenng on ther moton moel an moton parameters, but they wll also be reate an estroye usng a weghtng-resamplng mehansm spef to partle flter algorthms. The trakng algorthm wll be entere on partles, nstea of ells. An obstale gr erve from proessng a stereovson-generate elevaton map s use as measurement nformaton, an the measurement moel takes nto aount the unertantes of the stereo reonstruton. The resulte system s a flexble, real-tme trakng soluton for ynam unstruture rvng envronments. Inex terms-oupany grs, envronment moelng, trakng, partle flterng, stereovson. T I. INTRODUCTION he tasks of moelng an perevng the rvng envronment are a ontnuous hallenge, beause there are multple types of senaros, of fferent egrees of orer an omplexty. Some envronments are well-regulate, an the types of stat an ynam objets are easly moele an trake usng geometral moels an ther parameters. The obstales an be moele as ubos havng poston, sze an spee, an the rvng surfae elmters an be moele as parametral urves. The hghway an most of the urban an rural setons of roa are usually sutable for geometral moelng an trakng. The ontons hange when the envronment to be trake s an nterseton, a busy urban ente or an off-roa senaro. Even f parts of ths envronment an be trake by estmatng the parameters of a geometral moel, many essental parts of the envronment wll not fulfll the onstrants of the moels. Manusrpt reeve May 31, 010. Ths work was supporte by CNCSIS UEFISCSU, projet number PNII IDEI 15/008, an by the POSDRU program, fnanng ontrat POSDRU/89/1.5/S/6557. Rau Danesu, Florn Onga an Sergu Neevsh are wth the Tehnal Unversty of Cluj-Napoa, Computer Sene Department (e-mal: [email protected]). Department aress: Computer Sene Department, Str. Memoranumulu, Nr. 8, Cluj-Napoa, Romana. Phone: The authors ontrbute equally to ths work. Also, sometmes a rvng assstane applaton nees to have stat an ynam nformaton about the envronment before a moel an be nstantate an trake, or t may use ths atonal nformaton n moel fttng an moel-base trakng. For these reasons, solutons for ntermeate level representaton an trakng are evse. These ntermeate representaton an trakng solutons an be base on oupany grs, or retly on the 3D ponts (the 6D vson tehnque, presente n [1]), on ompat ynam obstale prmtves alle stxels [], or they an be replae wth spealze tehnques of etetng rtal moton [3]. In what follows, we ll fous on the works relate to oupany grs. Maybe one of the frst uses of oupany grs, uner the name of probablst loal maps, s presente by Elfes n [4], n the ontext of sonar base robot navgaton. Another paper by the same author [5] names the oupany maps oupany grs, an esrbes the probablty nferene mehansm for hanlng the unertanty of a range sensor n omputng the probablty of eah ell s oupany state. In the same referene we fn a efnton of the oupany gr: the oupany gr s a mult-mensonal ranom fel that mantans stohast estmates of the ells n a spatal latte. The ntal oupany grs, suh as those presente n [4] an [5], are smple D maps of the envronment, eah ell esrbng the probablty of t beng oupe or free. Howeve for many trakng applatons, espeally n the rvng assstane fel, there s a nee for estmatng the ynam parameters of the envronment, namely the spee of eah gr ell. By ang the spee fator n the envronment estmaton, the omplexty nreases sgnfantly, as the ells are now strongly nteronnete. The work of Coué et al, presente n [6], uses a 4D oupany gr, where eah ell has a poston an two spee omponents along eah axs. By estmatng the oupany of eah ell n the 4D gr, the spees for the lassal ells n the D gr an be ompute. Another soluton for the representaton of spees s presente by Chen et al, n [7]. Instea of havng a 4D gr, ths soluton omes bak to D, but uses for eah ell a strbuton of spees, n the form of a hstogram. The Bayesan nferene mehansm reles on sensor ata an anteeent ells, the lst of anteeents beng ee by the spee hypotheses. A smple but lmte way of hanlng the ynam aspets of the envronment s presente n [8]. Instea of estmatng the spee of eah ell, ths soluton reles on oupany trals, whh are spef patterns, smlar to the moton blur of the amera, whh an be use to erve the trajetory an

2 therefore the spee of the movng objets. A more sophstate metho s presente n [9], where the nonsstenes n the stat gr are etete as soon as they appea an a mult-moel Kalman flter traker s ntalze to trak the ynam objet. We an attempt a frst lassfaton of the ynam oupany gr solutons (not the grs themselves) nto fully ynam, as those presente n [6], [7] an [10], an statynam hybrs, as those presente n [8] an [9]. One of the most mportant features of an oupany gr trakng soluton s the way the sensor moel s use for gr upate. The most tme effent way of upatng a gr s to rely on the nverse sensor moel, whh erves the probablty of a ell beng oupe retly from sensor reaout, assumng the oupany of eah ell s nepenent of ts neghbors. Ths soluton s maybe stll the most popula manly n stat grs [9]. Howeve the work of Thrun [11] prove that forwar sensor probablty moels are preferable even n the ase of stat grs, even f ths sgnfantly nreases the omplexty of omputaton. The oupany grs an have multple spatal representatons, an n [1] we are shown a omparson between three types of grs, the Cartesan (lass, the polar (stane an angle) an the /sparty grs. All these grs have avantages an rawbaks. A Cartesan gr s loser to the real worl representaton, an an hanle velotes ease whle the other types of grs are more sensor-frenly, makng the omputaton of the sensor unertantes easer. The oupany gr s a flexble representaton of the envronment, an ths flexblty allows powerful ntegraton of multple nformaton soures. For nstane, map nformaton an be mappe on the gr, when avalable, as presente n [10]. The map an assoate to eah ell a terran type (suh as roa, urb or sewalk), an the terran type s translate nto a reahablty probablty for the ell. The use of terran nformaton an greatly mprove the preton of the poston of ynam objets on the roa. The flexblty of the oupany gr makes t well sute for ollaboratve upatng, usng the nformaton from multple sensors or multple observers. A soluton whh uses the oupany gr (name obstale map) to ntegrate laser an raar nformaton s presente n [13], an n [14] the grs are use to fuse stereo an optal flow nformaton. A soluton that ntegrates the observatons of multple moble observers nto a unfe esrpton of the envronment s presente n [15]. Ths paper presents a rvng envronment trakng soluton base on a partle oupany gr. Ths soluton s efne by a new an orgnal approah for the representaton of the oupany an veloty probablty strbuton of eah gr ell, an by the orgnal upatng algorthm erve from the propose representaton. The oupany probablty of eah gr ell s esrbe by the number of partles n that ell, an the partles have a ual nature they esrbe oupany hypotheses, as n the partle flterng algorthms suh as CONDENSATION [16], but an also be regare as physal bulng bloks of our moele worl. The trakng algorthm esrbe n ths paper s partle-orente, not ell orente. The partles have poston an spee, an they an mgrate from ell to ell epenng on ther moton moel an moton parameters, but they are also reate an estroye usng the same log as the weghtng-resamplng mehansm esrbe n [16]. The measurement ata s the raw obstale gr obtane by proessng the elevaton map, as esrbe n [17]. Bulng a suffently ense elevaton map requres aurate ense stereo nformaton, whh s ompute usng the tehnques esrbe n [18]. Other tehnques for ense stereo proessng are presente n [19]. Base on the surveye lterature, the oupany gr trakng soluton presente n ths paper an be lassfe as havng a Cartesan representaton, usng a forwar sensor probablty moel, an proung a fully ynam gr. The propose metho s most losely relate to the works presente n [7] an [10], whh use a spee probablty strbuton for eah ell n the gr, nstea of moelng the ynam gr as a hgh mensonal spae, as n [6]. We beleve that our soluton omes as an mprovement over these tehnques, beause ue to the use of movng partles the representaton of the spee probablty strbuton an the estmaton of ths strbuton are no longer a onern. We o not have to approxmate the veloty as a hstogram [7] or as a mxture of Gaussans [10], we on t have to assume that one ell belongs to only one objet wth only one veloty, an nether are we onerne wth estmaton of ths spee, as ths results naturally from the survval or elmnaton of the partles. The partles n a ell an have fferent spees, an therefore they an hanle the stuaton of overlappng objets, or the most lkely stuaton when the objets are too lose an the unertanty of one overlaps over the unertanty of the other. The omplexty of the algorthm s lnear wth the number of ells n the gr an wth the maxmum number of partles n a ell, a traeoff between auray an response tme beng always avalable as a smple parameter. Also, ntegratng other moton parameters, suh as aeleraton, oes not nrease the omplexty of the trakng algorthm, beause t only alters the way the poston of the partles n tme s ompute. The remaner of ths paper s organze as follows: frst, the partle gr moel s presente, an then the steps of the flterng algorthm are etale: preton, measurement an ntalzaton. Then, the paper esrbes the way the partle gr results an be use to extrat 3D ubos that have poston, sze an spee. The paper ens wth the testng an results seton, followe by onlusons. II. THE WORLD MODEL The worl s represente by a D gr, mappng the br-eye vew 3D spae nto srete 0 m x 0 m ells. The sze of the gr s 50 s x 10 s (ths orrespons to a sene sze of 50x4 meters). The am of the trakng algorthm

3 s to estmate the oupany probablty of eah gr ell, an the spee omponents on eah axs. The trakng goals are aheve by the use of a partle-base flterng mehansm. Conserng a oornate system where the z axs ponts towars the reton of the ego-vehle, an the x axs ponts to the rght, the obstales n the worl moel are represente by a set of partles S = p p = (, r, v, vr, a ), = 1... N }, eah partle { S havng a poston n the gr, esrbe by the r (a srete value of the stane n the 3D worl z) an the (srete value of the lateral poston x), an a spee, esrbe by the spee omponents v an vr. An atonal paramete a, esrbes the age of the partle, sne ts reaton. The purpose of ths parameter s to faltate the valaton proess, whh wll be esrbe n a subsequent seton of the paper. The total number of partles n the sene N S s not fxe. Ths number epens on the oupany egree of the sene, that s, the number of obstale ells. Havng the populaton of partles n plae, the oupany probablty of a ell C s estmate as the rato between the number of partles whose poston ones wth the poston of the ell C an the total number of partles allowe for a sngle ell, N C. P ( C) O { p S r = r, N = (1) C = } The number of allowe partles per ell N C s a onstant of the system. In settng ts value, a traeoff between auray an tme performane shoul be onsere. A large number means that on a sngle ell multple spee hypotheses an be mantane, an therefore the traker an have a better spee estmaton, an an hanle fast movng objets better. Howeve the total number of partles n the sene wll be retly proportonal wth N C, an therefore the tme onsumpton wll nrease. The spee estmaton of a gr ell an be estmate as the average spee of ts assoate partles, f we assume that only one obstale s present n that ell. Of ourse, the partle populaton an hanle the stuaton when multple obstales, havng fferent spees, share the same ell, an n ths ase the spee estmate of the ell must be ompute by lusterng. ( v C, vr C ) = { ( v, vr ) p S, x = x, z = z p S r = r, = } Thus, the populaton of partles s suffently representatve for the probablty ensty of oupany an spee for the whole gr. Multple spee hypotheses an be mantane smultaneously for a sngle ell, an the oupany unertanty s represente by the varyng number of partles assoate to the ell. The goal of the trakng algorthm an now be state: usng the measurement nformaton to reate, () upate an estroy partles suh that they aurately represent the real worl. III. ALGORITHM OVERVIEW The frst step of the algorthm s the preton, whh s apple to eah partle n the set. The postons of the partles are altere aorng to ther spee, an to the moton parameters of the ego vehle. Also, a ranom amount s ae to the poston an spee of eah partle, for the effet of stohast ffuson. The seon step s the proessng of measurement nformaton. Ths step s base on the raw oupany ells prove by ense stereo proessng, an proves the measurement moel for eah ell. The measurement moel nformaton s use to weght the partles, an resample them n the same step. By weghtng an resamplng, the partles n a ell an be multple or reue. The fnal step s to estmate the oupany an spees for eah ell, an to group the ells nto 3D orente objets, for result evaluaton. IV. PREDICTION Ths step wll erve the present partle strbuton from the past nformaton, preparng the partle set for measurement. The preton equatons wll use oometry an moton moel nformaton. The bas oometry nformaton avalable through the CAN bus of a moern ar s the spee v an the yaw rateψ&. Together wth the tme nterval t elapse between measurements, these parameters an be use to ompensate for the ego-moton, an separate t from the nepenent moton of the objets n the sene. Between measurements, the egovehle rotates wth an angleψ, an travels a stane. ψ =ψ t & (3) ψ v t sn ψ = (4) The orgn of the gr representaton s splae along the two oornate axes by an r. sn ψ / DX r os ψ / DZ = (5) = (6) We enote by DX an DZ the ell sze of the gr (n the urrent mplementaton, 0. m). A pont n the gr, at r an, s splae by the followng equaton: r n n osψ = snψ snψ osψ r The preton s aheve usng equaton 8, whh ombnes the etermnst rft ause by the ego-moton ompensaton an the partle s own spee, wth the stohast r (7)

4 ffuson ause by the unertantes n the moton moel. The quanttesδ, δ r, δv anδvr are ranomly rawn from a Gaussan strbuton of zero mean an a ovarane matrx Q equvalent to the state transton ovarane matrx of a Kalman flter. The ovarane matrx s agonal, wth the stanar evatons for the spee omponents orresponng to a real-worl amount of 1 m/s, an the stanar evatons for the poston orresponng to a real-worl value of 0.1 m. These values wll ensure that the system s able to ope wth fast-movng objets even at a 10 fps frame rate. 1 r = 0 v 0 vr t t r 0 v 1 v n n r δ δr + δv δvr From the gr moel pont of vew, the preton has the effet of movng partles from one ell to anothe as seen n fgure 1. The oupany probablty s thus ynamally ajuste usng the partle s moton moel an the vehle oometry. Fg. 1. Partles n the gr, before an after preton. V. MEASUREMENT MODEL The measurement moel wll relate the measurement ata, whh s a bnary oupe/free onton erve from the stereovson-generate elevaton map [10], to the ontonal probabltes p(measurement oupe) an p(measurement free), whh wll weght the partles. In orer to ompute these probablty values, we have to pass through several steps. A. The unertanty of the stereo measurement In orer to ompute these probabltes, we start by omputng the unertanty of the stereo reonstruton. Frst, the unertanty of the stane reonstruton, n the ase of a retfe system, s gven by: z z bf (8) = (9) In the above equaton, z enotes the stane (n the real worl oornates), b s the baselne of the stereo system, f s the foal stane n pxels, an s the error n sparty omputaton (usually about 0.5 pxels, for a goo stereo reonstruton engne). The error n lateral postonng (usually muh smaller than the error n z), an be erve from the stane error. Ths error epens on the lateral poston x (n the real worl oornates) an the stane z. x z x = (10) z The 3D errors are mappe nto gr ell errors, by vng them wth the gr ell sze on x an z. z = DZ x = DX (11) The values of an are ompute offlne, at the ntalzaton phase, for eah ell n the gr. B. The raw oupany ensty ue In orer to ompute the ontonal probablty of the measurement ell, uner the oupe or free assumpton, we have to take nto aount a realty that s spef to stereovson sensors. The stereo sensor oes not perform a san of the sene, an therefore t oes not output a sngle br-eye vew pont for a real-worl obstale ell. We ll take as example a plla whh has almost no wth, an no epth sprea. The representaton of a pllar n the oupany gr shoul be a sngle ell. If the pllar were observe by a sanner-type senso ths sensor wll output a ell, splae from the true poston by an amount spef to the sensor error. For the stereo senso thngs are fferent, beause the amera observes the whole heght of the plla an therefore eah pllar pxel wll get a stane an a lateral poston. Ths means that one we ollapse the pllar nformaton n the D gr representaton, eah part of the pllar may fall n a fferent ell, an the pllar wll generate a sprea of ells. The sze of the sprea area s ontrolle by the gr unertantes on the an r axes (real worl x an z). Ths property leas us to fn a goo ue, whh wll ontrbute to the ontonal probabltes of the measurement ells uner the oupe/free assumpton. We ll ount the obstale ells n the measurement gr aroun the urrent ell poston, n an area of heght an wth, an ve the number of foun obstale ells by the total number of ells n the unertanty area. We ll enote ths rato as p ensty (m( oupe). p ensty oupe) = = r+ = r ol= ( ol= + + 1)( O(, ol) + 1) (1)

5 By O(, ol) we enote the oupe value of the measurement gr, at poston an ol. Ths value s 1 when an obstale ell s present an 0 when not. The ensty ue for the free assumpton s: p ensty free) = 1 p oupe) ensty (13) A graph omparson between the raw measurement ata an the ensty ue (ontonal probablty) of the measurement uner the oupe assumpton s gven n the followng fgure. One eah ell has an obstruton value, the fnal analyss s performe. Eah ell that has an obstruton value hgher than 10 s onsere obstrute an onsere as suh n the partle weghtng an resamplng phase (to be esrbe n the next hapter). Howeve ths s not the only way we use the obstruton property. If a raw measurement ell s marke as oupe, but from the obstruton analyss t s foun to be obstrute, the oupe ell s remove. Ths wll make the raw oupany map look more lke a sanner-erve map. Ths reuton of measurement nformaton must be performe before the omputaton of the other partle weghng ue, whh reles on the stane from measurement. The obstruton-relate proessng steps are llustrate n fgure 3. The left panel shows the raw measurement ata, the mle panel shows the obstruton value for eah ell (the lghte the more obstrute), an the rght panel shows the measurement ata that remans after the obstrute ells are remove. Ths ata set s use for the next ue omputaton. Fg.. From the raw oupany gr to the raw measurement ensty ues. Bottom-left: raw oupany gr, bottom-rght: ensty ue for the oupe ell hypothess. C. Hanlng the olusons Not all ells n the gr an be observe retly, an ths fat must be taken nto onseraton by the trakng algorthm. Due to the lmtatons of the prmary soure of nformaton, the stereovson-base raw oupany gr, some of the ells are never observe. The raw oupany gr only overs a longtunal stane from 0 to 40 meters, a lateral span of 13 meters. Also, the fel of vew of the amera (angular span) lmts the areas that are vsble at lose stane. The ells that are exlue by the fel of vew an stane lmtatons are marke as obstrute (unobservable) by efault. Another way for a ell to beome unobservable s f t s obstrute by an obstale ell that s loate between t an the observaton orgn (amera poston). In orer to ee f a ell s n suh a stuaton, we swth to polar oornates. Eah ell s mappe to a polar gr. Then, for eah angle, the ells are sanne n the orer of ther stane. One a raw oupe ell s foun, an obstruton ounter s nremente for every ell that s behn the frst oupe one. Then, the obstruton values are re-mappe nto the Cartesan gr. Fg. 3. Hanlng the olusons. Left orgnal measurement nformaton, mle obstruton value for eah ell, rght unobstrute measurement. D. The stane from measurement ue For eah ell n the gr, we nee to ompute the stane to the nearest oupe ell n the measurement gr. For that, we ll use a mofe verson of the stane transform algorthm presente n [0]. The man ssue s that we nee to know not only the stane to the nearest measurement pont, but the stane omponents on the two oornate axes, an. The reason for ths requrement s that the stanar evatons for the postonng errors are fferent on the an on the, an therefore one annot be substtute for another. Our stane transform algorthm performs lke the lassal two-pass L1 norm one, but nstea of upatng only the ell stane to the nearest measurement, the poston of the nearest measurement s upate along. The followng algorthm upates a stane matrx D(, ntalze wth zero for measurement oupany ells, an wth 55 for the free ells, an two poston matres M r an M that hol the an the of the nearest oupe measurement ell. The values of M r an M are ntalze to the urrent an of eah ell.

6 Algorthm DstaneTransform For r=1 to max_r For =1 to max_ Upate (, -1, 0) Upate (, 0, -1) En For En For For r = max_r to 1 For = max_ to 1 Upate (, 1, 0) Upate (, 0, 1) En For En For Funton Upate(, n, k) If D( > D(r+n, +k) + 1 D( = D(r+n, +k) + 1 M r ( = M r (r+n, +k) M ( = M (r+n, +k) En If After the stane transform algorthm s apple, the stane-to-measurement-oupe on s an on s, for eah ell an be foun by: oupe oupe = r M = M r (14) The stane to measurement-free-ell s ompute as the fferene between the ouble of the stane stanar evaton an the stane-to-oupe, saturate to zero. free free = max( = max( oupe,0) (15),0) oupe These stanes are onverte to a probablty ensty value usng the multvarate Gaussan equaton (equaton 16). We have remove the an arguments for all the values nvolve, for the sake of reaablty. The same equaton s apple for both free an oupe stanes, an therefore the onton status s a plaeholer for both stuatons. p e stane 1 ( m status) = π status status 1 + status status. (16) At the en of ths step, we have, for eah ell, the values p stane oupe) an p stane free). VI. WEIGHTING AND RESAMPLING The lassal steps of a partle flter base traker are resamplng, rft, ffuson, an measurement (weghtng). Ths behavor replaes a populaton of a fxe number of partles wth an equal number of partles, whh approxmates an upate probablty ensty funton over a spae of parameters. Howeve ths approah works when the partles are hypotheses of the state of a system, not when the partles are the system tself (we an see our trake worl as physally ompose of partles). Our algorthm tres to use the partles n a ual form as hypotheses, an as bulng bloks of the worl that we trak. Ther role as bulng bloks has been alreay explane. Howeve f we restrt our reasonng to a sngle ell n the gr worl, we an see that the partle s also a hypothess. A partle n a gr ell s a hypothess that ths ell s oupe, an that the ell has the spee equal to the spee of the partle. More partles n the ell mean that the hypothess of oupany s strongly supporte. Less partles n the ell means that the hypothess of the ell beng free s supporte. We an regar the fferene between the number of partles n a ell an the total number of partles allowe n a ell as the number of partles havng the oupany hypothess zero. A. Weghtng the partles If we regar the number of partles n the ell to be onstant, an some of them havng the oupany value true whle some havng t false, we an apply the mehansm of weghtng an resamplng. If we assume that the measurement ata oes not ontan spee nformaton, the weght of the partle epens only on the oupe hypothess. Also, ths means that all the partles havng the same oupe hypothess wll have the same weght. For eah ell at poston n the gr, the weghts for the free an for the oupe hypotheses s obtane by fusng the ues ompute from the measurement ata usng the methos esrbe n seton V. woupe = p wfree = p p ensty p ensty oupe). oupe) free). stane stane free) (17) (18) The equatons 17 an 18 hol f the ell n the gr s not marke as obstrute, as esrbe n seton V.C. If the ell s obstrute, the weghts of the oupe an free hypotheses wll be equal, w = w = 0. 5 oupe free. The number of partles havng the oupe hypothess true s the number of real partles n the ell. N { p S r = } = (19) OC = The number of partles (hypotheses) havng the oupe value false s the omplement of N OC. We remn the reaer that N C s the maxmum number of partles allowe n a ell, an ths number s a onstant of the algorthm.

7 N FC N N = (0) C OC The total posteror probablty of a ell beng oupe an of a ell beng free an be ompute from the number of free/oupe hypotheses, an ther orresponng weghts. In the followng equatons we have remove the an parameters, but they are mple. P w N = oupe OC OC w ( ) oupe NOC + wfree NC N (1) OC P w ( N N free C OC FC = () woupe NOC + wfree( NC NOC ) The aggregate partle weghts P OC an P FC are use for partle resamplng. The resamplng of the partle populaton s one at the en of the measurement step, so that the next yle an start agan wth an upate populaton of partles wthout onernng about ther weght. B. Resamplng A lassal resamplng algorthm woul make N C ranom raws from the prevous partle populaton of a ell, whle the weght of eah partle ontrols ts hanes of beng selete. Beause we on t are for the ell free hypothess partles, our resamplng wll nstea ee for eah real partle (partle havng the oupe hypothess true) whether t s estroye or multple (an, f multple, how many opes of t are reate). The followng algorthm esrbes the proess of resamplng, whh s materalze as uplaton or removal of partles from the partle set. The key soluton for a real-tme operaton s that all the heavy omputng tasks are exeute at ell level, mostly by the use of LUT s, whle the partle level proessng s kept very lght. Algorthm Resample For eah ell C Compute N OC an P OC Compute resample number of partles N RC N RC =P OC N C Compute rato between atual number of partles an the number of resample partles N f C = N RC OC En For For eah partle p Fn orresponng ell C If (f C >1) number of partles wll nrease F n = Int(f C ) Integer part F f = f C -Int(f C ) Fratonal part For k=1 to F n-1 S.A(p.MakeCopy) En For ) r = ranom value between 0 an 1 If (r<f f ) S.A(p.MakeCopy) En f En f If (f C <1) number of partles wll erease r = ranom value between 0 an 1 If (r> f C ) S.Remove(p ) En f En f En For The system wll ompute the number of partles that eah ell shoul have after the proess of resamplng has been omplete. The rato f C between ths number an the exstng number of partles n the ell wll tell us f the partles have to be uplate or remove. If f C s hgher than 1, the number of partles has to be nrease. The nteger part of the fferene between f C an 1 tells us the number of ertan uplatons a partle must unergo (for nstane, f f C s, eah partle wll be ouble). The fratonal part of the fferene s use for hane uplaton: eah partle wll have a probablty of beng uplate equal to the fratonal part of ths fferene. If f s lower than 1, the number of partles has to be erease, by removng some of the partles. Eah partle has 1- f C hane of beng elmnate. At ths pont the yle s omplete, an the trakng algorthm an proess a new frame. Seonary estmatons for oupany, spee, or lusterng the ells nto objets an be performe at the en of ths step. Fg. 4. Weghtng an resamplng. The weght of the oupe hypothess s enoe n the arkness of the ell of the left gr. VII. INITIALIZATION Although the measurement step takes are of partle reaton an eleton, ths step only works f there are partles to be uplate or elete. For the preton-measurement yle to work, the partle populaton has to be ntalze. From a strtly probablst pont of vew, eah ell s state s unknown at startup, whh means that the ell has equal probablty of beng oupe or free. In our trakng system, ths woul mean that eah ell shoul be assgne a number of

8 partles equal to half the total number of partles allowable n a ell. Howeve ths approah woul sgnfantly reue the spee of the system, an woul requre permanent rentalzaton. Our soluton s to use the measurement oupany gr to reate partles. If a measurement ell s of type obstale, ts p(m( oupe) s hgh, an there are no partles n the orresponng trake gr ell, a small number of partles wll be reate. The ntal spee omponents vr an v of the reate partles wll be sample ranomly from an ntal range of possble values, an the ntal poston s onfne to the reaton ell. In ths way, the ntalzaton s a ontnuous proess. Partles are automatally remove when they go outse the gr area, n the preton phase. Another ase of amnstratve removal (removal not ause by the probablty mehansm esrbe n seton VI) s when, ue to partle rftng, the number of partles n a ell exees the allowe value. an a new label (whh mples a new objet) s generate. The fferene between our labelng an a lassal labelng algorthm s the way the neghborhoo relatonshp s efne. Two ells are neghbors f the followng ontons are fulflle: - The stane between them n the gr s less than 3, meanng that a one ell gap s allowe. - The fferene n the orentaton of the spee vetors n the two ells s less than 30 egrees. - The fferene n spee vetor magntues s less than 30% of the value of the largest magntue of the two ells. The labelng proess s shown n fgure 5, mle panel, where eah olor marks a fferent objet. We an see that by applyng vnty rtera only, the movng vehle wll be onnete to the statonary struture. Howeve ths oes not happen ue to the fat that we an use ynam nformaton prove by the gr to suessfully srmnate the two objets. VIII. CELL STATE ESTIMATION AND OBJECT EXTRACTION The result of the trakng algorthm s the partle populaton tself. Howeve for testng an valaton purposes, an for usng the trakng results n further stages of proessng, we wll estmate the oupany state an the spee of eah ell n the gr. The oupany probablty of eah gr ell s approxmate by the rato between the number of partles n that ell an the total number of allowe partles n a ell (equaton 1). The omponents of the spee vetor for eah ell are estmate usng equaton. Howeve ue to the fat that the spee of a newly reate partle s ompletely ranom, these partles are exlue from the spee estmaton of a gr ell. For ths purpose, we an use the age property of the partle. The age of the partle s set to 1 when the partle s reate, an nrease eah tme the partle s state (poston an spee) s altere by preton. Basally, the age of the partle tell us how many trakng yles the partle has survve n the system. All the partles n a ell that have an age hgher than two beome part of the spee estmaton. They are ounte, an the spee omponents on an are average. Also, the stanar evaton of these spee omponents s ompute. If both the estmate spee omponents are lower n absolute value than the ouble of ther stanar evatons, the ell s elare stat, beause t means that ether the spee s too low, or t s too sperse to raw a efnte onluson. For further testng an evaluaton, a subset of the gr ells s groupe nto 3D ubos. A ell s onsere for objet groupng f ts oupany probablty s at least 0.5, meanng that the partle ount n the ell s at least N C /. The nvual objets are entfe by a gener algorthm of onnete omponent labelng. The algorthm starts from a val ell, an reursvely propagates a unque label to the ell s oupe neghbors, untl no more onnetons are foun Fg. 5. Cell labelng an extraton of objets. The labele onnete omponents n the gr are use to generate the 3D objets n the form of orente ubos (fgure 5, thr panel). The objets are groupe nto two ategores, base on ther average spee, ompute from the spees of eah omponent ell: stat (shown n green) an ynam (shown n re). Only the ynam objets reeve orentaton, whh s the orentaton of ther average spee. A. Qualtatve assessment IX. TESTS AND RESULTS The qualtatve tests, whh allow us to montor the general behavor of the system n omplex stuatons, are performe on veo sequenes reore n real urban traff. These tests show how the oupany gr s ompute, how the spee vetor for eah ell s estmate, an how the gr results are groupe nto uboal objets havng poston, sze, orentaton an orente spee vetor. The spee of the ells s splaye n olo usng Hue for orentaton an Saturaton for magntue. Due to the nee for ompat representaton of the gr results, we have also enoe the oupany probablty as the olor s Intensty, makng full use of the whole HSI olor spae.

9 Fg. 6. Color ong for spee vetors (full an half oupany). Veo fles, esrbng results n fferent traff stuatons, an be ownloae from ths page: The man qualtatve test s the sequene whh shows the results over a sgnfant stane through Cluj- Napoa. Some hghlghts of ths sequene are presente n fgure 7: a) Crossng peestran, mxe wth lateral traff an stat stant objets. b) Inomng vehle, stat lateral senery. Two nomng vehles, the most stant one only vsble for a ouple of frames. ) Movng vehle aganst stat wall, ego vehle performng a sharp turn left. e) Dstant objet, aurately trake. f) Movng objet aganst stat bakgroun. The protruson from the stat bakgroun near the movng objet s atually an olue statonary ar. The ego vehle s performng a sharp rght turn, whh auses the nstablty n the estmaton of the stat nature of the bakgroun n the top rght orner. Also, that area was prevously olue by the movng vehle, whh means that the stat nature of the ells has not yet been etete, ue to the short observaton tme. g) Dstant rossng vehle gong through statonary vehles. The ego vehle s turnng rght. h) Trakng a movng target through a nar orror of statonary vehles. The behavor of the system n the ase of olusons s hghlghte by the sequene Key ponts from the sequene are presente n fgure 8. Whle the ego vehle s performng a sharp left turn, a vehle omes from our rght, an s olue by a vehle omng from our left. The olue vehle s also maneuverng, hangng ts heang to ts left. Whle olue, ts partle strbuton beomes ffuse, aountng for possble ext trajetores, an the orret heang s qukly entfe as the objet beomes observable agan. An extensve sequene, reore whle observng an nterseton wth the ego vehle stanng stll, proue the results that are avalable n the fle A hghlght of ths sequene s shown n fgure 9. A vehle omes from our rght, then turns left an proees to ext the sene. Fg. 7. Extene sequene n urban traff hghlghts. Fg. 8. Dynam oluson. Durng ths maneuver t olues the stat objet near ts left se, but oes not beome jone wth ths struture ue to

10 the spee-senstve nature of the ell lusterng algorthm. We an see how the oupany beomes ffuse as the objet s olue by a large truk, whh then agan olues the stat objets on the rght. B. Numeral evaluaton n ontrolle envronment The numeral evaluaton was performe on sequenes aqure n ontrolle senaros, wth known target spee an orentaton. We have performe four tests, wth the same orentaton, -45 egrees, but fferent spees, 30 km/h, 40 km/h, 50 km/h, 60 km/h. The results that were evaluate are the estmate spee an orentaton of the 3D ubo resulte from lusterng the oupe gr ells. These results are ompare to the groun truth, an they are also ompare to the results of another means of ntermeate extraton of 3D ynam nformaton, the optal flow ombne wth stereovson. The results of optal flow that are taken nto onseraton are the spee an orentaton of the 3D ubo obtane from groupng the ponts havng 3D an spee nformaton [1]. The ontrolle test sequene s hghly favorable to the optal flow approah, as the vehle s learly vsble, has plenty of features that an be mathe from one frame to anothe a stuaton whh proves plenty of goo spee vetors to be average nto an aurate vetor of the ubo. Fg. 10. Controlle test sequene. Fg. 11. Spee an orentaton estmaton, 30 km/h test. Fg. 9. Turnng near a statonary objet an oluson. The results of spee an orentaton estmaton are splaye n the graphs shown n fgures 11 to 14. The gr trakng results are shown wth the re otte lne. We an see that both methos qukly onverge towars the groun truth, but the gr trakng results are more stable (lower error stanar evaton) an more aurate (lower mean absolute error). Ths fat s onfrme by the tables I an II. Fg. 1. Spee an orentaton estmaton, 40 km/h test.

11 typal urban sene, an a total number of partles n a ell N C =50, the total runnng tme s about 40 ms per frame, on an Intel Core Duo proessor at.1 GHz. Due to the fat that the partle trakng system shares the proessor wth other sensoral proessng algorthms suh as lane eteton, objet lassfaton an so on, the total frame rate s about 10 fps. Note: veo fles showng results n multple traff stuatons an be ownloae from the aress: Fg. 13. Spee an orentaton estmaton, 50 km/h test. Fg. 14. Spee an orentaton estmaton, 60 km/h test. Spee of target TABLE 1 NUMERICAL RESULTS SPEED ESTIMATION ACCURACY Partle gr MAE Partle gr STDEV Optal flow MAE Optal flow STDEV 30 km/h km/h km/h km/h TABLE NUMERICAL RESULTS ORIENTATION ESTIMATION ACCURACY Spee of target Partle gr MAE Partle gr STDEV Optal flow MAE Optal flow STDEV 30 km/h km/h km/h km/h The tme performane epens on the obstale loa of the sene, whh nfluenes the total number of partles. For a X. CONCLUSION AND FUTURE WORK We have presente a soluton for rvng envronment moelng an trakng, whh employs partles n orer to estmate the oupany an spee of the ells of an oupany gr. Ths flexble an real-tme soluton s apable of orretly trak ynam envronments even at hgh relatve spees, wthout the nee of a very hgh frame rate from the measurement system. The test sequenes prove that the metho s senstve enough to etet an estmate the spee of a peestran, but also the spee of a fast movng vehle. The auray of the spee an orentaton estmaton s proven by the tests onute n ontrolle stuatons. The partle gr trakng soluton s an elegant extenson of the ynam oupany gr solutons that were surveye. The partle populaton approah releves the esgner of the hoe of a spee probablty strbuton for eah ell, an an hanle multple vergent spee hypotheses. Also, the spee strbuton oes not have to be estmate, an the measurement ata only ontrols the reaton or eleton of partles. We beleve that the propose tehnque s a new vew of the oupany gr problem, a vew orente towars pratal mplementaton, an a vew that an open the oor to nterestng extensons. The presente tehnque s not a substtute for moel-base trakng, but a metho for ntermeate representaton an proessng of sensoral ata. The oupany probablty an ynam parameters of eah ell an subsequent algorthms of feature groupng, moel-base objet trakng, or even sensor fuson. The avantages of havng a goo ynam ntermeate representaton are proven by the results of the expermental step of moel-base objet reonstruton. The qualty of the partle gr trakng results as ntermeate representaton towars objet eteton an trakng are also proven by the omparson wth the most use soure of ntermeate representaton n omputer vson, the Luas-Kanae optal flow mxe wth stereo 3D nformaton, an the omparson was mae n the most favorable ase for the optal flow tehnque. The soluton leaves plenty of room for future work. For example, many of the alulatons performe by the algorthm an be subjete to parallelzaton, for sgnfant spee mprovement. The partle-relate omputatons, suh as the preton of the new poston, an be subjete to massve parallelzaton, whle the gr-relate omputatons an be parallelze at regon level. Further work wll be eate to

12 the ssue of optmzaton through parallelzaton. We beleve that the most mportant evelopment for the future woul be to use the apablty of the partle to arry atonal nformaton. For example, the age nformaton may be use for more than valaton. One use of age s to ajust the varanes of the ranom alteratons of spee an poston that are apple n the preton phase one a partle s ole ts ranomness an be erease. The partles an be tagge wth a unque ID, allowng us to reonstrut the trajetory of an objet. Other parameters, suh as heght, or the lass of the objet from whh the partle s a part, an be ae to the partle, an use by the trakng mehansm or by the applatons evelope on top of t. REFERENCES [1] U. Franke, C. Rabe, H. Bano, an S. Gehrg, 6-vson: Fuson of stereo an moton for robust envronment perepton, n pro of 7th Annual Meetng of the German Assoaton for Pattern Reognton DAGM 05, Venna, Otobe 005, pp [] D. Pfeffe U. Franke, "Effent Representaton of Traff Senes by Means of Dynam Stxels", n pro of IEEE Intellgent Vehles Symposum (IEEE-IV), 010, pp [3] S. Cherng, C. Y. Fang, C. P. Chen, S. W. Chen, Crtal Moton Deteton of Nearby Movng Vehles n a Vson-Base Drver- Assstane System, IEEE Transatons on Intellgent Transportaton Systems, Vol. 10, No. 1, Marh 009, pp [4] A. Elfes, A Sonar-Base Mappng an Navgaton System, n pro of IEEE Internatonal Conferene on Robots an Automaton, Aprl 1986, pp [5] A. Elfes, Usng Oupany Grs for Moble Robot Perepton an Navgaton, Compute vol., No. 6, June 1989, pp [6] C. Coue, C.Praale C.Lauge T.Frahar, P.Bessere, Bayesan Oupany Flterng for Multtarget Trakng: An Automotve Applaton, The Internatonal Journal of Robots Researh, Vol 5, No 1, 006, pp [7] C. Chen, C. Tay, K. Mekhnaha, C. Lauge Dynam envronment moelng wth grmap: a multple-objet trakng applaton, n pro of Internatonal Conferene on Automaton, Robots an Computer Vson (ICARCV) 006, pp [8] T. Wess, B. Shele, K. Detmaye Robust Drvng Path Deteton n Urban an Hghway Senaros Usng a Laser Sanner an Onlne Oupany Grs, n pro of IEEE Intellgent Vehles Symposum 007, pp [9] S. Petzh, T. D. Vu, J. Burtlet, O. Ayar, T. Hakbarth, N. Appenrot, J. Dkmann, B. Rag, "Results of a Prerash Applaton base on Laser Sanner an Short Range Raars", IEEE Transatons on Intellgent Transportaton Systems, Vol. 10, No. 4, 009, pp [10] T. Gnele, S. Brehtel, J. Shroee R. Dllmann, Bayesan Oupany Gr Flter for Dynam Envronments Usng Pror Map Knowlege, n pro of IEEE Intellgent Vehles Symposum 009, pp [11] S. Thrun, "Learnng Oupany Grs Wth Forwar Sensor Moels", Autonomous Robots, Vol. 15, No, 003, pp [1] H. Bano, U. Franke, R. Meste Free Spae Computaton Usng Stohast Oupany Grs an Dynam Programmng, Workshop on Dynamal Vson, ICCV, 007, pp [13] M. S. Darms, P. E. Rybsk, C. Bake C. Urmson, Obstale Deteton an Trakng for the Urban Challenge, IEEE Transatons on Intellgent Transportaton Systems, Vol. 10, No. 3, September 009, pp [14] C. Brallon, K. Ushe C. Praale J. Cley, C. Lauge Fuson of stereo an optal flow ata usng oupany grs, n pro of IEEE Internatonal Conferene on Intellgent Transportaton Systems, 006, pp [15] J. Y. Chen, J. Hu, Probablst Map Bulng by Coornate Moble Sensors, n pro of IEEE Internatonal Conferene on Networkng, Sensng an Control, 006, pp [16] M. Isar, A. Blake, CONDENSATION -- ontonal ensty propagaton for vsual trakng, Internatonal Journal of Computer Vson, Vol. 9, No. 1, 1998, pp [17] F. Onga, S. Neevsh, Proessng Dense Stereo Data Usng Elevaton Maps: Roa Surfae, Traff Isle, an Obstale Deteton, IEEE Transatons on Vehular Tehnology, Vol. 59, No. 3, Marh 010, pp [18] I. Halle C. Pantle, F. Onga, S. Neevsh, Real-tme sem-global ense stereo soluton wth mprove sub-pxel auray, n pro of IEEE Intellgent Vehles Symposum 010 (IV 010), pp [19] W. van er Mark, D. M. Gavrla, Real-Tme Dense Stereo for Intellgent Vehles, IEEE Transatons on Intellgent Transportaton Systems, Vol. 7, No. 1, Marh 006, pp [0] A. Rosenfel, J. L. Pfaltz, Sequental Operatons n Dgtal Pture Proessng, Journal of the Assoaton for Computng Mahnery, Vol. 13, No. 4, Otober 1966, pp [1] C. Pantle, S. Neevsh, Real-tme Obstale Deteton n Complex Senaros Usng Dense Stereo Vson an Optal Flow, IEEE Conferene on Intellgent Transportaton Systems (IEEE-ITSC), 010, pp Rau Danesu reeve the Dploma Engneer egree n Computer Sene n 00 from the Tehnal Unversty of Cluj-Napoa, Romana, followe by the M.S. egree n 003 an the PhD (Computer Sene) egree n 009, from the same unversty. He s a Senor Leturer wth the Computer Sene Department, TUCN, teahng Image Proessng, Pattern Reognton, an esgn wth mroproessors. Hs man researh nterests are stereovson an probablty base trakng, wth applatons n rvng assstane. He s a member of the Image Proessng an Pattern Reognton Researh Laboratory at TUCN. Florn Onga reeve the Dploma Engneer egree n Computer Sene n 00 from the Tehnal Unversty of Cluj-Napoa, Romana, followe by the M.S. egree n 003 from the same unversty. He s urrently workng towars the Ph.D. egree n Computer Sene at Tehnal Unversty of Cluj- Napoa, spealzng n Computer Vson. He s a Leturer wth the Computer Sene Department, Tehnal Unversty of Cluj-Napoa, teahng Image Proessng, Pattern Reognton, an Computer Arhteture. Hs researh nterests nlue stereovson, gtal elevaton maps proessng, an vson base automotve applatons. He s a member of the Image Proessng an Pattern Reognton Researh Laboratory at TUCN. Sergu Neevsh (M 99) reeve the M.S. an PhD egrees n Eletral Engneerng from the Tehnal Unversty of Cluj-Napoa (TUCN), Cluj- Napoa, Romana, n 1975 an 1993, respetvely. From 1976 to 1983, he was wth the Researh Insttute for Computer Tehnologes, Cluj-Napoa, as researher. In 1998, he was apponte Professor n omputer sene an foune the Image Proessng an Pattern Reognton Researh Laboratory at the TUCN. From 000 to 004, he was the Hea of the Computer Sene Department, TUCN, an s urrently the Dean of the Faulty of Automaton an Computer Sene. He has publshe more than 00 sentf papers an has ete over ten volumes, nlung books an onferene proeengs. Hs researh nterests nlue Image Proessng, Pattern Reognton, Computer Vson, Intellgent Vehles, Sgnal Proessng, an Computer Arhteture.